1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
| void check_vertical1(int robot_x, int robot_y, int goal_x, int goal_y)
{ if(robot_y==goal_y)
check_horizontal1(robot_x,robot_y,goal_x,goal_y);
else if(robot_y<goal_y)
{ if(sCenter==1)
{ forward(255,255);
robot_y=robot_y+1;
new_state=1;
}
else if(sCenter==0 && sRight==1)
check_horizontal1(robot_x,robot_y,goal_x,goal_y);
else if(sCenter==0 && sLeft==1)
check_horizontal1(robot_x,robot_y,goal_x,goal_y);
else
{ right_turn(200,200);
right_turn(200,200);
new_state=3;
}
}
else if(robot_y>goal_y)
check_horizontal1(robot_x,robot_y,goal_x,goal_y);
}
void check_horizontal1(int robot_x, int robot_y, int goal_x, int goal_y)
{ if(robot_x==goal_x)
{ if(robot_y==goal_y)
robot==0;
else if(robot_y!=goal_y)
{ if(sRight==1)
{ right_turn(200,200);
forward(255,255);
robot_x=robot_x+1;
new_state=2;
}
else if(sRight==0 && sLeft==1)
{ left_turn(200,200);
forward(255,255);
robot_x=robot_x-1;
new_state=4;
}
}
}
else if(robot_x < goal_x)
{ if(sRight==1)
{ right_turn(200,200);
forward(255,255);
robot_x=robot_x+1;
new_state=2;
}
else
{ right_turn(200,200);
right_turn(200,200);
new_state=3;
}
}
else if(robot_x > goal_x)
{ if(sLeft==1)
{ left_turn(200,200);
forward(255,255);
robot_x=robot_x-1;
new_state=4;
}
else
{ right_turn(200,200);
right_turn(200,200);
new_state=3;
}
}
} |