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 }