Diff: odom.h

Revision:

2:086272a2da1c

Parent:

1:bdd17feaa4ce

diff -r bdd17feaa4ce -r 086272a2da1c odom.h
--- a/odom.h	Fri Dec 07 20:59:56 2018 +0000
+++ b/odom.h	Fri Dec 07 22:34:31 2018 +0000
@@ -2,20 +2,52 @@
 #define _ODOM_H_
 
 #include "mbed.h"
+#include "encoder.h"
+#include "myBNO055.h"
 #include "type.h"
-#include "encoder.h"
-#include "BNO055.h"
-#include "encoder.h"
+
+#define PI                          (3.14159265f)
+#define H_PI                        (1.57079632f)
+#define ENCODER_WHEEL_DIAMETER      (0.0508f)
+#define EDGE_EVALUATION             (4.0f)
+#define PULSE_ENCODER               (500.0f)
+#define ENCODER_COEFFICIENT         ((ENCODER_WHEEL_DIAMETER * PI) / (EDGE_EVALUATION * PULSE_ENCODER))
+
+#define DEG2RAD(x)  (((x) * 3.1416f) / 180.0f)
+#define RAD2DEG(x)  (((x) / 3.1416f) * 180.0f)
+#define ABS(x)      (((x) > 0) ? (x) : -(x))
 
 class Odom
 {
     private:
-        int a;
+        //class instance
+        My_BNO055 *imu_;
+        TIM2Encoder *enc0_;
+        TIM3Encoder *enc1_; 
 
+        float offset_angle_;
+        float actual_angle_;
+
+        Vec3f world_;
+        Vec3f velocity_;
+        Vec3f world_velocity_;
+        
     public:
+        //Constructor
         Odom(){
-            a = 10;  
-        }    
+            initialize();
+        }
+        //TIM IT Callback function
+        void update_it_cb(){
+            calc_odometry();
+        }
+        
+        //Update world angle
+        void update_angle();
+        
+    private:
+        void initialize();
+        void calc_odometry();
 };
 
 class Odom_Abstract