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