added information about current Roomba orientation; chargeText() function

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

diff --git a/main.cc b/main.cc
index cfba07b..13c5403 100644 (file)
--- a/main.cc
+++ b/main.cc
@@ -46,7 +46,6 @@ 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;
@@ -54,6 +53,14 @@ typedef wiselib::PCComUartModel<OsModel, uart> RoombaUart;
 typedef wiselib::RoombaModel<OsModel, RoombaUart> Roomba;
 typedef wiselib::ControlledMotion<OsModel, Roomba> ControlledMotion;
 
+/**
+ * return battery status as QString
+ */
+QString chargeText(Roomba& roomba) {
+  return QString("Battery: %1%\nPress Cancel to exit.\n\n").arg(int(float(
+    roomba().charge) / float(roomba().capacity) * 100.0));
+}
+
 /**
  * drive iterations. logs values to stdout.
  */
@@ -62,16 +69,14 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
   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 = getInt(0, "Input distance", chargeText
+    input_distance = getInt(0, "Input distance", chargeText(roomba)
       + "Input new distance in mm:", input_distance,
       numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
     if(ok) {
       // new turn velocity
-      velocity = getInt(0, "Input velocity", chargeText
+      velocity = getInt(0, "Input velocity", chargeText(roomba)
         + "Input drive velocity in mm/sec:", velocity, -500, 500, 10, &ok);
       if(ok) {
         ctrl_motion.move_distance(input_distance, velocity);
@@ -84,11 +89,11 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
     }
 
     // measured deviation
-    deviation_x = getInt(0, "Input x deviation", chargeText
+    deviation_x = getInt(0, "Input x deviation", chargeText(roomba)
       + "Input travelled distance on x axis in mm:", deviation_x,
       numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
     if(ok) {
-      deviation_y = getInt(0, "Input y deviation", chargeText
+      deviation_y = getInt(0, "Input y deviation", chargeText(roomba)
         + "Input travelled distance on y axis in mm:", deviation_y,
         numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
       if(ok) {
@@ -113,46 +118,56 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
  * turn iterations. logs values to stdout.
  */
 void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
-  int input_angle = 0, measured_angle = 0, velocity = 100;
+  int cur_angle = 0, turn_angle = 0, measured_angle = 0, velocity = 100;
   bool ok = false;
 
+  // current angle
+  cur_angle = getInt(0, "Input current orientation", chargeText(roomba)
+    + "Input current orientation in degree:", cur_angle, 0, 359, 1, &ok);
+  if(!ok) {
+    return;
+  }
+
   while(true) {
-    QString chargeText = QString(battery_string).arg(int(float(roomba().charge)
-      / float(roomba().capacity) * 100.0));
+    // new turn velocity
+    velocity = getInt(0, "Input velocity", chargeText(roomba)
+      + "Input turn velocity in mm/sec:", velocity, -500, 500, 10, &ok);
+    if(!ok) {
+      break;
+    }
 
-    // new angle to turn about
-    input_angle = 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 = 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 {
+    // angle to turn about
+    turn_angle = getInt(0, "Input turn angle", chargeText(roomba)
+      + "Input angle in degree to turn about:", turn_angle,
+      numeric_limits<int>::min() + 360, numeric_limits<int>::max() - 360, 1,
+      &ok);
+    if(!ok) {
       break;
     }
 
-    // measured angle
-    measured_angle = 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 {
+    ctrl_motion.turn_about(Math::degrees_to_radians(turn_angle), velocity);
+    roomba.wait_for_stop();
+
+    // new current angle
+    measured_angle = getInt(0, "Input measured angle", chargeText(roomba)
+      + QString("Orientation should be %1 degree now.\n\n").arg((cur_angle
+      + turn_angle) % 360) + "Input measured angle in degree the Roomba has "
+      "turned:", turn_angle, 0, 359, 1, &ok);
+    if(!ok) {
       break;
     }
+
+    cout << "turn_angle=" << turn_angle << " measured_angle=" << measured_angle
+      << " velocity=" << velocity << " 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;
+
+    // new orientation
+    cur_angle = (cur_angle + turn_angle) % 360;
+
   }
 }