fix maximum input limit for measured angle
[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 "main.h"
26 #include <iostream>
27 #include <stdint.h>
28 //#include <unistd.h>
29 //#include "util/standalone_math.h"
30 //#include "util/delegates/delegate.hpp"
31 #include <external_interface/pc/pc_os_model.h>
32 #include <external_interface/pc/pc_com_uart.h>
33 #include <external_interface/pc/pc_timer.h>
34 #include <intermediate/robot/roomba/roomba.h>
35 #include <intermediate/robot/controlled_motion.h>
36 #include <QApplication>
37 #include <QInputDialog>
38
39 #if QT_VERSION < 0x040500
40 #define getInt QInputDialog::getInteger
41 #else // QT_VERSION > 0x040500
42 #define getInt QInputDialog::getInt
43 #endif // QT_VERSION
44
45 using namespace std;
46
47 // UART port on which we communicate with the Roomba
48 char uart[] = "/dev/ttyUSB0";
49
50 typedef wiselib::PCOsModel OsModel;
51 typedef wiselib::StandaloneMath Math;
52 typedef wiselib::PCComUartModel<OsModel, uart> RoombaUart;
53 typedef wiselib::RoombaModel<OsModel, RoombaUart> Roomba;
54 typedef wiselib::ControlledMotion<OsModel, Roomba> ControlledMotion;
55
56 /**
57 * return battery status as QString
58 */
59 QString chargeText(Roomba& roomba) {
60 return QString("Battery: %1%\nPress Cancel to exit.\n\n").arg(int(float(
61 roomba().charge) / float(roomba().capacity) * 100.0));
62 }
63
64 /**
65 * drive iterations. logs values to stdout.
66 */
67 void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
68 int input_distance = 0, deviation_x = 0, deviation_y = 0, velocity = 100;
69 bool ok = false;
70
71 while(true) {
72
73 // new distance to drive
74 input_distance = getInt(0, "Input distance", chargeText(roomba)
75 + "Input new distance in mm:", input_distance,
76 numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
77 if(ok) {
78 // new turn velocity
79 velocity = getInt(0, "Input velocity", chargeText(roomba)
80 + "Input drive velocity in mm/sec:", velocity, -500, 500, 10, &ok);
81 if(ok) {
82 ctrl_motion.move_distance(input_distance, velocity);
83 roomba.wait_for_stop();
84 } else {
85 break;
86 }
87 } else {
88 break;
89 }
90
91 // measured deviation
92 deviation_x = getInt(0, "Input x deviation", chargeText(roomba)
93 + "Input travelled distance on x axis in mm:", deviation_x,
94 numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
95 if(ok) {
96 deviation_y = getInt(0, "Input y deviation", chargeText(roomba)
97 + "Input travelled distance on y axis in mm:", deviation_y,
98 numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
99 if(ok) {
100 cout << "input_distance=" << input_distance << " velocity=" << velocity
101 << " internal_distance=" << roomba.distance() << " deviation_x="
102 << deviation_x << " deviation_y=" << deviation_y
103 << " encoder_ticks_left=" << roomba().left_encoder_counts
104 << " encoder_ticks_right=" << roomba().right_encoder_counts
105 << " batt_charge=" << roomba().charge << " batt_capacity="
106 << roomba().capacity << " batt_voltage=" << roomba().voltage
107 << " batt_current=" << roomba().current << endl;
108 } else {
109 break;
110 }
111 } else {
112 break;
113 }
114 }
115 }
116
117 /**
118 * turn iterations. logs values to stdout.
119 */
120 void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
121 int cur_angle = 0, turn_angle = 0, measured_angle = 0, velocity = 100;
122 bool ok = false;
123
124 // current angle
125 cur_angle = getInt(0, "Input current orientation", chargeText(roomba)
126 + "Input current orientation in degree:", cur_angle, 0, 359, 1, &ok);
127 if(!ok) {
128 return;
129 }
130
131 while(true) {
132 // new turn velocity
133 velocity = getInt(0, "Input velocity", chargeText(roomba)
134 + "Input turn velocity in mm/sec:", velocity, -500, 500, 10, &ok);
135 if(!ok) {
136 break;
137 }
138
139 // angle to turn about
140 turn_angle = getInt(0, "Input turn angle", chargeText(roomba)
141 + "Input angle in degree to turn about:", turn_angle,
142 numeric_limits<int>::min() + 360, numeric_limits<int>::max() - 360, 1,
143 &ok);
144 if(!ok) {
145 break;
146 }
147
148 ctrl_motion.turn_about(Math::degrees_to_radians(turn_angle), velocity);
149 roomba.wait_for_stop();
150
151 // new current angle
152 measured_angle = getInt(0, "Input measured angle", chargeText(roomba)
153 + QString("Orientation should be %1 degree now.\n\n").arg((cur_angle
154 + turn_angle) % 360) + "Input measured angle in degree the Roomba has "
155 "turned:", turn_angle, 0, numeric_limits<int>::max(), 1, &ok);
156 if(!ok) {
157 break;
158 }
159
160 cout << "turn_angle=" << turn_angle << " measured_angle=" << measured_angle
161 << " velocity=" << velocity << " internal_angle=" << roomba.angle()
162 << " encoder_ticks_left=" << roomba().left_encoder_counts
163 << " encoder_ticks_right=" << roomba().right_encoder_counts
164 << " batt_charge=" << roomba().charge << " batt_capacity="
165 << roomba().capacity << " batt_voltage=" << roomba().voltage
166 << " batt_current=" << roomba().current << endl;
167
168 // new orientation
169 cur_angle = (cur_angle + measured_angle) % 360;
170
171 }
172 }
173
174 /**
175 * main function
176 */
177 int main(int argc, char ** argv) {
178
179 if(argc < 2) {
180 cerr << "Usage: " << argv[0] << " --turn|-t|--drive|-d" << endl;
181 exit(-1);
182 }
183
184 OsModel::Os os;
185 OsModel::Timer::self_t timer;
186 Roomba roomba;
187 RoombaUart roomba_uart(os);
188 ControlledMotion ctrl_motion;
189
190 // init stuff
191 QApplication app(argc, argv);
192
193 roomba_uart.set_baudrate(19200);
194 roomba_uart.enable_serial_comm();
195 roomba.init(roomba_uart, timer, Roomba::POSITION
196 | Roomba::BATTERY_AND_TEMPERATURE);
197
198 cout << "Got roomba at " << roomba_uart.address() << endl;
199
200 roomba.reset_distance();
201 roomba.reset_angle();
202 ctrl_motion.init(roomba);
203
204 // actual tests
205 if(strcmp(argv[1], "--turn") == 0 || strcmp(argv[1], "-t") == 0) {
206 turn(roomba, ctrl_motion);
207 } else if(strcmp(argv[1], "--drive") == 0 || strcmp(argv[1], "-d") == 0) {
208 drive(roomba, ctrl_motion);
209 }
210 }
This page took 0.136819 seconds and 5 git commands to generate.