Diff: odometry/odom.h

Revision:

3:a45557a0dcb8

Parent:

2:086272a2da1c

Child:

4:cf1a4e503974

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/odometry/odom.h	Tue Dec 11 17:51:47 2018 +0000
@@ -0,0 +1,121 @@
+#ifndef _ODOM_H_
+#define _ODOM_H_
+
+#include "mbed.h"
+#include "encoder.h"
+#include "myBNO055.h"
+#include "type.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:
+        //class instance
+        My_BNO055 *imu_;
+        TIM2Encoder *enc0_;
+        TIM3Encoder *enc1_; 
+
+        float offset_angle_;
+        float actual_angle_;
+
+        Vec3f world_;
+        Vec3f velocity_;
+        Vec3f world_velocity_;
+        
+        //For amcl
+        Vec3f initialpose_;
+        Vec3f drift_;
+        
+        //For TIM IT
+        Ticker *it_tick;
+        
+    public:
+        //Constructor
+        Odom(){
+            initialize();
+        }
+        //TIM IT Callback function
+        void update_it_cb(){
+            calc_odometry();
+        }
+        
+        //Update world angle
+        void update_angle();
+        
+        //Getter
+        Vec3f get_world(){
+            return world_;   
+        }
+        Vec3f get_initialpose(){
+            return initialpose_;   
+        }
+        Vec3f get_drift(){
+            return drift_;   
+        }
+        
+        //Setter
+        void set_initialpose(Vec3f initialpose){
+            initialpose_ = initialpose;   
+        }
+        void set_drift(Vec3f drift){
+            drift_ = drift;   
+        }
+        
+    private:
+        void initialize();
+        void calc_odometry();
+};
+
+class Odom_Abstract
+{
+    private:
+        Odom *odom_;
+        
+    public:
+        Odom_Abstract(){
+            
+        }
+        
+        Odom_Abstract(Odom *odom) : odom_(odom){
+            
+        }
+    
+        virtual void loop() = 0;
+        
+    protected:
+        void set_instance(Odom* odom){
+            odom_ = odom;   
+        }
+        
+        void set_initial_pose(Vec3f initialpose){
+            odom_->set_initialpose(initialpose);
+        }
+        
+        Vec3f get_initial_pose(){
+            return odom_->get_initialpose();
+        }
+        
+        Vec3f get_pose(){
+            return odom_->get_world();   
+        }
+        
+        void set_drift(Vec3f drift){
+            odom_->set_drift(drift);
+        }
+        
+        Vec3f get_drift(){
+            return odom_->get_drift();
+        }
+        
+};
+#endif
\ No newline at end of file