simple Roomba test suite with Qt dialogs

git-svn-id: https://svn.itm.uni-luebeck.de/wisebed/wiselib/trunk/pc_apps/roomba_tests@3642 f8795833-4959-0410-8ae9-8bcb0cfab428

diff --git a/Makefile b/Makefile
index 6818a48..ae4cfeb 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,6 @@
 export TARGET=roomba_test
 export SOURCES=main.cc
 export TARGET=roomba_test
 export SOURCES=main.cc

+export CXXFLAGS=-I/usr/include/qt4 -I/usr/include/qt4/QtCore \
+       -I/usr/include/qt4/QtGui -lQtGui 

 
 include ../Makefile.base
 
 include ../Makefile.base

diff --git a/main.cc b/main.cc
new file mode 100644 (file)
index 0000000..1a181fe
--- /dev/null
+++ b/main.cc
@@ -0,0 +1,189 @@
+/**
+ * @file main.cc
+ * @date 27 Nov 2010
+ * Copyright © 2010 Roland Hieber
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+//#include "main.h"
+#include <iostream>
+#include <stdint.h>
+//#include <unistd.h>
+//#include "util/standalone_math.h"
+//#include "util/delegates/delegate.hpp"
+#include <external_interface/pc/pc_os_model.h>
+#include <external_interface/pc/pc_com_uart.h>
+#include <external_interface/pc/pc_timer.h>
+#include <intermediate/robot/roomba/roomba.h>
+#include <intermediate/robot/controlled_motion.h>
+#include <QApplication>
+#include <QInputDialog>
+
+using namespace std;
+
+// UART port on which we communicate with the Roomba
+char uart[] = "/dev/ttyUSB0";
+char battery_string[] = "Battery: %1%\nPress Cancel to exit.\n\n";
+
+typedef wiselib::PCOsModel OsModel;
+typedef wiselib::StandaloneMath Math;
+typedef wiselib::PCComUartModel<OsModel, uart> RoombaUart;
+typedef wiselib::RoombaModel<OsModel, RoombaUart> Roomba;
+typedef wiselib::ControlledMotion<OsModel, Roomba> ControlledMotion;
+
+/**
+ * drive iterations. logs values to stdout.
+ */
+void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
+  int input_distance = 0, deviation_x = 0, deviation_y = 0, velocity = 100;
+  bool ok = false;
+
+  while(true) {
+    QString chargeText = QString(battery_string).arg(int(float(roomba().charge)
+      / float(roomba().capacity) * 100.0));
+
+    // new distance to drive
+    input_distance = QInputDialog::getInt(0, "Input distance", chargeText
+      + "Input new distance in mm:", input_distance,
+      numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
+    if(ok) {
+      // new turn velocity
+      velocity = QInputDialog::getInt(0, "Input velocity", chargeText
+        + "Input drive velocity in mm/sec:", velocity, -500, 500, 10, &ok);
+      if(ok) {
+        ctrl_motion.move_distance(input_distance, velocity);
+        roomba.wait_for_stop();
+      } else {
+        break;
+      }
+    } else {
+      break;
+    }
+
+    // measured deviation
+    deviation_x = QInputDialog::getInt(0, "Input x deviation", chargeText
+      + "Input travelled distance on x axis in mm:", deviation_x,
+      numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
+    if(ok) {
+      deviation_y = QInputDialog::getInt(0, "Input y deviation", chargeText
+        + "Input travelled distance on y axis in mm:", deviation_y,
+        numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
+      if(ok) {
+        cout << "input_distance=" << input_distance << " velocity=" << velocity
+          << " internal_distance=" << roomba.distance() << " deviation_x="
+          << deviation_x << " deviation_y=" << deviation_y
+          << " encoder_ticks_left=" << roomba().left_encoder_counts
+          << " encoder_ticks_right=" << roomba().right_encoder_counts
+          << " batt_charge=" << roomba().charge << " batt_capacity="
+          << roomba().capacity << " batt_voltage=" << roomba().voltage
+          << " batt_current=" << roomba().current << endl;
+      } else {
+        break;
+      }
+    } else {
+      break;
+    }
+  }
+}
+
+/**
+ * turn iterations. logs values to stdout.
+ */
+void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
+  int input_angle = 0, measured_angle = 0, velocity = 100;
+  bool ok = false;
+
+  while(true) {
+    QString chargeText = QString(battery_string).arg(int(float(roomba().charge)
+      / float(roomba().capacity) * 100.0));
+
+    // new angle to turn about
+    input_angle = QInputDialog::getInt(0, "Input angle", chargeText
+      + "Input new angle in degree to turn about:", input_angle,
+      numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
+    if(ok) {
+      // new turn velocity
+      velocity = QInputDialog::getInt(0, "Input velocity", chargeText
+        + "Input turn velocity in mm/sec:", velocity, -500, 500, 10, &ok);
+      if(ok) {
+        ctrl_motion.turn_about(Math::degrees_to_radians(input_angle), velocity);
+        roomba.wait_for_stop();
+      } else {
+        break;
+      }
+    } else {
+      break;
+    }
+
+    // measured angle
+    measured_angle = QInputDialog::getInt(0, "Input measured angle", chargeText
+      + "Input measured angle in degree:", input_angle,
+      numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
+    if(ok) {
+      cout << "input_angle=" << input_angle << " velocity=" << velocity
+        << " measured_angle=" << measured_angle << " internal_angle="
+        << roomba.angle() << " encoder_ticks_left="
+        << roomba().left_encoder_counts << " encoder_ticks_right="
+        << roomba().right_encoder_counts << " batt_charge=" << roomba().charge
+        << " batt_capacity=" << roomba().capacity << " batt_voltage="
+        << roomba().voltage << " batt_current=" << roomba().current << endl;
+    } else {
+      break;
+    }
+  }
+}
+
+/**
+ * main function
+ */
+int main(int argc, char ** argv) {
+
+  if(argc < 2) {
+    cerr << "Usage: " << argv[0] << " --turn|-t|--drive|-d" << endl;
+    exit(-1);
+  }
+
+  OsModel::Os os;
+  OsModel::Timer::self_t timer;
+  Roomba roomba;
+  RoombaUart roomba_uart(os);
+  ControlledMotion ctrl_motion;
+
+  // init stuff
+  QApplication app(argc, argv);
+
+  roomba_uart.set_baudrate(19200);
+  roomba_uart.enable_serial_comm();
+  roomba.init(roomba_uart, timer, Roomba::POSITION
+    | Roomba::BATTERY_AND_TEMPERATURE);
+
+  cout << "Got roomba at " << roomba_uart.address() << endl;
+
+  roomba.reset_distance();
+  roomba.reset_angle();
+  ctrl_motion.init(roomba);
+
+  // actual tests
+  if(strcmp(argv[1], "--turn") == 0 || strcmp(argv[1], "-t") == 0) {
+    turn(roomba, ctrl_motion);
+  } else if(strcmp(argv[1], "--drive") == 0 || strcmp(argv[1], "-d") == 0) {
+    drive(roomba, ctrl_motion);
+  }
+}