/*
? *
? * Function Name: <move_forward()>
? * Logic: detecting junction while following black line<d>
? * Example Call: <move_forward()>
? *
? */
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_L) && (Center_white_line > THRESHOLD_WL_C) && (Right_white_line > THRESHOLD_WL_R)) | ((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)))
{
motion_set(Stop);
preference();
k=k+1;
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();
break;
}
}
}while(1);
motion_set(Stop);
}
//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);
}
void main()
{
unsigned int value,value1;
int a=0,b=0;
cli();
INIT_PORTS(); //Initialize ports
uart0_init(); //Initialize UART0 for xbee communication
timer5_init();
sei();
INIT_PORTS_ROTATE(); //Initialize ports
right_position_encoder_interrupt_init(); //Initialize control registers for wheel
left_position_encoder_interrupt_init(); // encoders.
init_devices();
lcd_set_4bit(); //LCD initialization functions.
lcd_init();
unsigned char angle = 0;
init_devices_servo(); //Initialize servo motors.
data='0';
sharp = ADC_Conversion(11); //Stores the Analog value of front sharp connected to ADC channel 11 into variable "sharp"
value = Sharp_GP2D12_estimation(sharp); //Stores Distance calsulated in a variable "value".
// lcd_print(1,1,value,3);
// sharp1 = ADC_Conversion(10); //Stores the Analog value of front sharp connected to ADC channel 11 into variable "sharp"
// value1 = Sharp_GP2D12_estimation(sharp1); //Stores Distance calsulated in a variable "value".
// lcd_print(1,5,value1,3);
// set servo for camera to ground zero
servo_1(0); // code to bring the camera to ground state
_delay_ms(1000);
// set servo for incline to ground zero
servo_2(90); // align camera
_delay_ms(500);
// the bot rotating slowly trying to find the bot
int terminate = 0;
while(data=='0' && terminate < 150)
{
velocity(150,150); //If no ball is detected the rotate and scan
angle_rotate_left(3); // for ball in the arena.
_delay_ms(500);
stop();
_delay_ms(500);
sharp = ADC_Conversion(11); //Stores the Analog value of front sharp connected to ADC channel 11 into variable "sharp"
value = Sharp_GP2D12_estimation(sharp); //Stores Distance calsulated in a variable "value".
terminate++;
// lcd_print(1,1,value,3);
}
if(terminate == 60)
{
buzzer_on();
return;
}
/*If a ball(red colour) is detected then matlab code sends a '5' signal through
zigbee.If a '5' is received then the robot stops rotating and moves towards the
ball*/
value2 = 100;
if(atoi(data) > 5)
{
lcd_print(1,5,value2,2);
}
velocity(160,184);
// change value
while(value>140)
{
sharp = ADC_Conversion(11); //Stores the Analog value of front sharp connected to ADC channel 11 into variable "sharp"
value = Sharp_GP2D12_estimation(sharp); //Stores Distance calsulated in a variable "value".
lcd_print(1,1,value,3);
forward();
}
stop();
_delay_ms(2000);
/* the servo_1 is for the camera and the servo_2 is for the inclined plane */
// rotate 90 at left
left90_at_place();
_delay_ms(1000);
// rotate the camera 90 degrees to the right
servo_1(90);
_delay_ms(1000);
//rotate in circle
rotate_in_circle();
int perceived_height;
perceived_height = atoi(data);
lcd_print(1,5,perceived_height,5);
// rotate 90 degrees to the right to face the ball again
right90_at_place();
stop();
_delay_ms(2000);
// rotate camera also to ground state to face the ball again
servo_1(0); // code to bring the camera to ground state
_delay_ms(1000);
// set servo for incline
int angle_of_inclination = get_angle(perceived_height);
lcd_print(1,2,angle_of_inclination,5);
servo_2(angle_of_inclination);
_delay_ms(1000);
while(1);
}
