#include<avr/io.h>
#include <avr/interrupt.h>

//Czujniki
#define C4 PC0
#define C3 PC1
#define C1 PC2
#define C2 PC3
#define C8 PC4
#define C5 PC5
#define C6 20U
#define C7 21U

//Mostek
#define PWMA PD3
#define PWMB PD5
#define AIN1 PD2
#define AIN2 PD4
#define BIN1 PD6
#define BIN2 PD7
#define STBY PB4

#define reloadValue 65036

//Czuj i PID
int8_t stan_czuj[8] = { 0 };
int8_t tab_czuj[8] = { C1,C2,C3,C4,C5,C6,C7,C8 };
int16_t wag_czuj[8] = { -12,-8,-3,-1,1,3,8,12 };
uint8_t V_zal = 130;
int8_t error_last = 0;
int8_t error = 0;
int8_t uchyb, ostatni_uchyb = 0, d_blad;
uint8_t Kp = 15;
uint8_t Kd = 65;
uint8_t przestrzelony;
int16_t regulacja;
int16_t Blad();//zwraca blad
int16_t PID();
int16_t ADC_conv(int8_t czujnik_pin);


//ustawienia na poczatek
void ustaw_czujnik()
{
        DDRC |= (0 << C1) | (0 << C2) | (0 << C3) | (0 << C4) | (0 << C5) | (0 << C6) | (0 << C8);
}
void ADC_conv()
{
        ADCSRA |= ((1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0));
        ADMUX |= (1 << REFS0);
        ADCSRA |= (1 << ADEN);
        ADCSRA |= (1 << ADSC);
}
void set_motors()
{
        DDRD |= (1 << PWMA) | (1 << PWMB) | (1 << PWMB) | (1 << AIN1) | (1 << AIN2) | (1 << BIN1) | (1 << BIN2);
        DDRB |= (1 << STBY);
        PORTD |= (1 << AIN2);
        PORTD |= (1 << BIN2);
}
void set_PWM()
{
        TCCR1A |= (1 << WGM10);
        TCCR1B |= (1 << WGM12);
        TCCR1A |= (1 << COM1A1) | (1 << COM1B1);
        TCCR1B |= (1 << CS10) | (1 << CS11);
        OCR1A = 0;
        OCR1B = 0;
}
void wyw_timer()
{
        OCR2A = reloadValue;
        TCCR2A |= (1 << WGM21);
        TIMSK2 |= (1 << OCIE2A);
        TCCR2B |= (1 << CS22);
        sei();

}
ISR(TIMER2_COMPA_vect) {
        regulacja = PID();
}

int main(void)
{
        ustaw_czujnik(); // ustawienie czujników jako wyjście
		set_PWM();
        set_motors();//ustawienie mostka
        ADC_conv();//ustawienie ADC
        wyw_timer();//timer2
        while (1)
        {
                aktywne_czujniki(tab_czuj);
                uchyb = Blad();
                int16_t regulacja = PID();
                PWM(V_zal - regulacja, V_zal + regulacja);

        }
}

int16_t ADC_conv(int8_t czujnik_pin)//numer pinu jako argument np: PC1
{
        ADMUX &= 0xF0;
        ADMUX |= czujnik_pin;
        ADCSRA |= (1 << ADSC);
        ADCSRA |= (1 << ADSC);
        while (ADCSRA & (1 << ADSC));
        return ADCW;
}
void aktywne_czujniki(int8_t tab[8])//NR PINU jako argument np:PC1
{
        for (int i = 0; i < 8; i++)
        {
                if (ADC_conv(tab[i]) < 150)
                {
                        stan_czuj[i] = 0;
                }
                else
                {
                        stan_czuj[i] = 0;
                }
        }


}
int16_t Blad()
{
        error = 0;
        uint8_t lczujnikow = 0;
        for (uint8_t i = 0; i < 8; i++)
        {
                error = error + tab_czuj[i] * wag_czuj[i];
                lczujnikow += stan_czuj[i];

        }

        if (lczujnikow != 0)
        {
                error /= lczujnikow;
                error_last = error;
        }
        else
                error = error_last;
        if (przestrzelony == 1 && error >= 0)
                przestrzelony = 0;
        else if (przestrzelony == 2 && error <= 0)
                przestrzelony = 0;
        return error;
}
int16_t PID() // FUNKCJA REALIZUJE ALGORYTM REGULATORA PD
{

        d_blad = uchyb - ostatni_uchyb;

        ostatni_uchyb = uchyb;
        return Kp * uchyb + Kd * d_blad;
}
void PWM(int16_t lewy, int16_t prawy)// FUNKCJA
{
        if (lewy >= 0) {
                if (lewy > 255)
                        lewy = 255;
                PORTD |= (1 << AIN2);
                PORTD &= ~(1 << AIN1);

        }
        else
        {
                if (lewy < -255)
                        lewy = -255;
                PORTD |= (1 << AIN1);
                PORTD &= ~(1 << AIN2);


        }

        if (prawy >= 0)
        {
                if (prawy > 255)
                        prawy = 255;
                PORTD |= (1 << BIN2);
                PORTD &= ~(1 << BIN1);

        }
        else
        {
                if (prawy < -255)
                        prawy = -255;
                PORTD |= (1 << BIN1);
                PORTD &= ~(1 << BIN2);
        }

        OCR1A = abs(lewy);
        OCR1B = abs(prawy);

}