//junction detection and maze solving algorithm
void move_forward(void){
OCR5AL = SPEED_FAST_L;
OCR5BL = SPEED_FAST_R;
unsigned char count_node = 0;
ShaftCountLeft = 0;
motion_set(Forward);
flag=0;
do{
Initialise_White_Line_Sensors();
Average_Seven_White_Line_Sensors();
follow();
if (((Left_white_line < THRESHOLD_WL_L1) && (Center_white_line > THRESHOLD_WL_C1) && (Right_white_line > THRESHOLD_WL_R1))|((Left_white_line > THRESHOLD_WL_L1) && (Center_white_line > THRESHOLD_WL_C1) && (Right_white_line > THRESHOLD_WL_R1)))
{
move_forward_mm(COUNT_MM_20);
if ((Left_white_line > THRESHOLD_WL_L1) && (Center_white_line > THRESHOLD_WL_C1) && (Right_white_line > THRESHOLD_WL_R1))
{
PORTC=0x08;
_delay_ms(500);
PORTC=0x00;
rotate_right_end();
move_forward_mm(COUNT_MM_80_F);
move_forward_mm(COUNT_MM_80_F);
move_forward_mm(COUNT_MM_80_F);
flag=1;
break;
}
else
{
if (tag==0)
{
move_reverse_mm(COUNT_MM_20);
PORTJ=0x00;
rotate_right();
j=j+1;
a=d[j];
d[j]=1;
d[j]=d[j]+a;
if (d[j]==4)
{
d[j]=0;
j=j-2;
break;
}
else
{
break;
}
}
else if (tag==1)
{
move_reverse_mm(COUNT_MM_20);
PORTJ=0x00;
rotate_right();
j=j+1;
a=d[j];
d[j]=1;
d[j]=d[j]+a;
k=k+1;
b=d1[k];
d1[k]=1;
d1[k]=d1[k]+b;
if (((d[j]==4) && (d1[k]!=4)))
{
d[j]=0;
j=j-2;
break;
}
else if (((d[j]==4) && (d1[k]==4)))
{
d1[k]=0;
k=k-2;
d[j]=0;
j=j-2;
break;
}
else
{
break;
}
}
}
}
else if ((Left_white_line > THRESHOLD_WL_L1) && (Center_white_line > THRESHOLD_WL_C1) && (Right_white_line < THRESHOLD_WL_R1))
{
move_forward_mm(COUNT_MM_40);
if ((Left_white_line < THRESHOLD_WL_L1) && (Center_white_line > THRESHOLD_WL_C1) && (Right_white_line < THRESHOLD_WL_R1))
{
PORTJ=0xFF;
if (tag==0)
{
j=j+1;
a=d[j];
d[j]=2;
d[j]=d[j]+a;
if (d[j]==4)
{
d[j]=0;
j=j-2;
break;
}
else
{
break;
}
}
else if (tag==1)
{
j=j+1;
a=d[j];
d[j]=2;
d[j]=d[j]+a;
k=k+1;
b=d1[k];
d1[k]=2;
d1[k]=d1[k]+b;
if (((d[j]==4) && (d1[k]!=4)))
{
d[j]=0;
j=j-2;
break;
}
else if (((d[j]==4) && (d1[k]==4)))
{
d1[k]=0;
k=k-2;
d[j]=0;
j=j-2;
break;
}
else
{
break;
}
}
}
else if ((Left_white_line < THRESHOLD_WL_L1) && (Center_white_line < THRESHOLD_WL_C1) && (Right_white_line < THRESHOLD_WL_R1))
{
OCR5AL = SPEED_SLOW_L;
OCR5BL = SPEED_SLOW_R;
motion_set(Right);
angle_rotate_right();
if ((Center_white_line > THRESHOLD_WL_C1))
{
break;
}
else
{
OCR5AL = SPEED_SLOW_L;
OCR5BL = SPEED_SLOW_R;
motion_set(Left);
angle_rotate_left();
if ((Left_white_line < THRESHOLD_WL_L1) && (Center_white_line < THRESHOLD_WL_C1) && (Right_white_line < THRESHOLD_WL_R1))
{
if (tag==0)
{
move_reverse_mm(COUNT_MM_30);
rotate_left();
j=j+1;
a=d[j];
d[j]=3;
d[j]=d[j]+a;
if (d[j]==4)
{
d[j]=0;
j=j-2;
break;
}
else
{
break;
}
}
else if (tag==1)
{
move_reverse_mm(COUNT_MM_30);
rotate_left();
j=j+1;
a=d[j];
d[j]=3;
d[j]=d[j]+a;
k=k+1;
b=d1[k];
d1[k]=3;
d1[k]=d1[k]+b;
if (((d[j]==4) && (d1[k]!=4)))
{
d[j]=0;
j=j-2;
break;
}
else if (((d[j]==4) && (d1[k]==4)))
{
d1[k]=0;
k=k-2;
d[j]=0;
j=j-2;
break;
}
else
{
break;
}
}
}
else
{
break;
}
}
}
}
else if((Left_white_line < THRESHOLD_WL_L1) && (Center_white_line < THRESHOLD_WL_C1) && (Right_white_line < THRESHOLD_WL_R1)){
OCR5AL = SPEED_SLOW_L;
OCR5BL = SPEED_SLOW_R;
TCCR4B |= (1 << CS42) | (1 << CS40);
if (time>10)
{
move_forward_mm(COUNT_MM_30);
if((Left_white_line < THRESHOLD_WL_L1) && (Center_white_line < THRESHOLD_WL_C1) && (Right_white_line < THRESHOLD_WL_R1))
{
u_turn();
j--;
k--;
time=0;
TCCR4B |= (0 << CS42) | (0 << CS40);
break;
}
else if((Left_white_line > THRESHOLD_WL_L1) || (Center_white_line > THRESHOLD_WL_C1) || (Right_white_line > THRESHOLD_WL_R1))
{
u_turn();
j--;
if (tag==1)
{
k--;
}
PORTJ=0xFF;
tag=1;
time=0;
TCCR4B |= (0 << CS42) | (0 << CS40);
break;
}
}
else
{
motion_set(Right);
angle_rotate_right();
if ((Center_white_line > THRESHOLD_WL_C1)/*|(Right_white_line > THRESHOLD_WL_R1)*/)
{
time=0;
TCCR4B |= (0 << CS42) | (0 << CS40);
break;
}
else
{
OCR5AL = SPEED_SLOW_L;
OCR5BL = SPEED_SLOW_R;
motion_set(Left);
angle_rotate_left();
break;
}
}
}
}while(1);
motion_set(Stop);
}
int main(void)
{
/*TCCR1A=0xA1;
TCCR1B=0x05;
OCR1A=150; //Left Motor PWM
OCR1B=150; //Right Motor PWM
DDRD=0x30; //Declaring PD4,PD5 as output*/
DDRA=0X00; //Sensor Input
PORTA=0X07;
DDRB=0x0F;
while(1)
{
switch(PINA & 0X07)
{
case 0x00: //0
{
left();
}
break;
case 0x01: //1
{
left();
}
break;
case 0x02: //2
{
straight();
}
break;
case 0x03: //3
{
left();
}
break;
case 0x04: //4
{
right();
}
break;
case 0x05: //5
{
straight();
}
break;
case 0x06: //6
{
right();
}
break;
case 0x07: //7
{
u_turn();
}
break;
}
}
return 0;
}
