[260] | 1 | #include "r2ca.h"
|
---|
[228] | 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 */
|
---|