File content as of revision 2:086272a2da1c:

#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_;
        
    public:
        //Constructor
        Odom(){
            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
{
    private:
        Odom *odom_;
        Vec3f initial_pose_;
        Vec3f drift_;
        
    public:
        Odom_Abstract(){
            
        }
        
        Odom_Abstract(Odom *odom) : odom_(odom){
            
        }
        
        void set_initial_pose(Vec3f initial_pose){
            initial_pose_ = initial_pose;
        }
        
        Vec3f get_initial_pose(){
            return initial_pose_;
        }
        
        void set_drift(Vec3f drift){
            drift_ = drift;
        }
        
        Vec3f get_drift(){
            return drift_;
        }
        
//        virtual void loop() = 0;
};
#endif