From d6f759d6831f848602bd1849075cc3b873f1e838 Mon Sep 17 00:00:00 2001
From: Ben Wolsieffer <benwolsieffer@gmail.com>
Date: Thu, 23 Jun 2016 16:27:05 -0400
Subject: [PATCH] Expose individual wheel distances and requested velocities.
 Fix wheel distance calculation for the Create 1.

---
 include/create/create.h | 20 ++++++++++++++++--
 include/create/util.h   |  2 ++
 src/create.cpp          | 45 ++++++++++++++++++++++++++++++++++++++---
 src/data.cpp            |  4 ++--
 4 files changed, 64 insertions(+), 7 deletions(-)

diff --git a/include/create/create.h b/include/create/create.h
index 640bb88..3d37e97 100644
--- a/include/create/create.h
+++ b/include/create/create.h
@@ -73,8 +73,8 @@ namespace create {
 
       uint32_t prevTicksLeft;
       uint32_t prevTicksRight;
-      float prevLeftVel;
-      float prevRightVel;
+      float totalLeftDist;
+      float totalRightDist;
       bool firstOnData;
       util::timestamp_t prevOnDataTime;
 
@@ -444,6 +444,22 @@ namespace create {
        */
       bool isMovingForward() const;
 
+      /* Get the total distance (in meters) the left wheel has moved.
+       */
+      float getLeftWheelDistance() const;
+
+      /* Get the total distance (in meters) the right wheel has moved.
+       */
+      float getRightWheelDistance() const;
+
+      /* Get the requested velocity (in meters/sec) of the left wheel.
+       */
+      float getRequestedLeftWheelVel() const;
+
+      /* Get the requested velocity (in meters/sec) of the right wheel.
+       */
+      float getRequestedRightWheelVel() const;
+
       /* Get the current charging state.
        */
       create::ChargingState getChargingState() const;
diff --git a/include/create/util.h b/include/create/util.h
index 62e2bbb..7304209 100644
--- a/include/create/util.h
+++ b/include/create/util.h
@@ -40,6 +40,8 @@ POSSIBILITY OF SUCH DAMAGE.
 namespace create {
   namespace util {
 
+    static const float CREATE_1_AXLE_LENGTH = 0.26;
+
     static const uint8_t CREATE_2_HEADER = 19;
     static const float CREATE_2_AXLE_LENGTH = 0.235;
     static const float CREATE_2_TICKS_PER_REV = 508.8;
diff --git a/src/create.cpp b/src/create.cpp
index f2c7468..3d21fe8 100644
--- a/src/create.cpp
+++ b/src/create.cpp
@@ -28,6 +28,8 @@ namespace create {
     powerLEDIntensity = 0;
     prevTicksLeft = 0;
     prevTicksRight = 0;
+    totalLeftDist = 0.0;
+    totalRightDist = 0.0;
     firstOnData = true;
     pose.x = 0;
     pose.y = 0;
@@ -104,9 +106,9 @@ namespace create {
       deltaYaw = ((int16_t) GET_DATA(ID_ANGLE)) * (util::PI / 180.0); // D2R
       deltaX = deltaDist * cos( util::normalizeAngle(pose.yaw + deltaYaw) );
       deltaY = -deltaDist * sin( util::normalizeAngle(pose.yaw + deltaYaw) );
-      leftWheelDist = deltaDist / 2.0;
-      rightWheelDist = leftWheelDist;
-
+      const float deltaYawWheelDist = (util::CREATE_1_AXLE_LENGTH / 2.0) * deltaYaw;
+      leftWheelDist = deltaDist - deltaYawWheelDist;
+      rightWheelDist = deltaDist + deltaYawWheelDist;
     }
     else if (model == CREATE_2) {
       // Get cumulative ticks (wraps around at 65535)
@@ -149,6 +151,9 @@ namespace create {
       }
     } // if CREATE_2
 
+    totalLeftDist += leftWheelDist;
+    totalRightDist += rightWheelDist;
+
     if (fabs(dt) > util::EPS) {
       vel.x = deltaDist / dt;
       vel.y = 0.0;
@@ -863,6 +868,40 @@ namespace create {
     }
   }
 
+  float Create::getLeftWheelDistance() const {
+      return totalLeftDist;
+  }
+
+  float Create::getRightWheelDistance() const {
+      return totalRightDist;
+  }
+
+  float Create::getRequestedLeftWheelVel() const {
+      if (data->isValidPacketID(ID_LEFT_VEL)) {
+          uint16_t uvel = GET_DATA(ID_LEFT_VEL);
+          int16_t vel;
+          std::memcpy(&vel, &uvel, sizeof(vel));
+          return (vel / 1000.0);
+      }
+      else {
+        CERR("[create::Create] ", "Left wheel velocity not supported!");
+        return 0;
+      }
+  }
+
+  float Create::getRequestedRightWheelVel() const {
+      if (data->isValidPacketID(ID_RIGHT_VEL)) {
+        uint16_t uvel = GET_DATA(ID_RIGHT_VEL);
+        int16_t vel;
+        std::memcpy(&vel, &uvel, sizeof(vel));
+        return (vel / 1000.0);
+      }
+      else {
+        CERR("[create::Create] ", "Right wheel velocity not supported!");
+        return 0;
+      }
+  }
+
   create::CreateMode Create::getMode() const {
     if (data->isValidPacketID(ID_OI_MODE)) {
       return (create::CreateMode) GET_DATA(ID_OI_MODE);
diff --git a/src/data.cpp b/src/data.cpp
index 4960fed..198bcca 100644
--- a/src/data.cpp
+++ b/src/data.cpp
@@ -26,6 +26,8 @@ namespace create {
     ADD_PACKET(ID_CAPACITY, 2, "battery_capacity");
     ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall");
     ADD_PACKET(ID_OI_MODE, 1, "oi_mode");
+    ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right");
+    ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left");
 
     if (model == CREATE_1) {
       ADD_PACKET(ID_DISTANCE, 2, "distance");
@@ -48,8 +50,6 @@ namespace create {
       //ADD_PACKET(ID_NUM_STREAM_PACKETS, 1, "oi_stream_num_packets");
       //ADD_PACKET(ID_VEL, 2, "velocity");
       //ADD_PACKET(ID_RADIUS, 2, "radius");
-      //ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right");
-      //ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left");
       ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left");
       ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right");
       ADD_PACKET(ID_LIGHT, 1, "light_bumper");