1 | #include "r2ca.h"
|
---|
2 | #include <Wire.h>
|
---|
3 | #include <ZumoShield.h>
|
---|
4 |
|
---|
5 | #define BUTTON_BASIC
|
---|
6 | //#define MOTOR_BASIC
|
---|
7 | //#define BUZZER_BASIC
|
---|
8 | //#define GYRO_BASIC
|
---|
9 | //#define REFLECTANCE_BASIC
|
---|
10 | //#define COMPASS_BASIC
|
---|
11 | //#define RELECTORNCE_STOP
|
---|
12 | //#define ROTATIONRESIST
|
---|
13 | //#define FORCEUPHILL
|
---|
14 | //#define LINEFOLLOWER
|
---|
15 | //#define BORDERDETECT
|
---|
16 | //#define SUMOCOLLISIONDETECT
|
---|
17 | //#define MAZESOLVER
|
---|
18 |
|
---|
19 | #ifdef BUTTON_BASIC
|
---|
20 |
|
---|
21 | void button_int() {
|
---|
22 | Serial.println("Button Interrupt!");
|
---|
23 | }
|
---|
24 |
|
---|
25 | void setup() {
|
---|
26 | Serial.begin(115200);
|
---|
27 | ZumoInit();
|
---|
28 | attachInterrupt(ZUMO_BUTTON, button_int, FALLING);
|
---|
29 | }
|
---|
30 |
|
---|
31 | void loop() {
|
---|
32 | button.waitForPress();
|
---|
33 | led.on();
|
---|
34 | delay(1000);
|
---|
35 | led.off();
|
---|
36 | delay(1000);
|
---|
37 | led.on();
|
---|
38 | delay(1000);
|
---|
39 | led.off();
|
---|
40 | button.waitForRelease();
|
---|
41 | }
|
---|
42 | #endif /* BUTTON_BASIC */
|
---|
43 |
|
---|
44 | #ifdef MOTOR_BASIC
|
---|
45 |
|
---|
46 | void setup() {
|
---|
47 | ZumoInit();
|
---|
48 | buzzer.playOn();
|
---|
49 | }
|
---|
50 |
|
---|
51 | void loop() {
|
---|
52 | button.waitForPress();
|
---|
53 |
|
---|
54 | // run left motor forward
|
---|
55 | led.on();
|
---|
56 | for (int speed = 0; speed <= 400; speed++) {
|
---|
57 | motors.setLeftSpeed(speed);
|
---|
58 | delay(2);
|
---|
59 | }
|
---|
60 |
|
---|
61 | for (int speed = 400; speed >= 0; speed--) {
|
---|
62 | motors.setLeftSpeed(speed);
|
---|
63 | delay(2);
|
---|
64 | }
|
---|
65 |
|
---|
66 | // run left motor backward
|
---|
67 | led.off();
|
---|
68 | for (int speed = 0; speed >= -400; speed--) {
|
---|
69 | motors.setLeftSpeed(speed);
|
---|
70 | delay(2);
|
---|
71 | }
|
---|
72 |
|
---|
73 | for (int speed = -400; speed <= 0; speed++) {
|
---|
74 | motors.setLeftSpeed(speed);
|
---|
75 | delay(2);
|
---|
76 | }
|
---|
77 |
|
---|
78 | // run right motor forward
|
---|
79 | led.on();
|
---|
80 | buzzer.playNum(1);
|
---|
81 | for (int speed = 0; speed <= 400; speed++){
|
---|
82 | motors.setRightSpeed(speed);
|
---|
83 | delay(2);
|
---|
84 | }
|
---|
85 |
|
---|
86 | for (int speed = 400; speed >= 0; speed--){
|
---|
87 | motors.setRightSpeed(speed);
|
---|
88 | delay(2);
|
---|
89 | }
|
---|
90 |
|
---|
91 | // run right motor backward
|
---|
92 | led.off();
|
---|
93 | buzzer.playNum(1);
|
---|
94 | for (int speed = 0; speed >= -400; speed--){
|
---|
95 | motors.setRightSpeed(speed);
|
---|
96 | delay(2);
|
---|
97 | }
|
---|
98 |
|
---|
99 | for (int speed = -400; speed <= 0; speed++){
|
---|
100 | motors.setRightSpeed(speed);
|
---|
101 | delay(2);
|
---|
102 | }
|
---|
103 |
|
---|
104 | delay(500);
|
---|
105 |
|
---|
106 | led.on();
|
---|
107 | buzzer.playNum(1);
|
---|
108 | motors.setSpeeds(100, 100);
|
---|
109 | delay(2000);
|
---|
110 | motors.setSpeeds(0, 0);
|
---|
111 |
|
---|
112 | led.off();
|
---|
113 | buzzer.playNum(1);
|
---|
114 | motors.setSpeeds(-100, -100);
|
---|
115 | delay(2000);
|
---|
116 | motors.setSpeeds(0, 0);
|
---|
117 |
|
---|
118 | led.on();
|
---|
119 | buzzer.playNum(1);
|
---|
120 | motors.setSpeeds(-100, 100);
|
---|
121 | delay(2000);
|
---|
122 | motors.setSpeeds(0, 0);
|
---|
123 |
|
---|
124 | led.off();
|
---|
125 | buzzer.playNum(1);
|
---|
126 | motors.setSpeeds(100, -100);
|
---|
127 | delay(2000);
|
---|
128 | motors.setSpeeds(0, 0);
|
---|
129 | }
|
---|
130 | #endif /* MOTOR_BASIC */
|
---|
131 |
|
---|
132 | #ifdef BUZZER_BASIC
|
---|
133 | void setup() {
|
---|
134 | ZumoInit();
|
---|
135 | buzzer.playOn();
|
---|
136 | buzzer.playNum(2);
|
---|
137 | buzzer.playMode(PLAY_AUTOMATIC);
|
---|
138 | Serial.begin(115200);
|
---|
139 | }
|
---|
140 |
|
---|
141 | const char sound_effect[] = "O4 T100 V15 L4 MS g12>c12>e12>G6>E12 ML>G2"; // "charge" melody
|
---|
142 |
|
---|
143 | void loop() {
|
---|
144 | button.waitForPress();
|
---|
145 | buzzer.playFromProgramSpace(sound_effect);
|
---|
146 | button.waitForRelease();
|
---|
147 | }
|
---|
148 | #endif /* BUZZER_BASIC */
|
---|
149 |
|
---|
150 |
|
---|
151 | #ifdef GYRO_BASIC
|
---|
152 | void setup()
|
---|
153 | {
|
---|
154 | Serial.begin(115200);
|
---|
155 |
|
---|
156 | buzzer.playOn();
|
---|
157 | gyro.turnSensorSetup();
|
---|
158 | delay(500);
|
---|
159 | gyro.turnSensorReset();
|
---|
160 |
|
---|
161 | buzzer.playStart();
|
---|
162 | }
|
---|
163 |
|
---|
164 | void loop()
|
---|
165 | {
|
---|
166 | gyro.turnSensorUpdate();
|
---|
167 | Serial.println(gyro.turnAngleDegree);
|
---|
168 | }
|
---|
169 |
|
---|
170 | #endif /* GYRO_BASIC */
|
---|
171 |
|
---|
172 |
|
---|
173 | #ifdef REFLECTANCE_BASIC
|
---|
174 |
|
---|
175 | void setup() {
|
---|
176 | buzzer.playOn();
|
---|
177 | Serial.begin(115200);
|
---|
178 | Serial.println("Zumo sample Start!");
|
---|
179 | }
|
---|
180 |
|
---|
181 | void loop() {
|
---|
182 | reflectances.update();
|
---|
183 |
|
---|
184 | Serial.print(reflectances.value(1));
|
---|
185 | Serial.print(',');
|
---|
186 | Serial.print(reflectances.value(2));
|
---|
187 | Serial.print(',');
|
---|
188 | Serial.print(reflectances.value(3));
|
---|
189 | Serial.print(',');
|
---|
190 | Serial.print(reflectances.value(4));
|
---|
191 | Serial.print(',');
|
---|
192 | Serial.print(reflectances.value(5));
|
---|
193 | Serial.print(',');
|
---|
194 | Serial.print(reflectances.value(6));
|
---|
195 | Serial.print(',');
|
---|
196 | Serial.println();
|
---|
197 |
|
---|
198 | delay(1000);
|
---|
199 | }
|
---|
200 | #endif /* REFLECTANCE_BASIC */
|
---|
201 |
|
---|
202 |
|
---|
203 |
|
---|
204 |
|
---|
205 | #ifdef COMPASS_BASIC
|
---|
206 |
|
---|
207 | #define min(a,b) ((a)<(b)?(a):(b))
|
---|
208 | #define max(a,b) ((a)>(b)?(a):(b))
|
---|
209 | #define SPEED 200
|
---|
210 | #define CALIBRATION_SAMPLES 70
|
---|
211 |
|
---|
212 | void setup() {
|
---|
213 | Serial.begin(115200);
|
---|
214 | ZumoInit();
|
---|
215 | compass.begin();
|
---|
216 | #if 0
|
---|
217 | LSM303::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32767, -32767, -32767};
|
---|
218 | unsigned char index;
|
---|
219 | Serial.println("starting calibration");
|
---|
220 |
|
---|
221 | // To calibrate the magnetometer, the Zumo spins to find the max/min
|
---|
222 | // magnetic vectors. This information is used to correct for offsets
|
---|
223 | // in the magnetometer data.
|
---|
224 | motors.setLeftSpeed(SPEED);
|
---|
225 | motors.setRightSpeed(-SPEED);
|
---|
226 |
|
---|
227 | for(index = 0; index < CALIBRATION_SAMPLES; index ++)
|
---|
228 | {
|
---|
229 | // Take a reading of the magnetic vector and store it in compass.m
|
---|
230 | compass.read();
|
---|
231 |
|
---|
232 | running_min.x = min(running_min.x, compass.m.x);
|
---|
233 | running_min.y = min(running_min.y, compass.m.y);
|
---|
234 |
|
---|
235 | running_max.x = max(running_max.x, compass.m.x);
|
---|
236 | running_max.y = max(running_max.y, compass.m.y);
|
---|
237 |
|
---|
238 | Serial.println(index);
|
---|
239 |
|
---|
240 | delay(50);
|
---|
241 | }
|
---|
242 |
|
---|
243 | motors.setLeftSpeed(0);
|
---|
244 | motors.setRightSpeed(0);
|
---|
245 |
|
---|
246 | Serial.print("max.x ");
|
---|
247 | Serial.print(running_max.x);
|
---|
248 | Serial.println();
|
---|
249 | Serial.print("max.y ");
|
---|
250 | Serial.print(running_max.y);
|
---|
251 | Serial.println();
|
---|
252 | Serial.print("min.x ");
|
---|
253 | Serial.print(running_min.x);
|
---|
254 | Serial.println();
|
---|
255 | Serial.print("min.y ");
|
---|
256 | Serial.print(running_min.y);
|
---|
257 | Serial.println();
|
---|
258 |
|
---|
259 | // Set calibrated values to compass.m_max and compass.m_min
|
---|
260 | compass.m_max.x = running_max.x;
|
---|
261 | compass.m_max.y = running_max.y;
|
---|
262 | compass.m_min.x = running_min.x;
|
---|
263 | compass.m_min.y = running_min.y;
|
---|
264 | button.waitForButton();
|
---|
265 | #endif
|
---|
266 | }
|
---|
267 |
|
---|
268 | void loop() {
|
---|
269 | float heading;
|
---|
270 |
|
---|
271 | heading = compass.averageHeading();
|
---|
272 | Serial.print("Heading: ");
|
---|
273 | Serial.println(heading);
|
---|
274 | }
|
---|
275 |
|
---|
276 | #endif /* COMPASS_BASIC */
|
---|
277 |
|
---|
278 | #ifdef RELECTORNCE_STOP
|
---|
279 |
|
---|
280 | #define REFLECTANCE_THRESHOLD 400
|
---|
281 |
|
---|
282 | void setup() {
|
---|
283 | ZumoInit();
|
---|
284 | Serial.begin(115200);
|
---|
285 | buzzer.playOn();
|
---|
286 |
|
---|
287 | Serial.println("Zumo sample Start!");
|
---|
288 | button.waitForPress();
|
---|
289 | }
|
---|
290 |
|
---|
291 | void loop() {
|
---|
292 | reflectances.update();
|
---|
293 |
|
---|
294 | while ((reflectances.value(3) < REFLECTANCE_THRESHOLD) &&
|
---|
295 | (reflectances.value(4) < REFLECTANCE_THRESHOLD)) {
|
---|
296 | motors.setSpeeds(100, 100);
|
---|
297 | led.on();
|
---|
298 | reflectances.update();
|
---|
299 | }
|
---|
300 | motors.setSpeeds(0, 0);
|
---|
301 | led.off();
|
---|
302 | button.waitForPress();
|
---|
303 | }
|
---|
304 | #endif /* COMPASS_BASIC */
|
---|
305 |
|
---|
306 |
|
---|
307 | #ifdef ROTATIONRESIST
|
---|
308 | /* This demo shows how the Zumo can use its gyroscope to detect
|
---|
309 | when it is being rotated, and use the motors to resist that
|
---|
310 | rotation.
|
---|
311 |
|
---|
312 | This code was tested on a Zumo with 4 NiMH batteries and two 75:1
|
---|
313 | HP micro metal gearmotors. If you have different batteries or
|
---|
314 | motors, you might need to adjust the PID constants.
|
---|
315 |
|
---|
316 | Be careful to not move the robot for a few seconds after starting
|
---|
317 | it while the gyro is being calibrated. During the gyro
|
---|
318 | calibration, the yellow LED is on and the words "Gyro cal" are
|
---|
319 | displayed on the LCD.
|
---|
320 |
|
---|
321 | After the gyro calibration is done, press button A to start the
|
---|
322 | demo. If you try to turn the Zumo, or put it on a surface that
|
---|
323 | is turning, it will drive its motors to counteract the turning.
|
---|
324 |
|
---|
325 | This demo only uses the Z axis of the gyro, so it is possible to
|
---|
326 | pick up the Zumo, rotate it about its X and Y axes, and then put
|
---|
327 | it down facing in a new position. */
|
---|
328 |
|
---|
329 | // This is the maximum speed the motors will be allowed to turn.
|
---|
330 | // A maxSpeed of 400 lets the motors go at top speed. Decrease
|
---|
331 | // this value to impose a speed limit.
|
---|
332 | const int16_t maxSpeed = 400;
|
---|
333 |
|
---|
334 | void setup()
|
---|
335 | {
|
---|
336 | buzzer.playOn();
|
---|
337 | ZumoInit();
|
---|
338 | Serial.begin(115200);
|
---|
339 | button.waitForButton();
|
---|
340 |
|
---|
341 | gyro.turnSensorSetup();
|
---|
342 | delay(500);
|
---|
343 | gyro.turnSensorReset();
|
---|
344 |
|
---|
345 | buzzer.playStart();
|
---|
346 | delay(1000);
|
---|
347 | }
|
---|
348 |
|
---|
349 | void loop()
|
---|
350 | {
|
---|
351 | // Read the gyro to update turnAngle, the estimation of how far
|
---|
352 | // the robot has turned, and turnRate, the estimation of how
|
---|
353 | // fast it is turning.
|
---|
354 | gyro.turnSensorUpdate();
|
---|
355 | Serial.println(gyro.turnAngleDegree);
|
---|
356 |
|
---|
357 | // Calculate the motor turn speed using proportional and
|
---|
358 | // derivative PID terms. Here we are a using a proportional
|
---|
359 | // constant of 56 and a derivative constant of 1/20.
|
---|
360 | int32_t turnSpeed = -(int32_t)(gyro.turnAngle) / (gyro.turnAngle1 / 56)
|
---|
361 | - gyro.turnRate / 20;
|
---|
362 |
|
---|
363 | // Constrain our motor speeds to be between
|
---|
364 | // -maxSpeed and maxSpeed.
|
---|
365 | turnSpeed = constrain(turnSpeed, -maxSpeed, maxSpeed);
|
---|
366 |
|
---|
367 | motors.setSpeeds(-turnSpeed, turnSpeed);
|
---|
368 | }
|
---|
369 | #endif /* ROTATIONRESIST */
|
---|
370 |
|
---|
371 | #ifdef FORCEUPHILL
|
---|
372 | /* This demo shows how the Zumo can use its gyroscope to detect
|
---|
373 | when it is being rotated, and use the motors to resist that
|
---|
374 | rotation.
|
---|
375 |
|
---|
376 | This code was tested on a Zumo with 4 NiMH batteries and two 75:1
|
---|
377 | HP micro metal gearmotors. If you have different batteries or
|
---|
378 | motors, you might need to adjust the PID constants.
|
---|
379 |
|
---|
380 | Be careful to not move the robot for a few seconds after starting
|
---|
381 | it while the gyro is being calibrated. During the gyro
|
---|
382 | calibration, the yellow LED is on and the words "Gyro cal" are
|
---|
383 | displayed on the LCD.
|
---|
384 |
|
---|
385 | After the gyro calibration is done, press button A to start the
|
---|
386 | demo. If you try to turn the Zumo, or put it on a surface that
|
---|
387 | is turning, it will drive its motors to counteract the turning.
|
---|
388 |
|
---|
389 | This demo only uses the Z axis of the gyro, so it is possible to
|
---|
390 | pick up the Zumo, rotate it about its X and Y axes, and then put
|
---|
391 | it down facing in a new position. */
|
---|
392 |
|
---|
393 | // This is the maximum speed the motors will be allowed to turn.
|
---|
394 | // A maxSpeed of 400 lets the motors go at top speed. Decrease
|
---|
395 | // this value to impose a speed limit.
|
---|
396 | const int16_t maxSpeed = 400;
|
---|
397 |
|
---|
398 | void setup()
|
---|
399 | {
|
---|
400 | buzzer.playOn();
|
---|
401 | ZumoInit();
|
---|
402 | Serial.begin(115200);
|
---|
403 |
|
---|
404 | button.waitForButton();
|
---|
405 | compass.begin();
|
---|
406 | buzzer.playStart();
|
---|
407 | led.on();
|
---|
408 | }
|
---|
409 |
|
---|
410 | void loop()
|
---|
411 | {
|
---|
412 | // Read the acceleration from the LSM303.
|
---|
413 | // A value of 16384 corresponds to approximately 1 g.
|
---|
414 | compass.read();
|
---|
415 | int16_t x = compass.a.x;
|
---|
416 | int16_t y = compass.a.y;
|
---|
417 | int32_t magnitudeSquared = (int32_t)x * x + (int32_t)y * y;
|
---|
418 |
|
---|
419 | // Use the encoders to see how much we should drive forward.
|
---|
420 | // If the robot rolls downhill, the encoder counts will become
|
---|
421 | // negative, resulting in a positive forwardSpeed to counteract
|
---|
422 | // the rolling.
|
---|
423 | int16_t forwardSpeed = 20;
|
---|
424 |
|
---|
425 | // See if we are actually on an incline.
|
---|
426 | // 16384 * sin(5 deg) = 1427
|
---|
427 | int16_t turnSpeed;
|
---|
428 | if (magnitudeSquared > (int32_t)1427 * 1427)
|
---|
429 | {
|
---|
430 | // We are on an incline of more than 5 degrees, so
|
---|
431 | // try to face uphill using a feedback algorithm.
|
---|
432 | turnSpeed = y / 16;
|
---|
433 | }
|
---|
434 | else
|
---|
435 | {
|
---|
436 | // We not on a noticeable incline, so don't turn.
|
---|
437 | turnSpeed = 0;
|
---|
438 | }
|
---|
439 |
|
---|
440 | // To face uphill, we need to turn so that the X acceleration
|
---|
441 | // is negative and the Y acceleration is 0. Therefore, when
|
---|
442 | // the Y acceleration is positive, we want to turn to the
|
---|
443 | // left (counter-clockwise).
|
---|
444 | int16_t leftSpeed = forwardSpeed - turnSpeed;
|
---|
445 | int16_t rightSpeed = forwardSpeed + turnSpeed;
|
---|
446 |
|
---|
447 | // Constrain the speeds to be between -maxSpeed and maxSpeed.
|
---|
448 | leftSpeed = constrain(leftSpeed, -maxSpeed, maxSpeed);
|
---|
449 | rightSpeed = constrain(rightSpeed, -maxSpeed, maxSpeed);
|
---|
450 |
|
---|
451 | motors.setSpeeds(leftSpeed, rightSpeed);
|
---|
452 | }
|
---|
453 | #endif /* FORCEUPHILL */
|
---|
454 |
|
---|
455 |
|
---|
456 | #ifdef LINEFOLLOWER
|
---|
457 | /*
|
---|
458 | * Demo line-following code for the Pololu Zumo Robot
|
---|
459 | *
|
---|
460 | * This code will follow a black line on a white background, using a
|
---|
461 | * PID-based algorithm. It works decently on courses with smooth, 6"
|
---|
462 | * radius curves and has been tested with Zumos using 30:1 HP and
|
---|
463 | * 75:1 HP motors. Modifications might be required for it to work
|
---|
464 | * well on different courses or with different motors.
|
---|
465 | *
|
---|
466 | * http://www.pololu.com/catalog/product/2506
|
---|
467 | * http://www.pololu.com
|
---|
468 | * http://forum.pololu.com
|
---|
469 | */
|
---|
470 | int lastError = 0;
|
---|
471 |
|
---|
472 | // This is the maximum speed the motors will be allowed to turn.
|
---|
473 | // (400 lets the motors go at top speed; decrease to impose a speed limit)
|
---|
474 | const int MAX_SPEED = 400;
|
---|
475 |
|
---|
476 | // Sound Effects
|
---|
477 | const char sound_effect[] PROGMEM = "O4 T100 V15 L4 MS g12>c12>e12>G6>E12 ML>G2"; // "charge" melody
|
---|
478 |
|
---|
479 | void setup()
|
---|
480 | {
|
---|
481 | // Play a little welcome song
|
---|
482 | buzzer.playOn();
|
---|
483 | ZumoInit();
|
---|
484 | Serial.begin(115200);
|
---|
485 |
|
---|
486 | // Initialize the reflectance sensors module
|
---|
487 | reflectances.init();
|
---|
488 |
|
---|
489 | // Wait for the user button to be pressed and released
|
---|
490 | button.waitForButton();
|
---|
491 |
|
---|
492 | // Turn on LED to indicate we are in calibration mode
|
---|
493 | led.on();
|
---|
494 |
|
---|
495 | // Wait 1 second and then begin automatic sensor calibration
|
---|
496 | // by rotating in place to sweep the sensors over the line
|
---|
497 | delay(1000);
|
---|
498 | int i;
|
---|
499 | for(i = 0; i < 80; i++) {
|
---|
500 | if ((i > 10 && i <= 30) || (i > 50 && i <= 70))
|
---|
501 | motors.setSpeeds(-200, 200);
|
---|
502 | else
|
---|
503 | motors.setSpeeds(200, -200);
|
---|
504 | reflectances.calibrate();
|
---|
505 |
|
---|
506 | // Since our counter runs to 80, the total delay will be
|
---|
507 | // 80*20 = 1600 ms.
|
---|
508 | delay(20);
|
---|
509 | }
|
---|
510 | motors.setSpeeds(0,0);
|
---|
511 |
|
---|
512 | // Turn off LED to indicate we are through with calibration
|
---|
513 | led.off();
|
---|
514 | buzzer.playOn();
|
---|
515 |
|
---|
516 | // Wait for the user button to be pressed and released
|
---|
517 | button.waitForButton();
|
---|
518 |
|
---|
519 | // Play music and wait for it to finish before we start driving.
|
---|
520 | buzzer.playFromProgramSpace(sound_effect);
|
---|
521 | while(buzzer.isPlaying()){delay(1);};
|
---|
522 | }
|
---|
523 |
|
---|
524 | void loop()
|
---|
525 | {
|
---|
526 | unsigned int sensors[6];
|
---|
527 |
|
---|
528 | // Get the position of the line. Note that we *must* provide the "sensors"
|
---|
529 | // argument to readLine() here, even though we are not interested in the
|
---|
530 | // individual sensor readings
|
---|
531 | int position = reflectances.readLine(sensors);
|
---|
532 |
|
---|
533 | // Our "error" is how far we are away from the center of the line, which
|
---|
534 | // corresponds to position 2500.
|
---|
535 | int error = position - 2500;
|
---|
536 |
|
---|
537 | // Get motor speed difference using proportional and derivative PID terms
|
---|
538 | // (the integral term is generally not very useful for line following).
|
---|
539 | // Here we are using a proportional constant of 1/4 and a derivative
|
---|
540 | // constant of 6, which should work decently for many Zumo motor choices.
|
---|
541 | // You probably want to use trial and error to tune these constants for
|
---|
542 | // your particular Zumo and line course.
|
---|
543 | int speedDifference = error / 4 + 6 * (error - lastError);
|
---|
544 |
|
---|
545 | lastError = error;
|
---|
546 |
|
---|
547 | // Get individual motor speeds. The sign of speedDifference
|
---|
548 | // determines if the robot turns left or right.
|
---|
549 | int m1Speed = MAX_SPEED + speedDifference;
|
---|
550 | int m2Speed = MAX_SPEED - speedDifference;
|
---|
551 |
|
---|
552 | // Here we constrain our motor speeds to be between 0 and MAX_SPEED.
|
---|
553 | // Generally speaking, one motor will always be turning at MAX_SPEED
|
---|
554 | // and the other will be at MAX_SPEED-|speedDifference| if that is positive,
|
---|
555 | // else it will be stationary. For some applications, you might want to
|
---|
556 | // allow the motor speed to go negative so that it can spin in reverse.
|
---|
557 | if (m1Speed < 0)
|
---|
558 | m1Speed = 0;
|
---|
559 | if (m2Speed < 0)
|
---|
560 | m2Speed = 0;
|
---|
561 | if (m1Speed > MAX_SPEED)
|
---|
562 | m1Speed = MAX_SPEED;
|
---|
563 | if (m2Speed > MAX_SPEED)
|
---|
564 | m2Speed = MAX_SPEED;
|
---|
565 |
|
---|
566 | motors.setSpeeds(m1Speed, m2Speed);
|
---|
567 | }
|
---|
568 |
|
---|
569 | #endif /* LINEFOLLOWER */
|
---|
570 |
|
---|
571 | #ifdef BORDERDETECT
|
---|
572 |
|
---|
573 | // this might need to be tuned for different lighting conditions, surfaces, etc.
|
---|
574 | #define QTR_THRESHOLD 600
|
---|
575 |
|
---|
576 | // these might need to be tuned for different motor types
|
---|
577 | #define REVERSE_SPEED 200 // 0 is stopped, 400 is full speed
|
---|
578 | #define TURN_SPEED 200
|
---|
579 | #define FORWARD_SPEED 200
|
---|
580 | #define REVERSE_DURATION 300 // ms
|
---|
581 | #define TURN_DURATION 400 // ms
|
---|
582 |
|
---|
583 | void setup()
|
---|
584 | {
|
---|
585 | buzzer.playOn();
|
---|
586 |
|
---|
587 | ZumoInit();
|
---|
588 |
|
---|
589 | Serial.begin(115200);
|
---|
590 | Serial.println("Zumo sample Start!");
|
---|
591 |
|
---|
592 | led.on();
|
---|
593 | button.waitForButton();
|
---|
594 | led.off();
|
---|
595 | buzzer.playNum(3);
|
---|
596 | delay(1000);
|
---|
597 | buzzer.playStart();
|
---|
598 | delay(1000);
|
---|
599 | }
|
---|
600 |
|
---|
601 | void loop()
|
---|
602 | {
|
---|
603 | if (button.isPressed()) {
|
---|
604 | motors.setSpeeds(0, 0);
|
---|
605 | button.waitForRelease();
|
---|
606 |
|
---|
607 | led.on();
|
---|
608 | button.waitForButton();
|
---|
609 | led.off();
|
---|
610 | buzzer.playNum(3);
|
---|
611 | delay(1000);
|
---|
612 | buzzer.playStart();
|
---|
613 | delay(1000);
|
---|
614 | }
|
---|
615 |
|
---|
616 | reflectances.update();
|
---|
617 |
|
---|
618 | if (reflectances.value(1) > QTR_THRESHOLD) {
|
---|
619 | // if leftmost sensor detects line, reverse and turn to the right
|
---|
620 | motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
|
---|
621 | delay(REVERSE_DURATION);
|
---|
622 | motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
|
---|
623 | delay(TURN_DURATION);
|
---|
624 | motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
|
---|
625 | } else if (reflectances.value(6) > QTR_THRESHOLD) {
|
---|
626 | // if rightmost sensor detects line, reverse and turn to the left
|
---|
627 | motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
|
---|
628 | delay(REVERSE_DURATION);
|
---|
629 | motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
|
---|
630 | delay(TURN_DURATION);
|
---|
631 | motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
|
---|
632 | } else {
|
---|
633 | // otherwise, go straight
|
---|
634 | motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
|
---|
635 | }
|
---|
636 | }
|
---|
637 | #endif /* BORDERDETECT */
|
---|
638 |
|
---|
639 |
|
---|
640 | #ifdef SUMOCOLLISIONDETECT
|
---|
641 | /* This example uses the accelerometer in the Zumo Shield's onboard LSM303DLHC with the LSM303 Library to
|
---|
642 | * detect contact with an adversary robot in the sumo ring. The LSM303 Library is not included in the Zumo
|
---|
643 | * Shield libraries; it can be downloaded separately from GitHub at:
|
---|
644 | *
|
---|
645 | * https://github.com/pololu/LSM303
|
---|
646 | *
|
---|
647 | * This example extends the BorderDetect example, which makes use of the onboard Zumo Reflectance Sensor Array
|
---|
648 | * and its associated library to detect the border of the sumo ring. It also illustrates the use of the
|
---|
649 | * ZumoMotors, PushButton, and ZumoBuzzer libraries.
|
---|
650 | *
|
---|
651 | * In loop(), the program reads the x and y components of acceleration (ignoring z), and detects a
|
---|
652 | * contact when the magnitude of the 3-period average of the x-y vector exceeds an empirically determined
|
---|
653 | * XY_ACCELERATION_THRESHOLD. On contact detection, the forward speed is increased to FULL_SPEED from
|
---|
654 | * the default SEARCH_SPEED, simulating a "fight or flight" response.
|
---|
655 | *
|
---|
656 | * The program attempts to detect contact only when the Zumo is going straight. When it is executing a
|
---|
657 | * turn at the sumo ring border, the turn itself generates an acceleration in the x-y plane, so the
|
---|
658 | * acceleration reading at that time is difficult to interpret for contact detection. Since the Zumo also
|
---|
659 | * accelerates forward out of a turn, the acceleration readings are also ignored for MIN_DELAY_AFTER_TURN
|
---|
660 | * milliseconds after completing a turn. To further avoid false positives, a MIN_DELAY_BETWEEN_CONTACTS is
|
---|
661 | * also specified.
|
---|
662 | *
|
---|
663 | * This example also contains the following enhancements:
|
---|
664 | *
|
---|
665 | * - uses the Zumo Buzzer library to play a sound effect ("charge" melody) at start of competition and
|
---|
666 | * whenever contact is made with an opposing robot
|
---|
667 | *
|
---|
668 | * - randomizes the turn angle on border detection, so that the Zumo executes a more effective search pattern
|
---|
669 | *
|
---|
670 | * - supports a FULL_SPEED_DURATION_LIMIT, allowing the robot to switch to a SUSTAINED_SPEED after a short
|
---|
671 | * period of forward movement at FULL_SPEED. In the example, both speeds are set to 400 (max), but this
|
---|
672 | * feature may be useful to prevent runoffs at the turns if the sumo ring surface is unusually smooth.
|
---|
673 | *
|
---|
674 | * - logging of accelerometer output to the serial monitor when LOG_SERIAL is #defined.
|
---|
675 | *
|
---|
676 | * This example also makes use of the public domain RunningAverage library from the Arduino website; the relevant
|
---|
677 | * code has been copied into this .ino file and does not need to be downloaded separately.
|
---|
678 | */
|
---|
679 |
|
---|
680 | // #define LOG_SERIAL // write log output to serial port
|
---|
681 |
|
---|
682 | // Accelerometer Settings
|
---|
683 | #define RA_SIZE 3 // number of readings to include in running average of accelerometer readings
|
---|
684 | #define XY_ACCELERATION_THRESHOLD 2400 // for detection of contact (~16000 = magnitude of acceleration due to gravity)
|
---|
685 |
|
---|
686 | // this might need to be tuned for different lighting conditions, surfaces, etc.
|
---|
687 | #define QTR_THRESHOLD 1000 // microseconds
|
---|
688 |
|
---|
689 | // these might need to be tuned for different motor types
|
---|
690 | #define REVERSE_SPEED 200 // 0 is stopped, 400 is full speed
|
---|
691 | #define TURN_SPEED 200
|
---|
692 | #define SEARCH_SPEED 200
|
---|
693 | #define SUSTAINED_SPEED 400 // switches to SUSTAINED_SPEED from FULL_SPEED after FULL_SPEED_DURATION_LIMIT ms
|
---|
694 | #define FULL_SPEED 400
|
---|
695 | #define STOP_DURATION 100 // ms
|
---|
696 | #define REVERSE_DURATION 300 // ms
|
---|
697 | #define TURN_DURATION 300 // ms
|
---|
698 |
|
---|
699 | #define RIGHT 1
|
---|
700 | #define LEFT -1
|
---|
701 |
|
---|
702 | enum ForwardSpeed { SearchSpeed, SustainedSpeed, FullSpeed };
|
---|
703 | ForwardSpeed _forwardSpeed; // current forward speed setting
|
---|
704 | unsigned long full_speed_start_time;
|
---|
705 | #define FULL_SPEED_DURATION_LIMIT 250 // ms
|
---|
706 |
|
---|
707 | // Sound Effects
|
---|
708 | const char sound_effect[] PROGMEM = "O4 T100 V15 L4 MS g12>c12>e12>G6>E12 ML>G2"; // "charge" melody
|
---|
709 | // use V0 to suppress sound effect; v15 for max volume
|
---|
710 |
|
---|
711 | // Timing
|
---|
712 | unsigned long loop_start_time;
|
---|
713 | unsigned long last_turn_time;
|
---|
714 | unsigned long contact_made_time;
|
---|
715 | #define MIN_DELAY_AFTER_TURN 400 // ms = min delay before detecting contact event
|
---|
716 | #define MIN_DELAY_BETWEEN_CONTACTS 1000 // ms = min delay between detecting new contact event
|
---|
717 |
|
---|
718 | Accelerometer lsm303;
|
---|
719 | boolean in_contact; // set when accelerometer detects contact with opposing robot
|
---|
720 |
|
---|
721 | // forward declaration
|
---|
722 | void setForwardSpeed(ForwardSpeed speed);
|
---|
723 | void waitForButtonAndCountDown(bool restarting);
|
---|
724 | void turn(char direction, bool randomize);
|
---|
725 | bool check_for_contact();
|
---|
726 | void on_contact_made();
|
---|
727 | void on_contact_lost();
|
---|
728 | int getForwardSpeed();
|
---|
729 | void setForwardSpeed(ForwardSpeed speed);
|
---|
730 |
|
---|
731 | void setup()
|
---|
732 | {
|
---|
733 | buzzer.playOn();
|
---|
734 |
|
---|
735 | ZumoInit();
|
---|
736 |
|
---|
737 | lsm303.begin();
|
---|
738 |
|
---|
739 | #ifdef LOG_SERIAL
|
---|
740 | Serial.begin(115200);
|
---|
741 | lsm303.getLogHeader();
|
---|
742 | #endif
|
---|
743 |
|
---|
744 | randomSeed((unsigned int) millis());
|
---|
745 |
|
---|
746 | led.on();
|
---|
747 | buzzer.playMode(PLAY_AUTOMATIC);
|
---|
748 | waitForButtonAndCountDown(false);
|
---|
749 | }
|
---|
750 |
|
---|
751 | void loop()
|
---|
752 | {
|
---|
753 | if (button.isPressed()) {
|
---|
754 | // if button is pressed, stop and wait for another press to go again
|
---|
755 | motors.setSpeeds(0, 0);
|
---|
756 | button.waitForRelease();
|
---|
757 | waitForButtonAndCountDown(true);
|
---|
758 | }
|
---|
759 |
|
---|
760 | loop_start_time = millis();
|
---|
761 | lsm303.readAcceleration(loop_start_time);
|
---|
762 | reflectances.update();
|
---|
763 |
|
---|
764 | if ((_forwardSpeed == FullSpeed) &&
|
---|
765 | (loop_start_time - full_speed_start_time > FULL_SPEED_DURATION_LIMIT)){
|
---|
766 | setForwardSpeed(SustainedSpeed);
|
---|
767 | }
|
---|
768 |
|
---|
769 | if (reflectances.value(1) > QTR_THRESHOLD) {
|
---|
770 | // if leftmost sensor detects line, reverse and turn to the right
|
---|
771 | turn(RIGHT, true);
|
---|
772 | }
|
---|
773 | else if (reflectances.value(6) > QTR_THRESHOLD) {
|
---|
774 | // if rightmost sensor detects line, reverse and turn to the left
|
---|
775 | turn(LEFT, true);
|
---|
776 | }
|
---|
777 | else {
|
---|
778 | // otherwise, go straight
|
---|
779 | if (check_for_contact()) on_contact_made();
|
---|
780 | int speed = getForwardSpeed();
|
---|
781 | motors.setSpeeds(speed, speed);
|
---|
782 | }
|
---|
783 | }
|
---|
784 |
|
---|
785 | void waitForButtonAndCountDown(bool restarting)
|
---|
786 | {
|
---|
787 | #ifdef LOG_SERIAL
|
---|
788 | Serial.print(restarting ? "Restarting Countdown" : "Starting Countdown");
|
---|
789 | Serial.println();
|
---|
790 | #endif
|
---|
791 |
|
---|
792 | led.on();
|
---|
793 | button.waitForButton();
|
---|
794 | led.off();
|
---|
795 |
|
---|
796 | // play audible countdown
|
---|
797 | buzzer.playNum(3);
|
---|
798 | delay(1000);
|
---|
799 | buzzer.playFromProgramSpace(sound_effect);
|
---|
800 | delay(1000);
|
---|
801 |
|
---|
802 | // reset loop variables
|
---|
803 | in_contact = false; // 1 if contact made; 0 if no contact or contact lost
|
---|
804 | contact_made_time = 0;
|
---|
805 | last_turn_time = millis(); // prevents false contact detection on initial acceleration
|
---|
806 | _forwardSpeed = SearchSpeed;
|
---|
807 | full_speed_start_time = 0;
|
---|
808 | }
|
---|
809 |
|
---|
810 | // execute turn
|
---|
811 | // direction: RIGHT or LEFT
|
---|
812 | // randomize: to improve searching
|
---|
813 | void turn(char direction, bool randomize)
|
---|
814 | {
|
---|
815 | #ifdef LOG_SERIAL
|
---|
816 | Serial.print("turning ...");
|
---|
817 | Serial.println();
|
---|
818 | #endif
|
---|
819 |
|
---|
820 | // assume contact lost
|
---|
821 | on_contact_lost();
|
---|
822 |
|
---|
823 | static unsigned int duration_increment = TURN_DURATION / 4;
|
---|
824 |
|
---|
825 | // motors.setSpeeds(0,0);
|
---|
826 | // delay(STOP_DURATION);
|
---|
827 | motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
|
---|
828 | delay(REVERSE_DURATION);
|
---|
829 | motors.setSpeeds(TURN_SPEED * direction, -TURN_SPEED * direction);
|
---|
830 | delay(randomize ? TURN_DURATION + (random(8) - 2) * duration_increment : TURN_DURATION);
|
---|
831 | int speed = getForwardSpeed();
|
---|
832 | motors.setSpeeds(speed, speed);
|
---|
833 | last_turn_time = millis();
|
---|
834 | }
|
---|
835 |
|
---|
836 | void setForwardSpeed(ForwardSpeed speed)
|
---|
837 | {
|
---|
838 | _forwardSpeed = speed;
|
---|
839 | if (speed == FullSpeed) full_speed_start_time = loop_start_time;
|
---|
840 | }
|
---|
841 |
|
---|
842 | int getForwardSpeed()
|
---|
843 | {
|
---|
844 | int speed;
|
---|
845 | switch (_forwardSpeed)
|
---|
846 | {
|
---|
847 | case FullSpeed:
|
---|
848 | speed = FULL_SPEED;
|
---|
849 | break;
|
---|
850 | case SustainedSpeed:
|
---|
851 | speed = SUSTAINED_SPEED;
|
---|
852 | break;
|
---|
853 | default:
|
---|
854 | speed = SEARCH_SPEED;
|
---|
855 | break;
|
---|
856 | }
|
---|
857 | return speed;
|
---|
858 | }
|
---|
859 |
|
---|
860 | // check for contact, but ignore readings immediately after turning or losing contact
|
---|
861 | bool check_for_contact()
|
---|
862 | {
|
---|
863 | static long threshold_squared = (long) XY_ACCELERATION_THRESHOLD * (long) XY_ACCELERATION_THRESHOLD;
|
---|
864 | return (lsm303.ss_xy_avg() > threshold_squared) && \
|
---|
865 | (loop_start_time - last_turn_time > MIN_DELAY_AFTER_TURN) && \
|
---|
866 | (loop_start_time - contact_made_time > MIN_DELAY_BETWEEN_CONTACTS);
|
---|
867 | }
|
---|
868 |
|
---|
869 | // sound horn and accelerate on contact -- fight or flight
|
---|
870 | void on_contact_made()
|
---|
871 | {
|
---|
872 | #ifdef LOG_SERIAL
|
---|
873 | Serial.print("contact made");
|
---|
874 | Serial.println();
|
---|
875 | #endif
|
---|
876 | in_contact = true;
|
---|
877 | contact_made_time = loop_start_time;
|
---|
878 | setForwardSpeed(FullSpeed);
|
---|
879 | buzzer.playFromProgramSpace(sound_effect);
|
---|
880 | }
|
---|
881 |
|
---|
882 | // reset forward speed
|
---|
883 | void on_contact_lost()
|
---|
884 | {
|
---|
885 | #ifdef LOG_SERIAL
|
---|
886 | Serial.print("contact lost");
|
---|
887 | Serial.println();
|
---|
888 | #endif
|
---|
889 | in_contact = false;
|
---|
890 | setForwardSpeed(SearchSpeed);
|
---|
891 | }
|
---|
892 | #endif /* SUMOCOLLISIONDETECT */
|
---|
893 |
|
---|
894 |
|
---|
895 | #ifdef MAZESOLVER
|
---|
896 |
|
---|
897 | /* This example uses the Zumo Reflectance Sensor Array
|
---|
898 | * to navigate a black line maze with no loops. This program
|
---|
899 | * is based off the 3pi maze solving example which can be
|
---|
900 | * found here:
|
---|
901 | *
|
---|
902 | * http://www.pololu.com/docs/0J21/8.a
|
---|
903 | *
|
---|
904 | * The Zumo first calibrates the sensors to account
|
---|
905 | * for differences of the black line on white background.
|
---|
906 | * Calibration is accomplished in setup().
|
---|
907 | *
|
---|
908 | * In loop(), the function solveMaze() is called and navigates
|
---|
909 | * the Zumo until it finds the finish line which is defined as
|
---|
910 | * a large black area that is thick and wide enough to
|
---|
911 | * cover all six sensors at the same time.
|
---|
912 | *
|
---|
913 | * Once the Zumo reaches the finishing line, it will stop and
|
---|
914 | * wait for the user to place the Zumo back at the starting
|
---|
915 | * line. The Zumo can then follow the shortest path to the finish
|
---|
916 | * line.
|
---|
917 | *
|
---|
918 | * The macros SPEED, TURN_SPEED, ABOVE_LINE(), and LINE_THICKNESS
|
---|
919 | * might need to be adjusted on a case by case basis to give better
|
---|
920 | * line following results.
|
---|
921 | */
|
---|
922 |
|
---|
923 | // SENSOR_THRESHOLD is a value to compare reflectance sensor
|
---|
924 | // readings to to decide if the sensor is over a black line
|
---|
925 | #define SENSOR_THRESHOLD 300
|
---|
926 |
|
---|
927 | // ABOVE_LINE is a helper macro that takes returns
|
---|
928 | // 1 if the sensor is over the line and 0 if otherwise
|
---|
929 | #define ABOVE_LINE(sensor)((sensor) > SENSOR_THRESHOLD)
|
---|
930 |
|
---|
931 | // Motor speed when turning. TURN_SPEED should always
|
---|
932 | // have a positive value, otherwise the Zumo will turn
|
---|
933 | // in the wrong direction.
|
---|
934 | #define TURN_SPEED 200
|
---|
935 |
|
---|
936 | // Motor speed when driving straight. SPEED should always
|
---|
937 | // have a positive value, otherwise the Zumo will travel in the
|
---|
938 | // wrong direction.
|
---|
939 | #define SPEED 200
|
---|
940 |
|
---|
941 | // Thickness of your line in inches
|
---|
942 | #define LINE_THICKNESS .75
|
---|
943 |
|
---|
944 | // When the motor speed of the zumo is set by
|
---|
945 | // motors.setSpeeds(200,200), 200 is in ZUNITs/Second.
|
---|
946 | // A ZUNIT is a fictitious measurement of distance
|
---|
947 | // and only helps to approximate how far the Zumo has
|
---|
948 | // traveled. Experimentally it was observed that for
|
---|
949 | // every inch, there were approximately 17142 ZUNITs.
|
---|
950 | // This value will differ depending on setup/battery
|
---|
951 | // life and may be adjusted accordingly. This value
|
---|
952 | // was found using a 75:1 HP Motors with batteries
|
---|
953 | // partially discharged.
|
---|
954 | #define INCHES_TO_ZUNITS 17142.0
|
---|
955 |
|
---|
956 | // When the Zumo reaches the end of a segment it needs
|
---|
957 | // to find out three things: if it has reached the finish line,
|
---|
958 | // if there is a straight segment ahead of it, and which
|
---|
959 | // segment to take. OVERSHOOT tells the Zumo how far it needs
|
---|
960 | // to overshoot the segment to find out any of these things.
|
---|
961 | #define OVERSHOOT(line_thickness)(((INCHES_TO_ZUNITS * (line_thickness)) / SPEED))
|
---|
962 |
|
---|
963 | // path[] keeps a log of all the turns made
|
---|
964 | // since starting the maze
|
---|
965 | char path[100] = "";
|
---|
966 | unsigned char path_length = 0; // the length of the path
|
---|
967 |
|
---|
968 | void turn(char dir);
|
---|
969 | char selectTurn(unsigned char found_left, unsigned char found_straight, unsigned char found_right);
|
---|
970 | void solveMaze();
|
---|
971 | void goToFinishLine();
|
---|
972 | void simplifyPath();
|
---|
973 |
|
---|
974 | void setup()
|
---|
975 | {
|
---|
976 | unsigned int sensors[6];
|
---|
977 | unsigned short count = 0;
|
---|
978 | unsigned short last_status = 0;
|
---|
979 | int turn_direction = 1;
|
---|
980 |
|
---|
981 | buzzer.playOn();
|
---|
982 | ZumoInit();
|
---|
983 | reflectances.init();
|
---|
984 |
|
---|
985 | delay(500);
|
---|
986 | led.on(); // turn on LED to indicate we are in calibration mode
|
---|
987 |
|
---|
988 | button.waitForButton();
|
---|
989 |
|
---|
990 | // Calibrate the Zumo by sweeping it from left to right
|
---|
991 | for(int i = 0; i < 4; i ++) {
|
---|
992 | // Zumo will turn clockwise if turn_direction = 1.
|
---|
993 | // If turn_direction = -1 Zumo will turn counter-clockwise.
|
---|
994 | turn_direction *= -1;
|
---|
995 |
|
---|
996 | // Turn direction.
|
---|
997 | motors.setSpeeds(turn_direction * TURN_SPEED, -1*turn_direction * TURN_SPEED);
|
---|
998 |
|
---|
999 | // This while loop monitors line position
|
---|
1000 | // until the turn is complete.
|
---|
1001 | while(count < 2) {
|
---|
1002 | reflectances.calibrate();
|
---|
1003 | reflectances.readLine(sensors);
|
---|
1004 | if(turn_direction < 0) {
|
---|
1005 | // If the right most sensor changes from (over white space -> over
|
---|
1006 | // line or over line -> over white space) add 1 to count.
|
---|
1007 | count += ABOVE_LINE(sensors[5]) ^ last_status;
|
---|
1008 | last_status = ABOVE_LINE(sensors[5]);
|
---|
1009 | }
|
---|
1010 | else {
|
---|
1011 | // If the left most sensor changes from (over white space -> over
|
---|
1012 | // line or over line -> over white space) add 1 to count.
|
---|
1013 | count += ABOVE_LINE(sensors[0]) ^ last_status;
|
---|
1014 | last_status = ABOVE_LINE(sensors[0]);
|
---|
1015 | }
|
---|
1016 | }
|
---|
1017 |
|
---|
1018 | count = 0;
|
---|
1019 | last_status = 0;
|
---|
1020 | }
|
---|
1021 |
|
---|
1022 | // Turn left.
|
---|
1023 | turn('L');
|
---|
1024 |
|
---|
1025 | motors.setSpeeds(0, 0);
|
---|
1026 |
|
---|
1027 | // Sound off buzzer to denote Zumo is finished calibrating
|
---|
1028 | buzzer.playStart();
|
---|
1029 |
|
---|
1030 | // Turn off LED to indicate we are through with calibration
|
---|
1031 | led.off();
|
---|
1032 | }
|
---|
1033 |
|
---|
1034 | void loop()
|
---|
1035 | {
|
---|
1036 |
|
---|
1037 | // solveMaze() explores every segment
|
---|
1038 | // of the maze until it finds the finish
|
---|
1039 | // line.
|
---|
1040 | solveMaze();
|
---|
1041 |
|
---|
1042 | // Sound off buzzer to denote Zumo has solved the maze
|
---|
1043 | buzzer.play(">>a32");
|
---|
1044 |
|
---|
1045 | // The maze has been solved. When the user
|
---|
1046 | // places the Zumo at the starting line
|
---|
1047 | // and pushes the Zumo button, the Zumo
|
---|
1048 | // knows where the finish line is and
|
---|
1049 | // will automatically navigate.
|
---|
1050 | while(1) {
|
---|
1051 | button.waitForButton();
|
---|
1052 | goToFinishLine();
|
---|
1053 | // Sound off buzzer to denote Zumo is at the finish line.
|
---|
1054 | buzzer.play(">>a32");
|
---|
1055 | }
|
---|
1056 | }
|
---|
1057 |
|
---|
1058 | // Turns according to the parameter dir, which should be
|
---|
1059 | // 'L' (left), 'R' (right), 'S' (straight), or 'B' (back).
|
---|
1060 | void turn(char dir)
|
---|
1061 | {
|
---|
1062 | // count and last_status help
|
---|
1063 | // keep track of how much further
|
---|
1064 | // the Zumo needs to turn.
|
---|
1065 | unsigned short count = 0;
|
---|
1066 | unsigned short last_status = 0;
|
---|
1067 | unsigned int sensors[6];
|
---|
1068 |
|
---|
1069 | // dir tests for which direction to turn
|
---|
1070 | switch(dir) {
|
---|
1071 |
|
---|
1072 | // Since we're using the sensors to coordinate turns instead of timing them,
|
---|
1073 | // we can treat a left turn the same as a direction reversal: they differ only
|
---|
1074 | // in whether the zumo will turn 90 degrees or 180 degrees before seeing the
|
---|
1075 | // line under the sensor. If 'B' is passed to the turn function when there is a
|
---|
1076 | // left turn available, then the Zumo will turn onto the left segment.
|
---|
1077 | case 'L':
|
---|
1078 | case 'B':
|
---|
1079 | // Turn left.
|
---|
1080 | motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
|
---|
1081 |
|
---|
1082 | // This while loop monitors line position
|
---|
1083 | // until the turn is complete.
|
---|
1084 | while(count < 2) {
|
---|
1085 | reflectances.readLine(sensors);
|
---|
1086 |
|
---|
1087 | // Increment count whenever the state of the sensor changes
|
---|
1088 | // (white->black and black->white) since the sensor should
|
---|
1089 | // pass over 1 line while the robot is turning, the final
|
---|
1090 | // count should be 2
|
---|
1091 | count += ABOVE_LINE(sensors[1]) ^ last_status;
|
---|
1092 | last_status = ABOVE_LINE(sensors[1]);
|
---|
1093 | }
|
---|
1094 |
|
---|
1095 | break;
|
---|
1096 |
|
---|
1097 | case 'R':
|
---|
1098 | // Turn right.
|
---|
1099 | motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
|
---|
1100 |
|
---|
1101 | // This while loop monitors line position
|
---|
1102 | // until the turn is complete.
|
---|
1103 | while(count < 2) {
|
---|
1104 | reflectances.readLine(sensors);
|
---|
1105 | count += ABOVE_LINE(sensors[4]) ^ last_status;
|
---|
1106 | last_status = ABOVE_LINE(sensors[4]);
|
---|
1107 | }
|
---|
1108 |
|
---|
1109 | break;
|
---|
1110 |
|
---|
1111 | case 'S':
|
---|
1112 | // Don't do anything!
|
---|
1113 | break;
|
---|
1114 | }
|
---|
1115 | }
|
---|
1116 |
|
---|
1117 | // This function decides which way to turn during the learning phase of
|
---|
1118 | // maze solving. It uses the variables found_left, found_straight, and
|
---|
1119 | // found_right, which indicate whether there is an exit in each of the
|
---|
1120 | // three directions, applying the "left hand on the wall" strategy.
|
---|
1121 | char selectTurn(unsigned char found_left, unsigned char found_straight,
|
---|
1122 | unsigned char found_right)
|
---|
1123 | {
|
---|
1124 | // Make a decision about how to turn. The following code
|
---|
1125 | // implements a left-hand-on-the-wall strategy, where we always
|
---|
1126 | // turn as far to the left as possible.
|
---|
1127 | if(found_left)
|
---|
1128 | return 'L';
|
---|
1129 | else if(found_straight)
|
---|
1130 | return 'S';
|
---|
1131 | else if(found_right)
|
---|
1132 | return 'R';
|
---|
1133 | else
|
---|
1134 | return 'B';
|
---|
1135 | }
|
---|
1136 |
|
---|
1137 | // The maze is broken down into segments. Once the Zumo decides
|
---|
1138 | // which segment to turn on, it will navigate until it finds another
|
---|
1139 | // intersection. followSegment() will then return after the
|
---|
1140 | // intersection is found.
|
---|
1141 | void followSegment()
|
---|
1142 | {
|
---|
1143 | unsigned int position;
|
---|
1144 | unsigned int sensors[6];
|
---|
1145 | int offset_from_center;
|
---|
1146 | int power_difference;
|
---|
1147 |
|
---|
1148 | while(1) {
|
---|
1149 | // Get the position of the line.
|
---|
1150 | position = reflectances.readLine(sensors);
|
---|
1151 |
|
---|
1152 | // The offset_from_center should be 0 when we are on the line.
|
---|
1153 | offset_from_center = ((int)position) - 2500;
|
---|
1154 |
|
---|
1155 | // Compute the difference between the two motor power settings,
|
---|
1156 | // m1 - m2. If this is a positive number the robot will turn
|
---|
1157 | // to the left. If it is a negative number, the robot will
|
---|
1158 | // turn to the right, and the magnitude of the number determines
|
---|
1159 | // the sharpness of the turn.
|
---|
1160 | power_difference = offset_from_center / 3;
|
---|
1161 |
|
---|
1162 | // Compute the actual motor settings. We never set either motor
|
---|
1163 | // to a negative value.
|
---|
1164 | if(power_difference > SPEED)
|
---|
1165 | power_difference = SPEED;
|
---|
1166 | if(power_difference < -SPEED)
|
---|
1167 | power_difference = -SPEED;
|
---|
1168 |
|
---|
1169 | if(power_difference < 0)
|
---|
1170 | motors.setSpeeds(SPEED + power_difference, SPEED);
|
---|
1171 | else
|
---|
1172 | motors.setSpeeds(SPEED, SPEED - power_difference);
|
---|
1173 |
|
---|
1174 | // We use the inner four sensors (1, 2, 3, and 4) for
|
---|
1175 | // determining whether there is a line straight ahead, and the
|
---|
1176 | // sensors 0 and 5 for detecting lines going to the left and
|
---|
1177 | // right.
|
---|
1178 |
|
---|
1179 | if(!ABOVE_LINE(sensors[0]) && !ABOVE_LINE(sensors[1]) && !ABOVE_LINE(sensors[2]) && !ABOVE_LINE(sensors[3]) && !ABOVE_LINE(sensors[4]) && !ABOVE_LINE(sensors[5])) {
|
---|
1180 | // There is no line visible ahead, and we didn't see any
|
---|
1181 | // intersection. Must be a dead end.
|
---|
1182 | return;
|
---|
1183 | }
|
---|
1184 | else if(ABOVE_LINE(sensors[0]) || ABOVE_LINE(sensors[5])) {
|
---|
1185 | // Found an intersection.
|
---|
1186 | return;
|
---|
1187 | }
|
---|
1188 | }
|
---|
1189 | }
|
---|
1190 |
|
---|
1191 | // The solveMaze() function works by applying a "left hand on the wall" strategy:
|
---|
1192 | // the robot follows a segment until it reaches an intersection, where it takes the
|
---|
1193 | // leftmost fork available to it. It records each turn it makes, and as long as the
|
---|
1194 | // maze has no loops, this strategy will eventually lead it to the finish. Afterwards,
|
---|
1195 | // the recorded path is simplified by removing dead ends. More information can be
|
---|
1196 | // found in the 3pi maze solving example.
|
---|
1197 | void solveMaze()
|
---|
1198 | {
|
---|
1199 | while(1) {
|
---|
1200 | // Navigate current line segment
|
---|
1201 | followSegment();
|
---|
1202 |
|
---|
1203 | // These variables record whether the robot has seen a line to the
|
---|
1204 | // left, straight ahead, and right, while examining the current
|
---|
1205 | // intersection.
|
---|
1206 | unsigned char found_left = 0;
|
---|
1207 | unsigned char found_straight = 0;
|
---|
1208 | unsigned char found_right = 0;
|
---|
1209 |
|
---|
1210 | // Now read the sensors and check the intersection type.
|
---|
1211 | unsigned int sensors[6];
|
---|
1212 | reflectances.readLine(sensors);
|
---|
1213 |
|
---|
1214 | // Check for left and right exits.
|
---|
1215 | if(ABOVE_LINE(sensors[0]))
|
---|
1216 | found_left = 1;
|
---|
1217 | if(ABOVE_LINE(sensors[5]))
|
---|
1218 | found_right = 1;
|
---|
1219 |
|
---|
1220 | // Drive straight a bit more, until we are
|
---|
1221 | // approximately in the middle of intersection.
|
---|
1222 | // This should help us better detect if we
|
---|
1223 | // have left or right segments.
|
---|
1224 | motors.setSpeeds(SPEED, SPEED);
|
---|
1225 | delay(OVERSHOOT(LINE_THICKNESS)/2);
|
---|
1226 |
|
---|
1227 | reflectances.readLine(sensors);
|
---|
1228 |
|
---|
1229 | // Check for left and right exits.
|
---|
1230 | if(ABOVE_LINE(sensors[0]))
|
---|
1231 | found_left = 1;
|
---|
1232 | if(ABOVE_LINE(sensors[5]))
|
---|
1233 | found_right = 1;
|
---|
1234 |
|
---|
1235 | // After driving a little further, we
|
---|
1236 | // should have passed the intersection
|
---|
1237 | // and can check to see if we've hit the
|
---|
1238 | // finish line or if there is a straight segment
|
---|
1239 | // ahead.
|
---|
1240 | delay(OVERSHOOT(LINE_THICKNESS)/2);
|
---|
1241 |
|
---|
1242 | // Check for a straight exit.
|
---|
1243 | reflectances.readLine(sensors);
|
---|
1244 |
|
---|
1245 | // Check again to see if left or right segment has been found
|
---|
1246 | if(ABOVE_LINE(sensors[0]))
|
---|
1247 | found_left = 1;
|
---|
1248 | if(ABOVE_LINE(sensors[5]))
|
---|
1249 | found_right = 1;
|
---|
1250 |
|
---|
1251 | if(ABOVE_LINE(sensors[1]) || ABOVE_LINE(sensors[2]) || ABOVE_LINE(sensors[3]) || ABOVE_LINE(sensors[4]))
|
---|
1252 | found_straight = 1;
|
---|
1253 |
|
---|
1254 | // Check for the ending spot.
|
---|
1255 | // If all four middle sensors are on dark black, we have
|
---|
1256 | // solved the maze.
|
---|
1257 | if(ABOVE_LINE(sensors[1]) && ABOVE_LINE(sensors[2]) && ABOVE_LINE(sensors[3]) && ABOVE_LINE(sensors[4])) {
|
---|
1258 | motors.setSpeeds(0,0);
|
---|
1259 | break;
|
---|
1260 | }
|
---|
1261 |
|
---|
1262 | // Intersection identification is complete.
|
---|
1263 | unsigned char dir = selectTurn(found_left, found_straight, found_right);
|
---|
1264 |
|
---|
1265 | // Make the turn indicated by the path.
|
---|
1266 | turn(dir);
|
---|
1267 |
|
---|
1268 | // Store the intersection in the path variable.
|
---|
1269 | path[path_length] = dir;
|
---|
1270 | path_length++;
|
---|
1271 |
|
---|
1272 | // You should check to make sure that the path_length does not
|
---|
1273 | // exceed the bounds of the array. We'll ignore that in this
|
---|
1274 | // example.
|
---|
1275 |
|
---|
1276 | // Simplify the learned path.
|
---|
1277 | simplifyPath();
|
---|
1278 |
|
---|
1279 | }
|
---|
1280 | }
|
---|
1281 |
|
---|
1282 | // Now enter an infinite loop - we can re-run the maze as many
|
---|
1283 | // times as we want to.
|
---|
1284 | void goToFinishLine()
|
---|
1285 | {
|
---|
1286 | unsigned int sensors[6];
|
---|
1287 | int i = 0;
|
---|
1288 |
|
---|
1289 | // Turn around if the Zumo is facing the wrong direction.
|
---|
1290 | if(path[0] == 'B') {
|
---|
1291 | turn('B');
|
---|
1292 | i++;
|
---|
1293 | }
|
---|
1294 |
|
---|
1295 | for(;i<path_length;i++) {
|
---|
1296 |
|
---|
1297 | followSegment();
|
---|
1298 |
|
---|
1299 | // Drive through the intersection.
|
---|
1300 | motors.setSpeeds(SPEED, SPEED);
|
---|
1301 | delay(OVERSHOOT(LINE_THICKNESS));
|
---|
1302 |
|
---|
1303 | // Make a turn according to the instruction stored in
|
---|
1304 | // path[i].
|
---|
1305 | turn(path[i]);
|
---|
1306 | }
|
---|
1307 |
|
---|
1308 | // Follow the last segment up to the finish.
|
---|
1309 | followSegment();
|
---|
1310 |
|
---|
1311 | // The finish line has been reached.
|
---|
1312 | // Return and wait for another button push to
|
---|
1313 | // restart the maze.
|
---|
1314 | reflectances.readLine(sensors);
|
---|
1315 | motors.setSpeeds(0,0);
|
---|
1316 |
|
---|
1317 | return;
|
---|
1318 | }
|
---|
1319 |
|
---|
1320 |
|
---|
1321 | // simplifyPath analyzes the path[] array and reduces all the
|
---|
1322 | // turns. For example: Right turn + Right turn = (1) Back turn.
|
---|
1323 | void simplifyPath()
|
---|
1324 | {
|
---|
1325 |
|
---|
1326 | // only simplify the path if the second-to-last turn was a 'B'
|
---|
1327 | if(path_length < 3 || path[path_length - 2] != 'B')
|
---|
1328 | return;
|
---|
1329 |
|
---|
1330 | int total_angle = 0;
|
---|
1331 | int i;
|
---|
1332 |
|
---|
1333 | for(i = 1; i <= 3; i++) {
|
---|
1334 | switch(path[path_length - i]) {
|
---|
1335 | case 'R':
|
---|
1336 | total_angle += 90;
|
---|
1337 | break;
|
---|
1338 | case 'L':
|
---|
1339 | total_angle += 270;
|
---|
1340 | break;
|
---|
1341 | case 'B':
|
---|
1342 | total_angle += 180;
|
---|
1343 | break;
|
---|
1344 | }
|
---|
1345 | }
|
---|
1346 |
|
---|
1347 | // Get the angle as a number between 0 and 360 degrees.
|
---|
1348 | total_angle = total_angle % 360;
|
---|
1349 |
|
---|
1350 | // Replace all of those turns with a single one.
|
---|
1351 | switch(total_angle) {
|
---|
1352 | case 0:
|
---|
1353 | path[path_length - 3] = 'S';
|
---|
1354 | break;
|
---|
1355 | case 90:
|
---|
1356 | path[path_length - 3] = 'R';
|
---|
1357 | break;
|
---|
1358 | case 180:
|
---|
1359 | path[path_length - 3] = 'B';
|
---|
1360 | break;
|
---|
1361 | case 270:
|
---|
1362 | path[path_length - 3] = 'L';
|
---|
1363 | break;
|
---|
1364 | }
|
---|
1365 |
|
---|
1366 | // The path is now two steps shorter.
|
---|
1367 | path_length -= 2;
|
---|
1368 | }
|
---|
1369 | #endif /* MAZESOLVER */
|
---|