File content as of revision 4:cf1a4e503974:

#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))

#define COURT_RED   false
#define COURT_BLUE  true

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
        bool court_color_;
        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
        bool get_court_color(){
            return court_color_;   
        }
        Vec3f get_world(){
            return world_;   
        }
        Vec3f get_initialpose(){
            return initialpose_;   
        }
        Vec3f get_drift(){
            return drift_;   
        }
        
        //Setter
        void set_court_color(bool court_color){
            court_color_ = court_color;   
        }
        void set_initialpose(Vec3f initialpose){
            initialpose_ = initialpose;   
        }
        void set_world(Vec3f world){
            world_ = world;   
        }
        void set_drift(Vec3f drift){
            drift_ = drift;   
        }
        
    private:
        void initialize();
        void calc_odometry();
};

class Odom_Abstract
{
    private:
        Odom *odom_;
        
    public:
        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);
        }
        
        //Getter
        bool get_court_color(){
            return odom_->get_court_color();   
        }
        Vec3f get_initial_pose(){
            return odom_->get_initialpose();
        }
        Vec3f get_pose(){
            return odom_->get_world();   
        }
        Vec3f get_drift(){
            return odom_->get_drift();
        }
        
        //Setter
        void set_court_color(bool court_color){
            odom_->set_court_color(court_color);   
        }
        void set_pose(Vec3f pose){
            odom_->set_world(pose);   
        }
        void set_drift(Vec3f drift){
            odom_->set_drift(drift);
        }
    
};
#endif