-
Notifications
You must be signed in to change notification settings - Fork 5
/
LineFollowerSimple.ino
231 lines (193 loc) · 5.63 KB
/
LineFollowerSimple.ino
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
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
/* This example uses the line sensors on the 3pi+ 32U4 to follow
a black line on a white background, using a simple conditional-
based algorithm. */
#include <Pololu3piPlus32U4.h>
#include <PololuMenu.h>
using namespace Pololu3piPlus32U4;
// Change next line to this if you are using the older 3pi+
// with a black and green LCD display:
// LCD display;
OLED display;
Buzzer buzzer;
LineSensors lineSensors;
Motors motors;
ButtonA buttonA;
ButtonB buttonB;
ButtonC buttonC;
#define NUM_SENSORS 5
unsigned int lineSensorValues[NUM_SENSORS];
/* Configuration for specific 3pi+ editions: the Standard, Turtle, and
Hyper versions of 3pi+ have different motor configurations, requiring
the demo to be configured with different parameters for proper
operation. The following functions set up these parameters using a
menu that runs at the beginning of the program. To bypass the menu,
you can replace the call to selectEdition() in setup() with one of the
specific functions.
*/
// This is the maximum speed the motors will be allowed to turn.
// A maxSpeed of 400 lets the motors go at top speed. Decrease
// this value to impose a speed limit.
uint16_t maxSpeed;
uint16_t calibrationSpeed;
void selectHyper()
{
motors.flipLeftMotor(true);
motors.flipRightMotor(true);
// Encoders are not used in this example.
// encoders.flipEncoders(true);
maxSpeed = 75;
calibrationSpeed = 50;
}
void selectStandard()
{
maxSpeed = 100;
calibrationSpeed = 60;
}
void selectTurtle()
{
maxSpeed = 200;
calibrationSpeed = 120;
}
PololuMenu<typeof(display)> menu;
void selectEdition()
{
display.clear();
display.print(F("Select"));
display.gotoXY(0,1);
display.print(F("edition"));
delay(1000);
static const PololuMenuItem items[] = {
{ F("Standard"), selectStandard },
{ F("Turtle"), selectTurtle },
{ F("Hyper"), selectHyper },
};
menu.setItems(items, 3);
menu.setDisplay(display);
menu.setBuzzer(buzzer);
menu.setButtons(buttonA, buttonB, buttonC);
while(!menu.select());
display.gotoXY(0,1);
display.print("OK! ...");
}
// Sets up special characters in the LCD so that we can display
// bar graphs.
void loadCustomCharacters()
{
static const char levels[] PROGMEM = {
0, 0, 0, 0, 0, 0, 0, 63, 63, 63, 63, 63, 63, 63
};
display.loadCustomCharacter(levels + 0, 0); // 1 bar
display.loadCustomCharacter(levels + 1, 1); // 2 bars
display.loadCustomCharacter(levels + 2, 2); // 3 bars
display.loadCustomCharacter(levels + 3, 3); // 4 bars
display.loadCustomCharacter(levels + 4, 4); // 5 bars
display.loadCustomCharacter(levels + 5, 5); // 6 bars
display.loadCustomCharacter(levels + 6, 6); // 7 bars
}
void printBar(uint8_t height)
{
if (height > 8) { height = 8; }
const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, (char)255};
display.print(barChars[height]);
}
void calibrateSensors()
{
display.clear();
// Wait 1 second and then begin automatic sensor calibration
// by rotating in place to sweep the sensors over the line
delay(1000);
for(uint16_t i = 0; i < 80; i++)
{
if (i > 20 && i <= 60)
{
motors.setSpeeds(-(int16_t)calibrationSpeed, calibrationSpeed);
}
else
{
motors.setSpeeds(calibrationSpeed, -(int16_t)calibrationSpeed);
}
lineSensors.calibrate();
}
motors.setSpeeds(0, 0);
}
// Displays the estimated line position and a bar graph of sensor readings on
// the LCD. Returns after the user presses B.
void showReadings()
{
display.clear();
while(!buttonB.getSingleDebouncedPress())
{
uint16_t position = lineSensors.readLineBlack(lineSensorValues);
display.gotoXY(0, 0);
display.print(position);
display.print(" ");
display.gotoXY(0, 1);
for (uint8_t i = 0; i < NUM_SENSORS; i++)
{
uint8_t barHeight = map(lineSensorValues[i], 0, 1000, 0, 8);
printBar(barHeight);
}
delay(50);
}
}
void setup()
{
// Uncomment if necessary to correct motor directions:
//motors.flipLeftMotor(true);
//motors.flipRightMotor(true);
loadCustomCharacters();
// Play a little welcome song
buzzer.play(">g32>>c32");
// To bypass the menu, replace this function with
// selectHyper(), selectStandard(), or selectTurtle().
selectEdition();
// Wait for button B to be pressed and released.
display.clear();
display.print(F("Press B"));
display.gotoXY(0, 1);
display.print(F("to calib"));
while(!buttonB.getSingleDebouncedPress());
calibrateSensors();
showReadings();
// Play music and wait for it to finish before we start driving.
display.clear();
display.print(F("Go!"));
buzzer.play("L16 cdegreg4");
while(buzzer.isPlaying());
}
void loop()
{
// Get the position of the line. Note that we *must* provide
// the "lineSensorValues" argument to readLineBlack() here, even
// though we are not interested in the individual sensor
// readings.
int16_t position = lineSensors.readLineBlack(lineSensorValues);
if (position < 1000)
{
// We are far to the right of the line: turn left.
// Set the right motor to 100 and the left motor to zero,
// to do a sharp turn to the left. Note that the maximum
// value of either motor speed is 400, so we are driving
// it at just about 25% of the max.
motors.setSpeeds(0, maxSpeed);
// Just for fun, indicate the direction we are turning on
// the LEDs.
ledYellow(1);
ledRed(0);
}
else if (position < 3000)
{
// We are somewhat close to being centered on the line:
// drive straight.
motors.setSpeeds(maxSpeed, maxSpeed);
ledYellow(1);
ledRed(1);
}
else
{
// We are far to the left of the line: turn right.
motors.setSpeeds(maxSpeed, 0);
ledYellow(0);
ledRed(1);
}
}