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  * Global stuff we need
  58  */
  59 OsModel::Os os;
  60 OsModel::Timer::self_t timer;
  61 Roomba roomba;
  62 RoombaUart roomba_uart(os);
  63 ControlledMotion ctrl_motion;
  64
  65 /**
  66  * Sensor data we need, filled in callback
  67  */
  68 struct SensorData {
  69   uint16_t capacity, charge;
  70   uint8_t charging;
  71   int16_t current;
  72   int8_t temperature;
  73   uint16_t voltage;
  74   int16_t left_encoder_counts, right_encoder_counts;
  75 } sensor_data;
  76
  77 /**
  78  * Callback that fills the sensor data when data is available
  79  */
  80 struct DataAvailable {
  81   void cb(int state) {
  82     if(state != Roomba::DATA_AVAILABLE) {
  83       return;
  84     }
  85     sensor_data.capacity = roomba().capacity;
  86     sensor_data.charge = roomba().charge;
  87     sensor_data.charging = roomba().charging;
  88     sensor_data.current = roomba().current;
  89     sensor_data.voltage = roomba().voltage;
  90     sensor_data.left_encoder_counts = roomba().left_encoder_counts;
  91     sensor_data.right_encoder_counts = roomba().right_encoder_counts;
  92   }
  93 } data_available;
  94
  95 /**
  96  * return battery status as QString
  97  */
  98 QString chargeText(Roomba& roomba) {
  99   return QString("Battery: %1%\nPress Cancel to exit.\n\n").arg(int(float(
 100     sensor_data.charge) / float(sensor_data.capacity) * 100.0));
 101 }
 102
 103 /**
 104  * drive iterations. logs values to stdout.
 105  */
 106 void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
 107   int input_distance = 0, deviation_x = 0, deviation_y = 0, velocity = 100;
 108   bool ok = false;
 109
 110   while(true) {
 111
 112     // new distance to drive
 113     input_distance = getInt(0, "Input distance", chargeText(roomba)
 114       + "Input new distance in mm:", input_distance,
 115       numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
 116     if(ok) {
 117       // new turn velocity
 118       velocity = getInt(0, "Input velocity", chargeText(roomba)
 119         + "Input drive velocity in mm/sec:", velocity, -500, 500, 10, &ok);
 120       if(ok) {
 121         ctrl_motion.move_distance(input_distance, velocity);
 122         roomba.wait_for_stop();
 123       } else {
 124         break;
 125       }
 126     } else {
 127       break;
 128     }
 129
 130     // measured deviation
 131     deviation_x = getInt(0, "Input x deviation", chargeText(roomba)
 132       + "Input travelled distance on x axis in mm:", deviation_x,
 133       numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
 134     if(ok) {
 135       deviation_y = getInt(0, "Input y deviation", chargeText(roomba)
 136         + "Input travelled distance on y axis in mm:", deviation_y,
 137         numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
 138       if(ok) {
 139         cout << "input_distance=" << input_distance << " velocity=" << velocity
 140           << " internal_distance=" << roomba.distance() << " deviation_x="
 141           << deviation_x << " deviation_y=" << deviation_y
 142           << " encoder_ticks_left=" << sensor_data.left_encoder_counts
 143           << " encoder_ticks_right=" << sensor_data.right_encoder_counts
 144           << " batt_charge=" << sensor_data.charge << " batt_capacity="
 145           << sensor_data.capacity << " batt_voltage=" << sensor_data.voltage
 146           << " batt_current=" << sensor_data.current << endl;
 147       } else {
 148         break;
 149       }
 150     } else {
 151       break;
 152     }
 153   }
 154 }
 155
 156 /**
 157  * turn iterations. logs values to stdout.
 158  */
 159 void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
 160   int cur_angle = 0, turn_angle = 0, measured_angle = 0, velocity = 100;
 161   bool ok = false;
 162
 163   // current angle
 164   cur_angle = getInt(0, "Input current orientation", chargeText(roomba)
 165     + "Input current orientation in degree:", cur_angle, 0, 359, 1, &ok);
 166   if(!ok) {
 167     return;
 168   }
 169
 170   while(true) {
 171     // new turn velocity
 172     velocity = getInt(0, "Input velocity", chargeText(roomba)
 173       + "Input turn velocity in mm/sec:", velocity, -500, 500, 10, &ok);
 174     if(!ok) {
 175       break;
 176     }
 177
 178     // angle to turn about
 179     turn_angle = getInt(0, "Input turn angle", chargeText(roomba)
 180       + "Input angle in degree to turn about:", turn_angle,
 181       numeric_limits<int>::min() + 360, numeric_limits<int>::max() - 360, 1,
 182       &ok);
 183     if(!ok) {
 184       break;
 185     }
 186
 187     ctrl_motion.turn_about(Math::degrees_to_radians(turn_angle), velocity);
 188     roomba.wait_for_stop();
 189
 190     // new current angle
 191     measured_angle = getInt(0, "Input measured angle", chargeText(roomba)
 192       + QString("Orientation should be %1 degree now.\n\n").arg((cur_angle
 193       + turn_angle) % 360) + "Input measured angle in degree the Roomba has "
 194       "turned:", turn_angle, 0, numeric_limits<int>::max(), 1, &ok);
 195     if(!ok) {
 196       break;
 197     }
 198
 199     cout << "turn_angle=" << turn_angle << " measured_angle=" << measured_angle
 200       << " velocity=" << velocity << " internal_angle=" << roomba.angle()
 201       << " encoder_ticks_left=" << sensor_data.left_encoder_counts
 202       << " encoder_ticks_right=" << sensor_data.right_encoder_counts
 203       << " batt_charge=" << sensor_data.charge << " batt_capacity="
 204       << sensor_data.capacity << " batt_voltage=" << sensor_data.voltage
 205       << " batt_current=" << sensor_data.current << endl;
 206
 207     // new orientation
 208     cur_angle = (cur_angle + measured_angle) % 360;
 209
 210   }
 211 }
 212
 213 /**
 214  * main function
 215  */
 216 int main(int argc, char ** argv) {
 217
 218   if(argc < 2) {
 219     cerr << "Usage: " << argv[0] << " --turn|-t|--drive|-d" << endl;
 220     exit(-1);
 221   }
 222
 223   // init stuff
 224   QApplication app(argc, argv);
 225
 226   roomba_uart.set_baudrate(19200);
 227   roomba_uart.enable_serial_comm();
 228   roomba.init(roomba_uart, timer, Roomba::POSITION
 229     | Roomba::BATTERY_AND_TEMPERATURE);
 230
 231   cerr << "Got roomba at " << roomba_uart.address() << endl;
 232
 233   roomba.reset_distance();
 234   roomba.reset_angle();
 235   ctrl_motion.init(roomba);
 236
 237   // we do not want the probably corrupted data from roomba(), instead we fill
 238   // our own values when data is available
 239   roomba.register_state_callback<DataAvailable, &DataAvailable::cb> (
 240     &data_available);
 241
 242   // fill it once
 243   roomba.notify_state_receivers(Roomba::DATA_AVAILABLE);
 244
 245   // actual tests
 246   if(strcmp(argv[1], "--turn") == 0 || strcmp(argv[1], "-t") == 0) {
 247     turn(roomba, ctrl_motion);
 248   } else if(strcmp(argv[1], "--drive") == 0 || strcmp(argv[1], "-d") == 0) {
 249     drive(roomba, ctrl_motion);
 250   }
 251 }