some minor refactoring

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

diff --git a/main.cc b/main.cc
index 6051236..b598a4e 100644 (file)
--- a/main.cc
+++ b/main.cc
@@ -67,7 +67,7 @@ struct SensorData {
 
   SensorData() :
     capacity(0), charge(0), charging(0), current(0), temperature(0),
-      voltage(0), abs_left_encoder_counts(0), abs_right_encoder_counts(0) {
+      voltage(0), diff_left_ticks(0), diff_right_ticks(0) {
   }
 
   uint16_t capacity, charge;
@@ -75,8 +75,10 @@ struct SensorData {
   int16_t current;
   int8_t temperature;
   uint16_t voltage;
+  /** raw encoder counts; i.e. overflown, not consecutive */
+  int raw_left_ticks, raw_right_ticks;
   /** absolute encoder counts; i.e. not overflown, but consecutive */
-  int abs_left_encoder_counts, abs_right_encoder_counts;
+  int diff_left_ticks, diff_right_ticks;
 } sensor_data;
 
 /**
@@ -85,7 +87,7 @@ struct SensorData {
  * This is useful if you have an overflowing counter and you want to determine
  * when you have to "wrap over" the value.
  */
-int nearest_diff(unsigned short last, unsigned short current) {
+int nearestDiff(unsigned short last, unsigned short current) {
   int d = current - last;
   if(d < -0x8000) { // overflow in positive direction
     d = (0x10000 - last + current);
@@ -99,10 +101,10 @@ int nearest_diff(unsigned short last, unsigned short current) {
  * Callback that fills the sensor data when data is available
  */
 struct DataAvailable {
-  int latest_encoder_left_, latest_encoder_right_;
+  int latest_ticks_left_, latest_ticks_right_;
 
   DataAvailable() :
-    latest_encoder_left_(0), latest_encoder_right_(0) {
+    latest_ticks_left_(0), latest_ticks_right_(0) {
   }
 
   void cb(int state) {
@@ -114,23 +116,38 @@ struct DataAvailable {
     sensor_data.charging = roomba().charging;
     sensor_data.current = roomba().current;
     sensor_data.voltage = roomba().voltage;
-    sensor_data.abs_left_encoder_counts += nearest_diff(latest_encoder_left_,
+    sensor_data.raw_left_ticks = roomba().left_encoder_counts;
+    sensor_data.raw_right_ticks = roomba().right_encoder_counts;
+    sensor_data.diff_left_ticks += nearestDiff(latest_ticks_left_,
       roomba().left_encoder_counts);
-    latest_encoder_left_ = roomba().left_encoder_counts;
-    sensor_data.abs_right_encoder_counts += nearest_diff(latest_encoder_right_,
+    latest_ticks_left_ = roomba().left_encoder_counts;
+    sensor_data.diff_right_ticks += nearestDiff(latest_ticks_right_,
       roomba().right_encoder_counts);
-    latest_encoder_right_ = roomba().right_encoder_counts;
+    latest_ticks_right_ = roomba().right_encoder_counts;
   }
 } data_available;
 
 /**
  * return battery status as QString
  */
-QString chargeText(Roomba& roomba) {
+QString chargeText() {
   return QString("Battery: %1%\nPress Cancel to exit.\n\n").arg(int(float(
     sensor_data.charge) / float(sensor_data.capacity) * 100.0));
 }
 
+/**
+ * return log text for global values
+ */
+QString logText() {
+  return QString("svn=%1 roomba_id=%2 ground_type=%3 diff_left_ticks=%4 "
+    "diff_right_ticks=%5 raw_ticks_left=%6 raw_ticks_right=%7 batt_charge=%8 "
+    "batt_capacity=%9 batt_voltage=%10 batt_current=%11").arg(SVNREVISION).arg(
+    roomba_id).arg(ground_type).arg(sensor_data.diff_left_ticks).arg(
+    sensor_data.diff_right_ticks).arg(sensor_data.raw_left_ticks).arg(
+    sensor_data.raw_right_ticks).arg(sensor_data.charge).arg(
+    sensor_data.capacity).arg(sensor_data.voltage).arg(sensor_data.current);
+}
+
 /**
  * drive iterations. logs values to stdout.
  */
@@ -141,14 +158,14 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
   while(true) {
 
     // new distance to drive
-    input_distance = getInt(0, "Input distance", chargeText(roomba)
+    input_distance = getInt(0, "Input distance", chargeText()
       + "Input new distance in mm:", input_distance,
       numeric_limits<int>::min(), numeric_limits<int>::max(), 1, &ok);
     if(!ok) {
       break;
     }
     // new velocity
-    velocity = getInt(0, "Input velocity", chargeText(roomba)
+    velocity = getInt(0, "Input velocity", chargeText()
       + "Input drive velocity in mm/sec:", velocity, -500, 500, 10, &ok);
     if(!ok) {
       break;
@@ -157,32 +174,26 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
     roomba.wait_for_stop();
 
     // measured deviation
-    deviation_x = getInt(0, "Input x deviation", chargeText(roomba)
+    deviation_x = 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) {
       break;
     }
-    deviation_y = getInt(0, "Input y deviation", chargeText(roomba)
+    deviation_y = 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) {
       break;
     }
-    cout << "svn=" << SVNREVISION << " roomba_id=" << roomba_id
-      << " move=straight" << " ground_type=" << ground_type
-      << " input_distance=" << input_distance << " velocity=" << velocity
-      << " internal_distance=" << roomba.distance() << " deviation_x="
-      << deviation_x << " deviation_y=" << deviation_y
-      << " abs_encoder_ticks_left=" << sensor_data.abs_left_encoder_counts
-      << " abs_encoder_ticks_right=" << sensor_data.abs_right_encoder_counts
-      << " batt_charge=" << sensor_data.charge << " batt_capacity="
-      << sensor_data.capacity << " batt_voltage=" << sensor_data.voltage
-      << " batt_current=" << sensor_data.current << endl;
+    cout << logText().toAscii().constData() << " move=straight input_distance="
+      << input_distance << " velocity=" << velocity << " internal_distance="
+      << roomba.distance() << " deviation_x=" << deviation_x << " deviation_y="
+      << deviation_y << endl;
 
     // reset, because we only need the difference between two drive commands
-    sensor_data.abs_left_encoder_counts = 0;
-    sensor_data.abs_right_encoder_counts = 0;
+    sensor_data.diff_left_ticks = 0;
+    sensor_data.diff_right_ticks = 0;
   }
 }
 
@@ -194,7 +205,7 @@ void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
   bool ok = false;
 
   // current angle
-  cur_angle = getInt(0, "Input current orientation", chargeText(roomba)
+  cur_angle = getInt(0, "Input current orientation", chargeText()
     + "Input current orientation in degree:", cur_angle, 0, 359, 1, &ok);
   if(!ok) {
     return;
@@ -202,14 +213,14 @@ void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
 
   while(true) {
     // new turn velocity
-    velocity = getInt(0, "Input velocity", chargeText(roomba)
+    velocity = getInt(0, "Input velocity", chargeText()
       + "Input turn velocity in mm/sec:", velocity, -500, 500, 10, &ok);
     if(!ok) {
       break;
     }
 
     // angle to turn about
-    turn_angle = getInt(0, "Input turn angle", chargeText(roomba)
+    turn_angle = getInt(0, "Input turn angle", chargeText()
       + "Input angle in degree to turn about:", turn_angle,
       numeric_limits<int>::min() + 360, numeric_limits<int>::max() - 360, 1,
       &ok);
@@ -221,27 +232,21 @@ void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
     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 "
+    measured_angle = getInt(0, "Input measured angle", chargeText() + 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, numeric_limits<int>::max(), 1, &ok);
     if(!ok) {
       break;
     }
 
-    cout << "svn=" << SVNREVISION << " roomba_id=" << roomba_id
-      << " move=turn " << " ground_type=" << ground_type << " turn_angle="
+    cout << logText().toAscii().constData() << " move=turn turn_angle="
       << turn_angle << " measured_angle=" << measured_angle << " velocity="
-      << velocity << " internal_angle=" << roomba.angle()
-      << " abs_encoder_ticks_left=" << sensor_data.abs_left_encoder_counts
-      << " abs_encoder_ticks_right=" << sensor_data.abs_right_encoder_counts
-      << " batt_charge=" << sensor_data.charge << " batt_capacity="
-      << sensor_data.capacity << " batt_voltage=" << sensor_data.voltage
-      << " batt_current=" << sensor_data.current << endl;
+      << velocity << " internal_angle=" << roomba.angle() << endl;
 
     // reset, because we only need the difference between two turns
-    sensor_data.abs_left_encoder_counts = 0;
-    sensor_data.abs_right_encoder_counts = 0;
+    sensor_data.diff_left_ticks = 0;
+    sensor_data.diff_right_ticks = 0;
 
     // new orientation
     cur_angle = (cur_angle + measured_angle) % 360;