Merge pull request #1 from AutonomyLab/vel

Instantaneous velocity now available

include/create/create.h

@@ -34,12 +34,12 @@ POSSIBILITY OF SUCH DAMAGE.
 
 #include <boost/shared_ptr.hpp>
 #include <string>
-#include <sys/time.h>
 #include <unistd.h>
 
 #include "create/serial.h"
 #include "create/data.h"
 #include "create/types.h"
+#include "create/util.h"
 
 namespace create {
   class Create {
@@ -64,12 +64,14 @@ namespace create {
       uint8_t powerLEDIntensity;
 
       create::Pose pose;
+      create::Vel vel;
       
       uint32_t prevTicksLeft;
       uint32_t prevTicksRight;
       float prevLeftVel;
       float prevRightVel;
       bool firstOnData;
+      util::timestamp_t prevOnDataTime;
 
       void init();
       bool updateLEDs();
@@ -346,6 +348,10 @@ namespace create {
       /* Get the estimated position of Create based on it's wheel encoders.
        */
       const create::Pose& getPose() const;
+
+      /* Get the estimated velocity of Create based on wheel encoders.
+       */
+      const create::Vel& getVel() const;
   };  // end Create class
 
 }  // namespace create

include/create/types.h

@@ -185,6 +185,7 @@ namespace create {
     float yaw;
   };
 
+  typedef Pose Vel;
 } // namespace create
 
 #endif // CREATE_TYPES_H

include/create/util.h

@@ -32,6 +32,8 @@ POSSIBILITY OF SUCH DAMAGE.
 #ifndef CREATE_UTIL_H
 #define CREATE_UTIL_H
 
+#include <sys/time.h>
+
 #define COUT(prefix,msg) (std::cout<<prefix<<msg<<std::endl)
 #define CERR(prefix,msg) (std::cerr<<prefix<<msg<<std::endl)
 
@@ -46,7 +48,7 @@ namespace create {
     static const float CREATE_2_MAX_VEL = 0.5;
     static const float PI = 3.14159;
     static const float TWO_PI = 6.28318;
-    static const float EPS = 0.001;
+    static const float EPS = 0.0001;
   
     inline float normalizeAngle(const float& angle) {
       float a = angle;
@@ -55,6 +57,14 @@ namespace create {
       return a;
     };
 
+    /** Get a timestamp for the current time in micro-seconds.
+     */
+    typedef unsigned long long timestamp_t;
+    static timestamp_t getTimestamp() {
+      struct timeval now;
+      gettimeofday(&now, NULL);
+      return now.tv_usec + (timestamp_t) now.tv_sec * 1000000;
+    }
   }  // namespace util
 } // namespace create
 

src/create.cpp

@@ -30,6 +30,9 @@ namespace create {
     pose.x = 0;
     pose.y = 0;
     pose.yaw = 0;
+    vel.x = 0;
+    vel.y = 0;
+    vel.yaw = 0;
     data = boost::shared_ptr<Data>(new Data());
     serial = boost::make_shared<Serial>(data);
   }
@@ -52,9 +55,14 @@ namespace create {
       // Initialize tick counts
       prevTicksLeft = GET_DATA(ID_LEFT_ENC);
       prevTicksRight = GET_DATA(ID_RIGHT_ENC);
+      prevOnDataTime = util::getTimestamp();
       firstOnData = false;
     }
 
+    // Get current time
+    util::timestamp_t curTime = util::getTimestamp();
+    float dt = (curTime - prevOnDataTime) / 1000000.0;
+
     // Get cumulative ticks (wraps around at 65535)
     uint16_t totalTicksLeft = GET_DATA(ID_LEFT_ENC);
     uint16_t totalTicksRight = GET_DATA(ID_RIGHT_ENC);
@@ -82,18 +90,39 @@ namespace create {
     float deltaDist = (rightWheelDist + leftWheelDist) / 2.0;
     
     // Moving straight
+    float deltaX, deltaY;
     if (fabs(wheelDistDiff) < util::EPS) {
-      pose.x += deltaDist * cos(pose.yaw);
-      pose.y += deltaDist * sin(pose.yaw);
+      deltaX = deltaDist * cos(pose.yaw);
+      deltaY = deltaDist * sin(pose.yaw);
+      vel.yaw = 0;
     }
     else {
       float turnRadius = (util::CREATE_2_AXLE_LENGTH / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
       float deltaYaw = (rightWheelDist - leftWheelDist) / util::CREATE_2_AXLE_LENGTH; 
-      pose.x += turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw));
-      pose.y += turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw));
+      deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw));
+      deltaY = turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw));
       pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw);
+      if (fabs(dt) > util::EPS) {
+        vel.yaw = deltaYaw / dt;
+      }
+      else {
+        vel.yaw = 0;
+      }
     }
   
+    if (fabs(dt) > util::EPS) {
+      vel.x = deltaX / dt;
+      vel.y = deltaY / dt;
+    }
+    else {
+      vel.x = 0;
+      vel.y = 0;
+    }
+
+    pose.x += deltaDist * cos(pose.yaw);
+    pose.y += deltaDist * sin(pose.yaw);
+   
+    prevOnDataTime = curTime;
     // Make user registered callbacks, if any
     // TODO
   }
@@ -472,4 +501,8 @@ namespace create {
     return pose;
   }
 
+  const Vel& Create::getVel() const {
+    return vel;
+  }
+
 } // end namespace