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