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
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85

#include

Servo left;
Servo right;
const int leftSensor=A2;
const int rightSensor=A3;
int i=0;
int FL=1700; // Left Servo Forward
int FR=1300; //Right Servo Forward
int R=0;
int L=0;
void setup()
{
Serial.begin(9600);
left.attach(12); //left servo motor
right.attach(13); //right servo motor
pinMode(leftSensor, INPUT);
pinMode(rightSensor, INPUT);

}
void loop()
{
int leftSens;
int rightSens;
leftSens = ReadSens_L();
rightSens = ReadSens_R();
Serial.println("Beginning");
Serial.println(leftSens);
Serial.println(rightSens);
if(leftSens<400&&rightSens<400) { left.writeMicroseconds(FL); right.writeMicroseconds(FR); Serial.println("Forward!"); delay(40); } else { if(leftSens>400&&rightSens<400)
{
left.writeMicroseconds(1450);
right.writeMicroseconds(1400);
Serial.println("Turn Left!");
delay(10);
Serial.println(leftSens);
delay(10);
Serial.println(rightSens);
delay(10);
int leftSens = ReadSens_L();
delay(10);
int rightSens = ReadSens_R();
delay(10);
}
else
{
if(leftSens<400&&rightSens>400)
{
left.writeMicroseconds(1600);
right.writeMicroseconds(1550);
Serial.println("Turn Right!");
delay(10);
Serial.println(leftSens);
delay(10);
Serial.println(rightSens);
delay(10);
int leftSens = ReadSens_L();
delay(10);
int rightSens = ReadSens_R();
delay(10);
}
else{

}}}
}
int ReadSens_L(){
int i;
int L = 0;
for (i = 0; i < 5; i++){
L = L + analogRead(A2); // sensor on analog pin A2
}

L = L / 5; // average
return L;
}
int ReadSens_R(){
int j;
int R = 0;

for (j = 0; j < 5; j++){
R = R + analogRead(A3); // sensor on analog pin A3
}
R = R / 5; // avarage
return R;
}