#include <mega128a.h>
#include <delay.h>
#define _BV(b) (1 << (b))
#define MUX (_BV(REFS0))
#define BIT_IS_CLEAR(Reg, b) ((Reg & _BV(b)) == 0)


unsigned char Digit (unsigned int d, unsigned char m)
{
//[]-----------------------------------------------------[]
//| Назначение: выделение цифр из разрядов пятиразрядного |
//| десятичного положительного числа |
//| Входные параметры: |
//| d - целое десятичное положительное число |
//| m - номер разряда (от 1 до 5, слева направо) |
//| Функция возвращает значение цифры в разряде m числа d |
//[]-----------------------------------------------------[]
    unsigned char i = 5, a;
    while(i)
    { // цикл по разрядам числа 21
        a = d%10; // выделяем очередной разряд
        if(i-- == m) break; // выделен заданный разряд - уходим
        d /= 10; // уменьшаем число в 10 раз
    }
    return(a);
}

void sgmnt_init()
{
    DDRC = 0xFF;
    DDRA |= (1<<1) | (1<<2) | (1<<3) | (1<<4) | (1<<5);
}

void sgmnt_write(unsigned int number)
{
    unsigned char i;
    unsigned char num;
    for (i = 1; i < 6; i++)
    {
        num = Digit (number, i);
        switch (num)
        {
        case 0:
        PORTC = 0x3F;
        break;

        case 1:
        PORTC = 0x06;
        break;

        case 2:
        PORTC = 0x5B;
        break;

        case 3:
        PORTC = 0x4F;
        break;

        case 4:
        PORTC = 0x66;
        break;

        case 5:
        PORTC = 0x6D;
        break;

        case 6:
        PORTC = 0x7D;
        break;

        case 7:
        PORTC = 0x7;
        break;

        case 8:
        PORTC = 0x7F;
        break;

        case 9:
        PORTC = 0x6F;
        break;

        }

        PORTA |= (1 << i);
        delay_us(10);
        PORTA &= ~(1 << i);
    }
}


unsigned int readAdc(unsigned char channel)
{
    ADMUX = MUX | channel;
    delay_us(10);
    ADCSRA |= _BV(ADSC);
    while(BIT_IS_CLEAR(ADCSRA, ADIF));
    ADCSRA |= _BV(ADIF);
    return ADCW;
}



void main(void)
{
    int field; // Значение с датчика Холла
    int ref; // Задание
    int error0 = 0;
    int error1 = 0; // Ошибка управления
    int control; // Сигнал управления
    int kp = 2;
    int kd = 0.1;


    // Инициализация портов ввода/вывода
    DDRB = _BV(5);
    DDRG = _BV(3);
    // Инициализация АЦП
    ADCSRA = _BV(ADEN) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0);
    // Инициализация таймера 1
    // Быстрая ШИМ 8 бит
    TCCR1A = _BV(COM1A1) | _BV(WGM10);
    TCCR1B = _BV(WGM12) | _BV(CS10);

    sgmnt_init();

    while(1)
    {

        // Получение и фильтрация сигнала с датчика
        // магнитного поля
        field = (readAdc(1) + readAdc(1) +
        readAdc(1) + readAdc(1) + readAdc(1)) / 5;
        ref = readAdc(3); // Получение задания с ручки
        //потенциомера
        // Расчёт ошибки управления
        error1 = ref - field;
        // Расчёт сигнала управления
        control = error1*1.1 + (error1 - error0)*50;
        // Ограничение сигнала управления
//        if (error1 > 0)
//            sgmnt_write(error1);
//        else
//            sgmnt_write(-1*error1);
//        delay_ms(300);
        error0 = error1; //предыдущее значение


        if(control > 255)
        {
            control = 255;
        }
        if(control < -255)
        {
            control = -255;
        }
        // Установка сигнала управления
        if(control >= 0)
        {
            PORTG &= ~_BV(3);
            OCR1AL = (unsigned char)control;
        }
        else
        {
            PORTG |= _BV(3);
            OCR1AL = (unsigned char)(-control);
        }

        /*
        int field; // Значение с датчика Холла
        int ref; // Задание
        int error = 0; // Ошибка управления
        int control; // Сигнал управления
        //управления
        // Получение и фильтрация сигнала с датчика
        // магнитного поля
        field = (readAdc(1) + readAdc(1) +
        readAdc(1) + readAdc(1) + readAdc(1)) / 5;
        field -= 512;
        ref = readAdc(3); // Получение задания с ручки
        //потенциомера
        // Расчёт ошибки управления
        error = ref - field;
        // Расчёт сигнала управления
        control = error / 1.3 + (error1-error0)*400;
        // Ограничение сигнала управления
        error0 = error1; //предыдущее значение

        if(control > 255)
        {
        control = 255;
        }
        if(control < -255)
        {
        control = -255;
        }
        // Установка сигнала управления
        if(control >= 0)
        {
        PORTG &= ~_BV(3);
        OCR1AL = (unsigned char)control;
        }
        else
        {
        PORTG |= _BV(3);
        OCR1AL = (unsigned char)(-control);
        }*/
    }
}