File content as of revision 11:375d474b8522:

#ifndef _ODOM_H_
#define _ODOM_H_

#include "mbed.h"
#include "encoder.h"
#include "myBNO055.h"
#include "type.h"
#include "math.h"

#define PI                          (3.14159265f)
#define H_PI                        (1.57079632f)
#define ENCODER_WHEEL_DIAMETER      (0.054f)
#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 odom_;
        
        //For ROS
        bool court_color_;
        Vec3f initialpose_;
        Vec3f drift_;
        
        //For TIM IT
        Ticker *it_tick;
        
        //For OLED
        bool enable_oled_;
        
    public:
        //Constructor
        Odom(){
            initialize();
        }
        //TIM IT Callback function
        void update_it_cb(){
            calc_odometry();
        }
        
        //Update world angle
        void update_angle();
        
        void reset_odom(){
            odom_.x(0.0);
            odom_.y(0.0);
            odom_.angle(0.0);
        }
        
        void reset_drift(){
            drift_.x(0.0);
            drift_.y(0.0);
            drift_.angle(0.0);
        }
        
        //現在角度を0度に設定
        void set_offset_angle_zero(){
            offset_angle_ = actual_angle_;
        }
        
        //現在角度を0度に設定
        void set_offset_angle_initialpose(){
            offset_angle_ = actual_angle_ - initialpose_.angle();
        }
                
        //Getter
        bool get_court_color(){
            return court_color_;   
        }
        Vec3f get_odom(){
            return odom_;   
        }
        Vec3f get_initialpose(){
            return initialpose_;   
        }
        Vec3f get_drift(){
            return drift_;   
        }
        bool get_enable_oled(){
            return enable_oled_;   
        }
            
        //Setter
        void set_court_color(bool court_color){
            court_color_ = court_color;   
        }
        void set_initialpose(Vec3f initialpose){
            initialpose_ = initialpose;   
        }
        void set_drift(Vec3f drift){
            drift_ = drift;   
        }
        void set_enable_oled(bool enable_oled){
            enable_oled_ = enable_oled;   
        }
        
    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 reset_odometry(){
            odom_->reset_odom();
            odom_->reset_drift(); 
            odom_->set_offset_angle_zero();  
//            odom_->set_offset_angle_initialpose();
        }
        
        //Getter
        bool get_court_color(){
            return odom_->get_court_color();   
        }
        
        Vec3f get_initial_pose(){
            return odom_->get_initialpose();
        }
        Vec3f get_odom(){
            return odom_->get_odom();
        }
        Vec3f get_drift(){
            return odom_->get_drift();
        }
        
        Vec3f get_pose(){
            return get_initial_pose() + get_odom();
        }
        Vec3f get_world(){
//            return get_pose() + get_drift();   
            Vec3f ret;
            float t = get_drift().angle();
            ret.x(get_pose().x() * cos(t) - get_pose().y() * sin(t) + get_drift().x());
            ret.y(get_pose().x() * sin(t) + get_pose().y() * cos(t) + get_drift().y());
            ret.angle(get_pose().angle() + t); 
            return ret;
        }
        
        bool get_enable_oled(){
            return odom_->get_enable_oled();   
        }
        
        //Setter
        void set_court_color(bool court_color){
            odom_->set_court_color(court_color);   
        }
        
        void set_initial_pose(Vec3f initialpose){
            odom_->set_initialpose(initialpose);//CANから受け取った初期位置の情報を格納するためのメソッド
        }
        void set_drift(Vec3f drift){
            odom_->set_drift(drift);//ROSからサブスクライブしたデータを格納するためのメソッド
        }
        
        void set_enable_oled(bool enable_oled){
            odom_->set_enable_oled(enable_oled);   
        }
    
};
#endif