[136] | 1 | /* Robot Rescue
|
---|
| 2 |
|
---|
| 3 | In this example, the robot enters the line following mode and
|
---|
| 4 | plays some music until it reaches its target. Once it finds the
|
---|
| 5 | target, it pushes it out of the track. It then returns to the
|
---|
| 6 | track and looks for a second target.
|
---|
| 7 |
|
---|
| 8 | You can make the robot push as many objects as you want to, just
|
---|
| 9 | add more to calls to the rescue function or even move that code
|
---|
| 10 | into the loop.
|
---|
| 11 |
|
---|
| 12 | Circuit:
|
---|
| 13 | * Arduino Robot
|
---|
| 14 | * some objects for the robot to push
|
---|
| 15 | * a line-following circuit
|
---|
| 16 |
|
---|
| 17 | created 1 May 2013
|
---|
| 18 | by X. Yang
|
---|
| 19 | modified 12 May 2013
|
---|
| 20 | by D. Cuartielles
|
---|
| 21 |
|
---|
| 22 | This example is in the public domain
|
---|
| 23 | */
|
---|
| 24 |
|
---|
| 25 | #include <ArduinoRobot.h> // include the robot library
|
---|
| 26 | #include <Wire.h>
|
---|
| 27 | #include <SPI.h>
|
---|
| 28 |
|
---|
| 29 | void setup() {
|
---|
| 30 | // initialize the Robot, SD card, display, and speaker
|
---|
| 31 | Robot.begin();
|
---|
| 32 | Robot.beginTFT();
|
---|
| 33 | Robot.beginSD();
|
---|
| 34 | Robot.beginSpeaker();
|
---|
| 35 |
|
---|
| 36 | // draw "lg0.bmp" and "lg1.bmp" on the screen
|
---|
| 37 | Robot.displayLogos();
|
---|
| 38 |
|
---|
| 39 | // display the line following instructional image from the SD card
|
---|
| 40 | Robot.drawBMP("lf.bmp", 0, 0);
|
---|
| 41 |
|
---|
| 42 | // play the chase music file
|
---|
| 43 | Robot.playFile("chase.sqm");
|
---|
| 44 |
|
---|
| 45 | // add the instructions
|
---|
| 46 | Robot.text("Rescue\n\n place the robot on\n the rescue track\n pushing the\n obstacles away", 5, 5);
|
---|
| 47 | Robot.text("Press the middle\n button to start...", 5, 61);
|
---|
| 48 | Robot.waitContinue();
|
---|
| 49 |
|
---|
| 50 | // start
|
---|
| 51 | Robot.fill(255, 255, 255);
|
---|
| 52 | Robot.stroke(255, 255, 255);
|
---|
| 53 | Robot.rect(0, 0, 128, 80); // erase the previous text
|
---|
| 54 | Robot.stroke(0, 0, 0);
|
---|
| 55 | Robot.text("Start", 5, 5);
|
---|
| 56 |
|
---|
| 57 | // use this to calibrate the line following algorithm
|
---|
| 58 | // uncomment one or the other to see the different behaviors of the robot
|
---|
| 59 | // Robot.lineFollowConfig(14, 9, 50, 10);
|
---|
| 60 | Robot.lineFollowConfig(11, 7, 60, 5);
|
---|
| 61 |
|
---|
| 62 | // run the rescue sequence
|
---|
| 63 | rescueSequence();
|
---|
| 64 | // find the track again
|
---|
| 65 | goToNext();
|
---|
| 66 | // run the rescue sequence a second time
|
---|
| 67 | rescueSequence();
|
---|
| 68 |
|
---|
| 69 | // here you could go on ...
|
---|
| 70 |
|
---|
| 71 |
|
---|
| 72 | }
|
---|
| 73 |
|
---|
| 74 | void loop() {
|
---|
| 75 | //nothing here, the program only runs once.
|
---|
| 76 | }
|
---|
| 77 |
|
---|
| 78 | // run the sequence
|
---|
| 79 | void rescueSequence() {
|
---|
| 80 | //set the motor board into line-follow mode
|
---|
| 81 | Robot.setMode(MODE_LINE_FOLLOW);
|
---|
| 82 |
|
---|
| 83 | while (!Robot.isActionDone()) { // wait until it is no longer following the line
|
---|
| 84 | }
|
---|
| 85 | delay(1000);
|
---|
| 86 |
|
---|
| 87 | // do the rescue operation
|
---|
| 88 | doRescue();
|
---|
| 89 | delay(1000);
|
---|
| 90 | }
|
---|
| 91 |
|
---|
| 92 | void doRescue() {
|
---|
| 93 | // Reached the endline, engage the target
|
---|
| 94 | Robot.motorsWrite(200, 200);
|
---|
| 95 | delay(250);
|
---|
| 96 | Robot.motorsStop();
|
---|
| 97 | delay(1000);
|
---|
| 98 |
|
---|
| 99 | // Turn the robot
|
---|
| 100 | Robot.turn(90);
|
---|
| 101 | Robot.motorsStop();
|
---|
| 102 | delay(1000);
|
---|
| 103 |
|
---|
| 104 | // Move forward
|
---|
| 105 | Robot.motorsWrite(200, 200);
|
---|
| 106 | delay(500);
|
---|
| 107 | Robot.motorsStop();
|
---|
| 108 | delay(1000);
|
---|
| 109 |
|
---|
| 110 | // move backwards, leave the target
|
---|
| 111 | Robot.motorsWrite(-200, -200);
|
---|
| 112 | delay(500);
|
---|
| 113 | Robot.motorsStop();
|
---|
| 114 | }
|
---|
| 115 |
|
---|
| 116 | void goToNext() {
|
---|
| 117 | // Turn the robot
|
---|
| 118 | Robot.turn(-90);
|
---|
| 119 | Robot.motorsStop();
|
---|
| 120 | delay(1000);
|
---|
| 121 | }
|
---|