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 char battery_string[] = "Battery: %1%\nPress Cancel to exit.\n\n";
  50
  51 typedef wiselib::PCOsModel OsModel;
  52 typedef wiselib::StandaloneMath Math;
  53 typedef wiselib::PCComUartModel<OsModel, uart> RoombaUart;
  54 typedef wiselib::RoombaModel<OsModel, RoombaUart> Roomba;
  55 typedef wiselib::ControlledMotion<OsModel, Roomba> ControlledMotion;
  56
  57 /**
  58  * drive iterations. logs values to stdout.
  59  */
  60 void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
  61   int input_distance = 0, deviation_x = 0, deviation_y = 0, velocity = 100;
  62   bool ok = false;
  63
  64   while(true) {
  65     QString chargeText = QString(battery_string).arg(int(float(roomba().charge)
  66       / float(roomba().capacity) * 100.0));
  67
  68     // new distance to drive
  69     input_distance = getInt(0, "Input distance", chargeText
  70       + "Input new distance in mm:", input_distance,
  71       numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
  72     if(ok) {
  73       // new turn velocity
  74       velocity = getInt(0, "Input velocity", chargeText
  75         + "Input drive velocity in mm/sec:", velocity, -500, 500, 10, &ok);
  76       if(ok) {
  77         ctrl_motion.move_distance(input_distance, velocity);
  78         roomba.wait_for_stop();
  79       } else {
  80         break;
  81       }
  82     } else {
  83       break;
  84     }
  85
  86     // measured deviation
  87     deviation_x = getInt(0, "Input x deviation", chargeText
  88       + "Input travelled distance on x axis in mm:", deviation_x,
  89       numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
  90     if(ok) {
  91       deviation_y = getInt(0, "Input y deviation", chargeText
  92         + "Input travelled distance on y axis in mm:", deviation_y,
  93         numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
  94       if(ok) {
  95         cout << "input_distance=" << input_distance << " velocity=" << velocity
  96           << " internal_distance=" << roomba.distance() << " deviation_x="
  97           << deviation_x << " deviation_y=" << deviation_y
  98           << " encoder_ticks_left=" << roomba().left_encoder_counts
  99           << " encoder_ticks_right=" << roomba().right_encoder_counts
 100           << " batt_charge=" << roomba().charge << " batt_capacity="
 101           << roomba().capacity << " batt_voltage=" << roomba().voltage
 102           << " batt_current=" << roomba().current << endl;
 103       } else {
 104         break;
 105       }
 106     } else {
 107       break;
 108     }
 109   }
 110 }
 111
 112 /**
 113  * turn iterations. logs values to stdout.
 114  */
 115 void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
 116   int input_angle = 0, measured_angle = 0, velocity = 100;
 117   bool ok = false;
 118
 119   while(true) {
 120     QString chargeText = QString(battery_string).arg(int(float(roomba().charge)
 121       / float(roomba().capacity) * 100.0));
 122
 123     // new angle to turn about
 124     input_angle = getInt(0, "Input angle", chargeText
 125       + "Input new angle in degree to turn about:", input_angle,
 126       numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
 127     if(ok) {
 128       // new turn velocity
 129       velocity = getInt(0, "Input velocity", chargeText
 130         + "Input turn velocity in mm/sec:", velocity, -500, 500, 10, &ok);
 131       if(ok) {
 132         ctrl_motion.turn_about(Math::degrees_to_radians(input_angle), velocity);
 133         roomba.wait_for_stop();
 134       } else {
 135         break;
 136       }
 137     } else {
 138       break;
 139     }
 140
 141     // measured angle
 142     measured_angle = getInt(0, "Input measured angle", chargeText
 143       + "Input measured angle in degree:", input_angle,
 144       numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
 145     if(ok) {
 146       cout << "input_angle=" << input_angle << " velocity=" << velocity
 147         << " measured_angle=" << measured_angle << " internal_angle="
 148         << roomba.angle() << " encoder_ticks_left="
 149         << roomba().left_encoder_counts << " encoder_ticks_right="
 150         << roomba().right_encoder_counts << " batt_charge=" << roomba().charge
 151         << " batt_capacity=" << roomba().capacity << " batt_voltage="
 152         << roomba().voltage << " batt_current=" << roomba().current << endl;
 153     } else {
 154       break;
 155     }
 156   }
 157 }
 158
 159 /**
 160  * main function
 161  */
 162 int main(int argc, char ** argv) {
 163
 164   if(argc < 2) {
 165     cerr << "Usage: " << argv[0] << " --turn|-t|--drive|-d" << endl;
 166     exit(-1);
 167   }
 168
 169   OsModel::Os os;
 170   OsModel::Timer::self_t timer;
 171   Roomba roomba;
 172   RoombaUart roomba_uart(os);
 173   ControlledMotion ctrl_motion;
 174
 175   // init stuff
 176   QApplication app(argc, argv);
 177
 178   roomba_uart.set_baudrate(19200);
 179   roomba_uart.enable_serial_comm();
 180   roomba.init(roomba_uart, timer, Roomba::POSITION
 181     | Roomba::BATTERY_AND_TEMPERATURE);
 182
 183   cout << "Got roomba at " << roomba_uart.address() << endl;
 184
 185   roomba.reset_distance();
 186   roomba.reset_angle();
 187   ctrl_motion.init(roomba);
 188
 189   // actual tests
 190   if(strcmp(argv[1], "--turn") == 0 || strcmp(argv[1], "-t") == 0) {
 191     turn(roomba, ctrl_motion);
 192   } else if(strcmp(argv[1], "--drive") == 0 || strcmp(argv[1], "-d") == 0) {
 193     drive(roomba, ctrl_motion);
 194   }
 195 }