e373c5187a54169e1683dc0a9adb22751329b76b
[bachelor-thesis/roomba_tests.git] / main.cc
1 /**
2 * @file main.cc
3 * @date 27 Nov 2010
4 * Copyright © 2010 Roland Hieber
5 *
6 * Permission is hereby granted, free of charge, to any person obtaining
7 * copy of this software and associated documentation files (the "Software"),
8 * to deal in the Software without restriction, including without limitation
9 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
10 * and/or sell copies of the Software, and to permit persons to whom the
11 * Software is furnished to do so, subject to the following conditions:
12 *
13 * The above copyright notice and this permission notice shall be included in
14 * all copies or substantial portions of the Software.
15 *
16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
22 * THE SOFTWARE.
23 */
24
25 #include "svnrevision.h"
26 #include <iostream>
27 #include <stdint.h>
28 #include <external_interface/pc/pc_os_model.h>
29 #include <external_interface/pc/pc_com_uart.h>
30 #include <external_interface/pc/pc_timer.h>
31 #include <intermediate/robot/roomba/roomba.h>
32 #include <intermediate/robot/controlled_motion.h>
33 #include <QApplication>
34 #include <QInputDialog>
35
36 using namespace std;
37
38 #if QT_VERSION < 0x040500
39 #define getInt QInputDialog::getInteger
40 #else // QT_VERSION > 0x040500
41 #define getInt QInputDialog::getInt
42 #endif // QT_VERSION
43 // UART port on which we communicate with the Roomba
44 char uart[] = "/dev/ttyUSB0";
45
46 typedef wiselib::PCOsModel OsModel;
47 typedef wiselib::StandaloneMath Math;
48 typedef wiselib::PCComUartModel<OsModel, uart> RoombaUart;
49 typedef wiselib::RoombaModel<OsModel, RoombaUart> Roomba;
50 typedef wiselib::ControlledMotion<OsModel, Roomba> ControlledMotion;
51
52 /**
53 * Global stuff we need
54 */
55 OsModel::Os os;
56 OsModel::Timer::self_t timer;
57 Roomba roomba;
58 RoombaUart roomba_uart(os);
59 ControlledMotion ctrl_motion;
60 char * roomba_id;
61 char * ground_type;
62
63 /**
64 * Sensor data we need, filled in callback
65 */
66 struct SensorData {
67
68 SensorData() :
69 capacity(0), charge(0), charging(0), current(0), temperature(0),
70 voltage(0), diff_left_ticks(0), diff_right_ticks(0) {
71 }
72
73 uint16_t capacity, charge;
74 uint8_t charging;
75 int16_t current;
76 int8_t temperature;
77 uint16_t voltage;
78 /** raw encoder counts; i.e. overflown, not consecutive */
79 int raw_left_ticks, raw_right_ticks;
80 /** absolute encoder counts; i.e. not overflown, but consecutive */
81 int diff_left_ticks, diff_right_ticks;
82 } sensor_data;
83
84 /**
85 * Returns the difference between two unsigned short values. The calculated
86 * value is always smaller or equal to 0x8000.
87 * This is useful if you have an overflowing counter and you want to determine
88 * when you have to "wrap over" the value.
89 */
90 int nearestDiff(unsigned short last, unsigned short current) {
91 int d = current - last;
92 if(d < -0x8000) { // overflow in positive direction
93 d = (0x10000 - last + current);
94 }
95 if(d >= 0x8000) { // overflow in negative direction
96 d = -(0x10000 - current + last);
97 }
98 }
99
100 /**
101 * Callback that fills the sensor data when data is available
102 */
103 struct DataAvailable {
104 int latest_ticks_left_, latest_ticks_right_;
105
106 /**
107 * Initialisation. Calling this function is optional, but strongly encouraged
108 * if you want to get right results.
109 * @param init_ticks_left Initial ticks of left wheel used to calculate the
110 * difference
111 * @param init_ticks_left Initial ticks of right wheel used to calculate the
112 * difference
113 */
114 void init(int init_ticks_left, int init_ticks_right) {
115 latest_ticks_left_ = init_ticks_left;
116 latest_ticks_right_ = init_ticks_right;
117 }
118
119 /**
120 * Callback for Roomba<...>::register_state_callback()
121 * @param state Callback parameter that describes the state ot the data
122 */
123 void cb(int state) {
124 if(state != Roomba::DATA_AVAILABLE) {
125 return;
126 }
127 sensor_data.capacity = roomba().capacity;
128 sensor_data.charge = roomba().charge;
129 sensor_data.charging = roomba().charging;
130 sensor_data.current = roomba().current;
131 sensor_data.voltage = roomba().voltage;
132 sensor_data.raw_left_ticks = roomba().left_encoder_counts;
133 sensor_data.raw_right_ticks = roomba().right_encoder_counts;
134 sensor_data.diff_left_ticks += nearestDiff(latest_ticks_left_,
135 roomba().left_encoder_counts);
136 latest_ticks_left_ = roomba().left_encoder_counts;
137 sensor_data.diff_right_ticks += nearestDiff(latest_ticks_right_,
138 roomba().right_encoder_counts);
139 latest_ticks_right_ = roomba().right_encoder_counts;
140 }
141 } data_available;
142
143 /**
144 * return battery status as QString
145 */
146 QString chargeText() {
147 return QString("Battery: %1%\nPress Cancel to exit.\n\n").arg(int(float(
148 sensor_data.charge) / float(sensor_data.capacity) * 100.0));
149 }
150
151 /**
152 * return log text for global values
153 */
154 QString logText() {
155 return QString("svn=%1 roomba_id=%2 ground_type=%3 diff_left_ticks=%4 "
156 "diff_right_ticks=%5 raw_ticks_left=%6 raw_ticks_right=%7 batt_charge=%8 "
157 "batt_capacity=%9 batt_voltage=%10 batt_current=%11").arg(SVNREVISION).arg(
158 roomba_id).arg(ground_type).arg(sensor_data.diff_left_ticks).arg(
159 sensor_data.diff_right_ticks).arg(sensor_data.raw_left_ticks).arg(
160 sensor_data.raw_right_ticks).arg(sensor_data.charge).arg(
161 sensor_data.capacity).arg(sensor_data.voltage).arg(sensor_data.current);
162 }
163
164 /**
165 * drive iterations. logs values to stdout.
166 */
167 void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
168 int input_distance = 0, deviation_x = 0, deviation_y = 0, velocity = 100;
169 bool ok = false;
170
171 while(true) {
172
173 // new distance to drive
174 input_distance = getInt(0, "Input distance", chargeText()
175 + "Input new distance in mm:", input_distance,
176 numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
177 if(!ok) {
178 break;
179 }
180 // new velocity
181 velocity = getInt(0, "Input velocity", chargeText()
182 + "Input drive velocity in mm/sec:", velocity, -500, 500, 10, &ok);
183 if(!ok) {
184 break;
185 }
186 ctrl_motion.move_distance(input_distance, velocity);
187 roomba.wait_for_stop();
188
189 // measured deviation
190 deviation_x = getInt(0, "Input x deviation", chargeText()
191 + "Input travelled distance on x axis in mm:", deviation_x,
192 numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
193 if(!ok) {
194 break;
195 }
196 deviation_y = getInt(0, "Input y deviation", chargeText()
197 + "Input travelled distance on y axis in mm:", deviation_y,
198 numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
199 if(!ok) {
200 break;
201 }
202 cout << logText().toAscii().constData() << " move=straight input_distance="
203 << input_distance << " velocity=" << velocity << " internal_distance="
204 << roomba.distance() << " deviation_x=" << deviation_x << " deviation_y="
205 << deviation_y << endl;
206
207 // reset, because we only need the difference between two drive commands
208 sensor_data.diff_left_ticks = 0;
209 sensor_data.diff_right_ticks = 0;
210 }
211 }
212
213 /**
214 * turn iterations. logs values to stdout.
215 */
216 void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
217 int cur_angle = 0, turn_angle = 0, measured_angle = 0, velocity = 100;
218 bool ok = false;
219
220 // current angle
221 cur_angle = getInt(0, "Input current orientation", chargeText()
222 + "Input current orientation in degree:", cur_angle, 0, 359, 1, &ok);
223 if(!ok) {
224 return;
225 }
226
227 while(true) {
228 // new turn velocity
229 velocity = getInt(0, "Input velocity", chargeText()
230 + "Input turn velocity in mm/sec:", velocity, -500, 500, 10, &ok);
231 if(!ok) {
232 break;
233 }
234
235 // angle to turn about
236 turn_angle = getInt(0, "Input turn angle", chargeText()
237 + "Input angle in degree to turn about:", turn_angle,
238 numeric_limits<int>::min() + 360, numeric_limits<int>::max() - 360, 1,
239 &ok);
240 if(!ok) {
241 break;
242 }
243
244 ctrl_motion.turn_about(Math::degrees_to_radians(turn_angle), velocity);
245 roomba.wait_for_stop();
246
247 // new current angle
248 measured_angle = getInt(0, "Input measured angle", chargeText() + QString(
249 "Orientation should be %1 degree now.\n\n").arg((cur_angle + turn_angle)
250 % 360) + "Input measured angle in degree the Roomba has "
251 "turned:", turn_angle, 0, numeric_limits<int>::max(), 1, &ok);
252 if(!ok) {
253 break;
254 }
255
256 cout << logText().toAscii().constData() << " move=turn turn_angle="
257 << turn_angle << " measured_angle=" << measured_angle << " velocity="
258 << velocity << " internal_angle=" << roomba.angle() << endl;
259
260 // reset, because we only need the difference between two turns
261 sensor_data.diff_left_ticks = 0;
262 sensor_data.diff_right_ticks = 0;
263
264 // new orientation
265 cur_angle = (cur_angle + measured_angle) % 360;
266 }
267 }
268
269 /**
270 * main function
271 */
272 int main(int argc, char ** argv) {
273
274 if(argc < 4) {
275 cerr << "Usage: " << argv[0]
276 << " --turn|-t|--drive|-d roomba-id ground-type" << endl;
277 exit(-1);
278 }
279
280 // init stuff
281 QApplication app(argc, argv);
282
283 roomba_uart.set_baudrate(19200);
284 roomba_uart.enable_serial_comm();
285 roomba.init(roomba_uart, timer, Roomba::POSITION
286 | Roomba::BATTERY_AND_TEMPERATURE);
287
288 cerr << "Got roomba at " << roomba_uart.address() << endl;
289
290 roomba.reset_distance();
291 roomba.reset_angle();
292 ctrl_motion.init(roomba);
293
294 // we do not want the probably corrupted data from roomba(), instead we fill
295 // our own values when data is available
296 roomba.register_state_callback<DataAvailable, &DataAvailable::cb> (
297 &data_available);
298
299 // fill it once
300 roomba.notify_state_receivers(Roomba::DATA_AVAILABLE);
301 data_available.init(sensor_data.raw_left_ticks, sensor_data.raw_right_ticks);
302
303 // actual tests
304 roomba_id = argv[2];
305 ground_type = argv[3];
306 if(strcmp(argv[1], "--turn") == 0 || strcmp(argv[1], "-t") == 0) {
307 turn(roomba, ctrl_motion);
308 } else if(strcmp(argv[1], "--drive") == 0 || strcmp(argv[1], "-d") == 0) {
309 drive(roomba, ctrl_motion);
310 }
311 }
This page took 0.13664 seconds and 3 git commands to generate.