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 | }
|
---|