/**************************************************************************************************************/
/* kod do Linefollowera PRIMUS*/
/* Atmega8*/


#include <avr/io.h>
#include <util/delay.h>

//silnik prawy
#define MP1_DIR DDRD
#define MP1_PORT PORTD
#define MP1 (1 << PD7)
#define MP2_DIR DDRD
#define MP2_PORT PORTD
#define MP2 (1 << PD2)
#define MPP_DIR DDRB
#define MPP_PORT PORTB
#define MPP (1 << PB2)
//silnik lewy
#define ML1_DIR DDRD
#define ML1_PORT PORTD
#define ML1 (1 << PD1)
#define ML2_DIR DDRD
#define ML2_PORT PORTD
#define ML2 (1 << PD0)
#define MLP_DIR DDRB
#define MLP_PORT PORTB
#define MLP (1 << PB1)
#define prog 180
int Init_ADC()
{
ADMUX|=(_BV(ADLAR) | _BV(REFS0) | _BV(REFS1)) ;   /*wlaczenie i wybranie wewn odnośnika 2,56V */
ADCSRA|=(_BV(ADEN) | _BV(ADPS2) | _BV(ADPS1)) ;
ADCSRA&=~(_BV(ADSC)|_BV(ADIF) |(1<<5)|_BV(ADIE)|_BV(ADPS1));
return 0;
}
int Read_ADC(int nr)
{
ADMUX = nr;
ADMUX|=(_BV(ADLAR) | _BV(REFS0)) ;
ADCSRA|=_BV(ADSC);
while(!((ADCSRA&_BV(ADIF))>>ADIF));
return ADCH;
}
/*********************** kierunki silników *****************************************/
int prawy(int kier,int moc)   /* prawy silnik*/
{
if(kier == 1)
{
MP1_PORT |= MP1;
MP2_PORT &= ~MP2;
}
if(kier == -1)
{
MP1_PORT &= ~MP1;  /* MP1= (1<<PD7)*/
MP2_PORT |= MP2;   /* MP2=(1<<PD2)*/
}
if(kier == 0)
{
MP1_PORT |= MP1;
MP2_PORT |= MP2;
}
return 0;
}

int lewy(int kier,int moc)   /* lewy silnik*/
{
if(kier == 1)
{
ML1_PORT |= ML1;
ML2_PORT &= ~ML2;
}
if(kier == -1)
{
ML1_PORT &= ~ML1;
ML2_PORT |= ML2;
}
if(kier == 0)
{
ML1_PORT |= ML1;
ML2_PORT |= ML2;
}
return 0;
}
int pwm_init()
{
ML1_DIR |= ML1;
ML2_DIR |= ML2;
MLP_DIR |= MLP;
MLP_PORT |= MLP;
MPP_PORT |= MPP;
MP1_DIR |= MP1;
MP2_DIR |= MP2;
MPP_DIR |= MPP;
/*Ustawienie sprzętowego PWM */
ICR1 = 20;
OCR1A =20;
OCR1B = 20;
TCCR1A=(1<<COM1A1)|(1<<COM1B1)|(1<<WGM11);
TCCR1B=(1<<WGM13)|(1<WGM12)|(1<<CS10);
return 0;
}
int main()
{
Init_ADC();
pwm_init();
int L,P,S,LS,PS;
while(1)
{
L = Read_ADC(3) ;
P = Read_ADC(1);
S = Read_ADC(2);
LS = Read_ADC(4);
PS = Read_ADC(0);
/****************************sterowanie silnikami ****************************************/
/* jeżeli czujnik>prog to jest nad czarną linią*/

/*wszystkie czujniki widzą linie*/
if(LS >prog && L>prog && S>prog && P>prog && PS>prog )
{
lewy(0,100);
prawy(0,100);
}
/* zgubienie trasy*/
if(LS<prog && L<prog && S<prog && P<prog && PS<prog )
{
lewy(0,100);
prawy(0,100);
}
/* lewy czujnik widzi*/
if(LS<prog && L > prog && S<prog && P < prog && PS<prog)
{
lewy(0,100);
prawy(1,100);
}
/* prawy czujnik widzi*/
if(LS<prog && L < prog && S<prog && P > prog && PS<prog)
{
lewy(1,100);
prawy(0,100);
}
/* jazda na wprost*/
if(LS<prog && L < prog && S>prog && P < prog && PS<prog)
{
lewy(1,100);
prawy(1,100);
}
/*L oraz S widzą linie*/
if(LS<prog && L > prog && S>prog && P < prog && PS<prog)
{
lewy(1,100);
prawy(1,100);
}
/*P oraz S widzą linie*/
if(LS<prog && L < prog && S>prog && P > prog && PS<prog)
{
lewy(1,100);
prawy(1,100);
}
/* Lewy skrajny*/
if(LS>prog && L < prog && S<prog && P < prog && PS<prog)
{
lewy(-1,100);
prawy(1,100);
}
/* Prawy skrajny*/
if(LS<prog && L < prog && S<prog && P < prog && PS>prog)
{
lewy(1,100);
prawy(-1,100);
}
/*widzi LewySkrajny i Lewy*/
if(LS>prog && L > prog && S<prog && P < prog && PS<prog)
{
lewy(1,100);
prawy(0,100);
}
/* widzi PrawySkrajny i Prawy*/
if(LS<prog && L < prog && S<prog && P > prog && PS>prog)
{
lewy(0,100);
prawy(1,100);
}
/* widzi Środkowy,Lewy ,LewySkrajny*/
if(LS>prog && L>prog && S>prog && P<prog && PS<prog )
{
lewy(-1,100);
prawy(1,100);
}
/* widzi Środkowy,Prawy,PrawySkrajny*/
if(LS<prog && L<prog && S>prog && P>prog && PS>prog )
{
lewy(1,100);
prawy(-1,100);
}
/*widzą 4 czujniki ( PS,P,S,L)*/
if(LS<prog && L<prog && S>prog && P>prog && PS>prog )
{
lewy(1,100);
prawy(-1,100);
}
/*widzą 4 czujniki (LS,L,S,P)*/
if(LS<prog && L<prog && S>prog && P>prog && PS>prog )
{
lewy(-1,100);
prawy(1,100);
}
}
}