-
Notifications
You must be signed in to change notification settings - Fork 0
/
lineFollowerMaze
166 lines (139 loc) · 4.42 KB
/
lineFollowerMaze
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
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
#include <Servo.h> // Use the Servo library (included with Arduino IDE)
Servo servoL; // Define the left and right servos
Servo servoR;
int delayTimes[] = {7000, 1000, 1000};
bool isDelayFinished = false;
unsigned long timeSinceStart = 0;
int wDetectNum = 0;
bool isSecond = false;
// Perform these steps with the Arduino is first powered on
void setup()
{
Serial.begin(9600); // Set up Arduino Serial Monitor at 9600 baud
servoL.attach(13); // Attach (programmatically connect) servos to pins on Arduino
servoR.attach(12);
Serial.begin(9600);
}
// This code repeats indefinitely
void loop()
{
decisionTree();
}
void decisionTree() {
DDRD |= B11110000; // Set direction of Arduino pins D4-D7 as OUTPUT
PORTD |= B11110000; // Set level of Arduino pins D4-D7 to HIGH
delayMicroseconds(230); // Short delay to allow capacitor charge in QTI module
DDRD &= B00001111; // Set direction of pins D4-D7 as INPUT
PORTD &= B00001111; // Set level of pins D4-D7 to LOW
delayMicroseconds(230); // Short delay
int pins = PIND; // Get values of pins D0-D7
pins >>= 4; // Drop off first four bits of the port; keep only pins D4-D7
Serial.println(pins, BIN); // Display result of D4-D7 pins in Serial Monitor
// Determine how to steer based on state of the four QTI sensors
int vL, vR;
switch (pins) // Compare pins to known line following states
{
case B1000:
vL = -100;
vR = 100;
break;
case B1100:
vL = 0;
vR = 100;
break;
case B0100:
vL = 50;
vR = 100;
break;
case B0110:
vL = 100;
vR = 100;
break;
case B0010:
vL = 100;
vR = 50;
break;
case B0011:
vL = 100;
vR = 0;
break;
case B0001:
vL = 100;
vR = -100;
break;
case B1111:
vL = 100;
vR = 100;
break;
case B0000:
wDetectNum += 1;
wCounter(wDetectNum);
break;
}
servoL.writeMicroseconds(1500 + vL); // Steer robot to recenter it over the line
servoR.writeMicroseconds(1500 - vR);
delay(50);
}
void wCounter(int wNum) {
if (wNum == 1) { //turn backwards then right to large black line
servoL.writeMicroseconds(1350);
servoR.writeMicroseconds(1650);
delay(1000);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1490);
delay(1200);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1300);
delay(2300);
servoL.writeMicroseconds(1470);
servoR.writeMicroseconds(1700);
delay(1300);
{ else if (wNum == 2) { //turn left if it doesn't lock onto line during turning corners
servoL.writeMicroseconds(1500);
servoR.writeMicroseconds(1300);
delay(2000);
}
} else if (wNum == 3) { // traverse through the white space to reach to end goal
servoL.writeMicroseconds(1350);
servoR.writeMicroseconds(1650);
delay(1000);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1490);
delay(1500);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1300);
delay(800);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1500);
delay(1300);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1300);
delay(3000);
servoL.writeMicroseconds(1500);
servoR.writeMicroseconds(1300);
delay(1500);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1300);
delay(1500);
servoL.writeMicroseconds(1500);
servoR.writeMicroseconds(1300);
delay(1350);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1300);
delay(2950);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1500);
delay(1500);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1300);
delay(1500);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1500);
delay(1350);
servoL.writeMicroseconds(1700);
servoR.writeMicroseconds(1300);
delay(6400);
servoL.detach();
servoR.detach();
}
}