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),
 
   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;
   }
 
   uint16_t capacity, charge;


@@ -75,8 +75,10 @@ struct SensorData {
   int16_t current;
   int8_t temperature;
   uint16_t voltage;
   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 */
   /** 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;
 
 /**
 } 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.
  */
  * 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);
   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 {
  * 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() :
 
   DataAvailable() :

-    latest_encoder_left_(0), latest_encoder_right_(0) {
+    latest_ticks_left_(0), latest_ticks_right_(0) {

   }
 
   void cb(int state) {
   }
 
   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.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);
       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);
       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
  */
   }
 } 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 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.
  */
 /**
  * drive iterations. logs values to stdout.
  */


@@ -141,14 +158,14 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
   while(true) {
 
     // new distance to drive
   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
       + "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;
       + "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
     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;
     }
       + "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;
     }
       + "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
 
     // 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
   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;
     + "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
 
   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
       + "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);
       + "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
     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;
     }
 
       "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="
       << 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
 
     // 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;
 
     // new orientation
     cur_angle = (cur_angle + measured_angle) % 360;