From 37da2859aefe405088aefbf8ce772a4fa9cd7292 Mon Sep 17 00:00:00 2001
From: svnhieber <svnhieber@f8795833-4959-0410-8ae9-8bcb0cfab428>
Date: Tue, 11 Jan 2011 16:53:57 +0000
Subject: [PATCH] encoder tick "wrapping", some useful initialisations

git-svn-id: https://svn.itm.uni-luebeck.de/wisebed/wiselib/trunk/pc_apps/roomba_tests@4001 f8795833-4959-0410-8ae9-8bcb0cfab428
---
 main.cc | 56 ++++++++++++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 48 insertions(+), 8 deletions(-)

diff --git a/main.cc b/main.cc
index 5002d70..3eb6874 100644
--- a/main.cc
+++ b/main.cc
@@ -40,7 +40,6 @@ using namespace std;
 #else // QT_VERSION > 0x040500
 #define getInt QInputDialog::getInt
 #endif // QT_VERSION
-
 // UART port on which we communicate with the Roomba
 char uart[] = "/dev/ttyUSB0";
 
@@ -65,18 +64,47 @@ char * ground_type;
  * Sensor data we need, filled in callback
  */
 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) {
+  }
+
   uint16_t capacity, charge;
   uint8_t charging;
   int16_t current;
   int8_t temperature;
   uint16_t voltage;
-  int16_t left_encoder_counts, right_encoder_counts;
+  /** absolute encoder counts; i.e. not overflown, but consecutive */
+  int abs_left_encoder_counts, abs_right_encoder_counts;
 } sensor_data;
 
+/**
+ * Returns the difference between two unsigned short values. The calculated
+ * value is always smaller or equal to 0x8000.
+ * 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 d = current - last;
+  if(d < -0x8000) { // overflow in positive direction
+    d = (0x10000 - last_i + this_i);
+  }
+  if(d >= 0x8000) { // overflow in negative direction
+    d = -(0x10000 - this_i + last_i);
+  }
+}
+
 /**
  * Callback that fills the sensor data when data is available
  */
 struct DataAvailable {
+  int latest_encoder_left_, latest_encoder_right_;
+
+  DataAvailable() :
+    latest_encoder_left_(0), latest_encoder_right_(0) {
+  }
+
   void cb(int state) {
     if(state != Roomba::DATA_AVAILABLE) {
       return;
@@ -86,8 +114,12 @@ struct DataAvailable {
     sensor_data.charging = roomba().charging;
     sensor_data.current = roomba().current;
     sensor_data.voltage = roomba().voltage;
-    sensor_data.left_encoder_counts = roomba().left_encoder_counts;
-    sensor_data.right_encoder_counts = roomba().right_encoder_counts;
+    sensor_data.abs_left_encoder_counts += nearest_diff(latest_encoder_left_,
+      roomba().left_encoder_counts);
+    latest_encoder_left_ = roomba().left_encoder_counts;
+    sensor_data.abs_right_encoder_counts += nearest_diff(latest_encoder_right_,
+      roomba().right_encoder_counts);
+    latest_encoder_right_ = roomba().right_encoder_counts;
   }
 } data_available;
 
@@ -142,11 +174,15 @@ void drive(Roomba& roomba, ControlledMotion& ctrl_motion) {
       << " input_distance=" << input_distance << " velocity=" << velocity
       << " internal_distance=" << roomba.distance() << " deviation_x="
       << deviation_x << " deviation_y=" << deviation_y
-      << " encoder_ticks_left=" << sensor_data.left_encoder_counts
-      << " encoder_ticks_right=" << sensor_data.right_encoder_counts
+      << " 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;
+
+    // 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;
   }
 }
 
@@ -197,12 +233,16 @@ void turn(Roomba& roomba, ControlledMotion& ctrl_motion) {
       << " move=straight" << " ground_type=" << ground_type << "turn_angle="
       << turn_angle << " measured_angle=" << measured_angle << " velocity="
       << velocity << " internal_angle=" << roomba.angle()
-      << " encoder_ticks_left=" << sensor_data.left_encoder_counts
-      << " encoder_ticks_right=" << sensor_data.right_encoder_counts
+      << " 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;
 
+    // reset, because we only need the difference between two turns
+    sensor_data.abs_left_encoder_counts = 0;
+    sensor_data.abs_right_encoder_counts = 0;
+
     // new orientation
     cur_angle = (cur_angle + measured_angle) % 360;
   }
-- 
2.20.1