File content as of revision 10:3b47050b1652:

#include "odom.h"

void Odom::initialize(){
    //Create instance
    imu_ = new My_BNO055(PB_4, PA_8, PB_10, 0x29 << 1, MODE_NDOF);
    enc0_ = new TIM2Encoder;
    enc1_ = new TIM3Encoder;
    
    //Initialize BNO055
    wait(0.3);
    if (imu_->chip_ready() == 0){
        do {
            wait(0.1);
        } while(imu_->reset());
    }
    
    offset_angle_ = 0;
    enable_oled_ = true;
    
    //Set TIM IT Callback
    it_tick = new Ticker;
    it_tick->attach(this, &Odom::update_it_cb, 0.02);
}
    
void Odom::calc_odometry(){
    Vec3f velocity;
    Vec3f world_velocity;
        
    velocity.x((enc0_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f)));
    velocity.y((enc1_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f)));
    
    world_velocity.x(((velocity.x() * cosf(odom_.angle() + initialpose_.angle() - H_PI)) - (velocity.y() * sinf(odom_.angle() + initialpose_.angle() - H_PI))));
    world_velocity.y(((velocity.x() * sinf(odom_.angle() + initialpose_.angle() - H_PI)) + (velocity.y() * cosf(odom_.angle() + initialpose_.angle() - H_PI))));
        
    odom_.x(odom_.x() + world_velocity.x() * 0.02f);
    odom_.y(odom_.y() + world_velocity.y() * 0.02f);   
}

void Odom::update_angle(){
    static float prev_angle;
    static int rotation_flag;//180回転ごとに記録
    
    Vec3f angle = imu_->get_radian();
    float z_angle = angle.z();

    if(prev_angle < -H_PI && z_angle > H_PI){
        rotation_flag--;
    }else if(prev_angle > H_PI && z_angle < -H_PI){
        rotation_flag++;   
    }else if(rotation_flag && 0 < prev_angle && prev_angle < H_PI && -H_PI < z_angle && z_angle < 0){
        rotation_flag--;
    }else if(rotation_flag && -H_PI < prev_angle && prev_angle < 0 && 0 < z_angle && z_angle < H_PI){
        rotation_flag++;
    }
    
    if(!rotation_flag){
        actual_angle_ = z_angle;
    }else if(ABS(rotation_flag) % 2){
        actual_angle_ = -((rotation_flag > 0 ? -PI : PI) - z_angle) + (rotation_flag * PI);        
    }else{
        actual_angle_ = z_angle + (rotation_flag * PI);
    }
    
    //オフセットを考慮して角度を設定
    odom_.angle(actual_angle_ - offset_angle_);
    
    //値更新
    prev_angle = z_angle; 
}