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 }