| 0d72813 by Trever Fischer at 2009-10-19 |
1 |
#include "DriveRobot.h" |
|
2 |
#include <libirobot/Robot.h> |
|
3 |
#include <libirobot/Drive.h> |
|
4 |
#include <libirobot/SerialControl.h> |
|
5 |
#include <libirobot/sensors/BumpAndDropSensor.h> |
| d171b40 by Trever Fischer at 2009-10-21 |
6 |
#include <libirobot/sensors/BooleanSensor.h> |
|
7 |
#include <libirobot/Song.h> |
| 3a2200f by Trever Fischer at 2009-10-23 |
8 |
#include <libirobot/sensors/IRSensor.h> |
|
9 |
#include <libirobot/sensors/ChargeSourceSensor.h> |
|
10 |
#include <libirobot/sensors/UnsignedValueSensor.h> |
|
11 |
#include <libirobot/sensors/ValueSensor.h> |
| 27bfcbc by Trever Fischer at 2010-01-26 |
12 |
#include <libirobot/Debug.h> |
| 0d72813 by Trever Fischer at 2009-10-19 |
13 |
|
| 0265068 by Trever Fischer at 2009-10-28 |
14 |
#include <math.h> |
|
15 |
|
| 6939b20 by Trever Fischer at 2009-10-29 |
16 |
#ifndef __AVR__ |
|
17 |
#include <iostream> |
|
18 |
#endif |
|
19 |
|
| 0d72813 by Trever Fischer at 2009-10-19 |
20 |
using namespace iRobot; |
|
21 |
|
| d171b40 by Trever Fischer at 2009-10-21 |
22 |
#define QUARTER_NOTE 16 |
|
23 |
|
| a9cac15 by Trever Fischer at 2009-10-19 |
24 |
DriveRobot::DriveRobot ( const char ttyDevice[] ) |
| 426cf0e by Trever Fischer at 2009-10-20 |
25 |
: Robot ( ttyDevice ), |
| 3a2200f by Trever Fischer at 2009-10-23 |
26 |
m_speed(0), |
|
27 |
m_lastDir(Drive::Right), |
|
28 |
m_tryingToDock(false), |
|
29 |
m_dockAttempted(false), |
|
30 |
m_dockTimeout(0) |
| 0d72813 by Trever Fischer at 2009-10-19 |
31 |
{ |
| 426cf0e by Trever Fischer at 2009-10-20 |
32 |
setMode ( Full ); |
| ca49b9d by Trever Fischer at 2009-10-27 |
33 |
setAutoUpdate( Sensor::All, true ); |
| 3a2200f by Trever Fischer at 2009-10-23 |
34 |
|
|
35 |
Song cliffMusic(2); |
|
36 |
Song::Note cliffnotes[] = { |
|
37 |
{ 60, QUARTER_NOTE }, |
|
38 |
{ 57, QUARTER_NOTE }, |
|
39 |
}; |
|
40 |
for(int i = 0;i<2;i++) { |
|
41 |
cliffMusic.setNotePitch(i, cliffnotes[i].pitch); |
|
42 |
cliffMusic.setNoteLength(i, cliffnotes[i].length); |
|
43 |
} |
|
44 |
setSong(1, cliffMusic); |
| 0d72813 by Trever Fischer at 2009-10-19 |
45 |
} |
|
46 |
|
|
47 |
void |
| 3a2200f by Trever Fischer at 2009-10-23 |
48 |
DriveRobot::avoid() |
|
49 |
{ |
|
50 |
Drive* d = getDrive(); |
|
51 |
d->move(0); |
| 797ed99 by Trever Fischer at 2009-10-29 |
52 |
d->move(-(int)Drive::Medium ); |
|
53 |
m_dockTimeout+=d->waitForDistance(100); |
| 3a2200f by Trever Fischer at 2009-10-23 |
54 |
d->move ( Drive::Medium, m_lastDir ); |
| 797ed99 by Trever Fischer at 2009-10-29 |
55 |
d->waitForRotation(15); |
| 3a2200f by Trever Fischer at 2009-10-23 |
56 |
} |
|
57 |
|
|
58 |
bool |
|
59 |
DriveRobot::reachedCliff() |
|
60 |
{ |
|
61 |
BooleanSensor* cliffLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffLeft ) ); |
|
62 |
BooleanSensor* cliffFrontLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontLeft ) ); |
|
63 |
BooleanSensor* cliffFrontRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontRight ) ); |
|
64 |
BooleanSensor* cliffRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffRight ) ); |
|
65 |
return cliffLeft->value() || cliffFrontRight->value() || cliffFrontLeft->value() || cliffRight->value(); |
|
66 |
} |
|
67 |
|
| 797ed99 by Trever Fischer at 2009-10-29 |
68 |
DriveRobot::Side |
|
69 |
DriveRobot::cliffSide() |
|
70 |
{ |
|
71 |
BooleanSensor* cliffLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffLeft ) ); |
|
72 |
BooleanSensor* cliffFrontLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontLeft ) ); |
|
73 |
BooleanSensor* cliffFrontRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontRight ) ); |
|
74 |
BooleanSensor* cliffRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffRight ) ); |
|
75 |
if (cliffFrontLeft->value() && cliffFrontRight->value()) |
|
76 |
return Center; |
|
77 |
if (cliffLeft->value() || cliffFrontLeft->value()) |
|
78 |
return Left; |
|
79 |
if (cliffRight->value() || cliffFrontRight->value()) |
|
80 |
return Right; |
|
81 |
} |
|
82 |
|
| 3a2200f by Trever Fischer at 2009-10-23 |
83 |
bool |
|
84 |
DriveRobot::fellOffCliff() |
| 0d72813 by Trever Fischer at 2009-10-19 |
85 |
{ |
| a9cac15 by Trever Fischer at 2009-10-19 |
86 |
BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) ); |
| 3a2200f by Trever Fischer at 2009-10-23 |
87 |
return s->flags() & BumpAndDropSensor::DropAny; |
|
88 |
} |
|
89 |
|
|
90 |
void |
|
91 |
DriveRobot::dock() |
|
92 |
{ |
| 0d72813 by Trever Fischer at 2009-10-19 |
93 |
Drive* d = getDrive(); |
| 3a2200f by Trever Fischer at 2009-10-23 |
94 |
IRSensor* ir = static_cast<IRSensor*> ( getSensor( Sensor::IR ) ); |
|
95 |
unsigned char c = ir->value(); |
|
96 |
ChargeSourceSensor* sources = static_cast<ChargeSourceSensor*> ( getSensor( Sensor::ChargeSources ) ); |
|
97 |
ValueSensor* distance = static_cast<ValueSensor*> ( getSensor( Sensor::Distance ) ); |
|
98 |
m_dockTimeout+=distance->value(); |
|
99 |
if (m_dockTimeout > 2000) { |
|
100 |
m_dockTimeout = 0; |
|
101 |
m_tryingToDock = false; |
|
102 |
m_dockAttempted = false; |
|
103 |
} else if (sources->sources() & ChargeSourceSensor::Base) { |
|
104 |
d->move(0); |
|
105 |
m_tryingToDock = false; |
|
106 |
return; |
|
107 |
} else if (m_tryingToDock) { |
|
108 |
if (m_dockAttempted) { |
|
109 |
d->move(-Drive::Medium); |
| 797ed99 by Trever Fischer at 2009-10-29 |
110 |
m_dockTimeout+=d->waitForDistance(300); |
| 3a2200f by Trever Fischer at 2009-10-23 |
111 |
if (m_lastDir == Drive::Right) |
|
112 |
d->move(Drive::Fast, Drive::Left); |
|
113 |
else |
|
114 |
d->move(Drive::Fast, Drive::Right); |
| 797ed99 by Trever Fischer at 2009-10-29 |
115 |
d->waitForRotation(180); |
|
116 |
d->move(Drive::Medium); |
|
117 |
m_dockTimeout+=d->waitForDistance(100); |
|
118 |
m_dockAttempted = false; |
| 3a2200f by Trever Fischer at 2009-10-23 |
119 |
} else if ((c == (unsigned char)IRSensor::RedAndGreenBuoysAndForceField || c == (unsigned char)IRSensor::ForceField) && bumped()) { |
|
120 |
d->move(-Drive::Slow); |
| 797ed99 by Trever Fischer at 2009-10-29 |
121 |
m_dockTimeout+=d->waitForDistance(15); |
| 3a2200f by Trever Fischer at 2009-10-23 |
122 |
d->move(0); |
|
123 |
delay(1000000); |
|
124 |
m_dockAttempted = true; |
|
125 |
} else if (c == (unsigned char)IRSensor::RedAndGreenBuoys || c == (unsigned char)IRSensor::RedAndGreenBuoysAndForceField) { |
|
126 |
d->move(170); |
|
127 |
} else if (c == (unsigned char)IRSensor::RedBuoy || c == (unsigned char)IRSensor::RedBuoyAndForceField) { |
|
128 |
d->move(Drive::Medium, 100); |
|
129 |
m_lastDir = Drive::Left; |
|
130 |
} else if (c == (unsigned char)IRSensor::GreenBuoy || c == (unsigned char)IRSensor::GreenBuoyAndForceField) { |
|
131 |
d->move(Drive::Medium, -100); |
|
132 |
m_lastDir = Drive::Right; |
|
133 |
} else { |
|
134 |
if (m_lastDir == Drive::Right) |
|
135 |
d->move(Drive::Medium, 200); |
|
136 |
else |
|
137 |
d->move(Drive::Medium, -200); |
|
138 |
} |
|
139 |
} else { |
|
140 |
if (c == (unsigned char) IRSensor::ForceField || c == (unsigned char)IRSensor::RedBuoy || c == (unsigned char)IRSensor::GreenBuoy || c == (unsigned char)IRSensor::RedAndGreenBuoys) { |
|
141 |
m_tryingToDock = true; |
|
142 |
} else { |
|
143 |
d->move(Drive::Medium); |
|
144 |
} |
|
145 |
} |
|
146 |
} |
|
147 |
|
|
148 |
bool |
|
149 |
DriveRobot::bumped() |
|
150 |
{ |
|
151 |
BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) ); |
| 797ed99 by Trever Fischer at 2009-10-29 |
152 |
BooleanSensor* wall = static_cast<BooleanSensor*> ( getSensor ( Sensor::VirtualWall ) ); |
|
153 |
return s->flags() & BumpAndDropSensor::BumpAny || wall->value(); |
| 3a2200f by Trever Fischer at 2009-10-23 |
154 |
} |
|
155 |
|
|
156 |
bool |
|
157 |
DriveRobot::bumpedLeft() |
|
158 |
{ |
|
159 |
BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) ); |
|
160 |
return s->flags() & BumpAndDropSensor::BumpLeft; |
|
161 |
} |
|
162 |
|
|
163 |
bool |
|
164 |
DriveRobot::bumpedRight() |
|
165 |
{ |
|
166 |
BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) ); |
|
167 |
return s->flags() & BumpAndDropSensor::BumpRight; |
|
168 |
} |
|
169 |
|
|
170 |
void |
|
171 |
DriveRobot::loop() |
|
172 |
{ |
| 09c9222 by Trever Fischer at 2010-01-19 |
173 |
m_powerColor++; |
|
174 |
uint8_t leds; |
|
175 |
if (m_powerColor > 128) |
|
176 |
leds = Play; |
|
177 |
else |
|
178 |
leds = Advance; |
|
179 |
setLEDs(leds, m_powerColor, ((float)m_powerColor/255)*255); |
| d171b40 by Trever Fischer at 2009-10-21 |
180 |
BooleanSensor* song = static_cast<BooleanSensor*> ( getSensor( Sensor::SongPlaying ) ); |
| 3a2200f by Trever Fischer at 2009-10-23 |
181 |
if (song->value()) |
|
182 |
return; |
|
183 |
ChargeSourceSensor* sources = static_cast<ChargeSourceSensor*> ( getSensor( Sensor::ChargeSources ) ); |
|
184 |
if (sources->sources() & ChargeSourceSensor::Base) { |
|
185 |
UnsignedValueSensor* battery = static_cast<UnsignedValueSensor*> ( getSensor( Sensor::BatteryCharge ) ); |
|
186 |
UnsignedValueSensor* capacity = static_cast<UnsignedValueSensor*> ( getSensor( Sensor::BatteryCapacity ) ); |
| 27bfcbc by Trever Fischer at 2010-01-26 |
187 |
IROBOT_DEBUG("Battery: " << battery->value() << "/" << capacity->value() << " " << (int)((battery->value()/(float)capacity->value())*100) << "%"); |
| 3a2200f by Trever Fischer at 2009-10-23 |
188 |
return; |
|
189 |
} |
|
190 |
dock(); |
|
191 |
Drive* d = getDrive(); |
|
192 |
if (fellOffCliff()) { |
|
193 |
d->move(0); |
|
194 |
if (!song->value()) { |
|
195 |
playSong(1); |
|
196 |
delay(1000000); |
|
197 |
} |
|
198 |
} else if (reachedCliff()) { |
| 797ed99 by Trever Fischer at 2009-10-29 |
199 |
if (cliffSide() == Left) |
|
200 |
m_lastDir = Drive::Left; |
|
201 |
if (cliffSide() == Right) |
|
202 |
m_lastDir = Drive::Right; |
| 3a2200f by Trever Fischer at 2009-10-23 |
203 |
avoid(); |
|
204 |
} else if ( bumped() ) { |
|
205 |
if ( bumpedLeft() && bumpedRight() ) { |
|
206 |
avoid(); |
|
207 |
} else if ( bumpedLeft() ) { |
|
208 |
d->move ( Drive::Medium, Drive::Right); |
|
209 |
m_lastDir = Drive::Right; |
| 797ed99 by Trever Fischer at 2009-10-29 |
210 |
delay ( 3000 ); |
| d171b40 by Trever Fischer at 2009-10-21 |
211 |
} else { |
| 3a2200f by Trever Fischer at 2009-10-23 |
212 |
d->move ( Drive::Medium, Drive::Left ); |
|
213 |
m_lastDir = Drive::Left; |
| 797ed99 by Trever Fischer at 2009-10-29 |
214 |
BooleanSensor* wall = static_cast<BooleanSensor*> ( getSensor( Sensor::Wall ) ); |
|
215 |
do { |
|
216 |
updateSensor( Sensor::Wall ); |
|
217 |
} while (wall->value()); |
| d171b40 by Trever Fischer at 2009-10-21 |
218 |
} |
| 3a2200f by Trever Fischer at 2009-10-23 |
219 |
m_speed = 30; |
|
220 |
} else if (!m_tryingToDock) { |
|
221 |
if (m_speed < Drive::Medium) |
|
222 |
m_speed+=20; |
|
223 |
d->move ( m_speed ); |
| d171b40 by Trever Fischer at 2009-10-21 |
224 |
} |
| 0d72813 by Trever Fischer at 2009-10-19 |
225 |
} |
|
226 |
|
|
227 |
void |
|
228 |
DriveRobot::disconnected() |
|
229 |
{ |
| 27bfcbc by Trever Fischer at 2010-01-26 |
230 |
IROBOT_DEBUG("Disconnected."); |
| a9cac15 by Trever Fischer at 2009-10-19 |
231 |
exit ( 0 ); |
| 0d72813 by Trever Fischer at 2009-10-19 |
232 |
} |
