File content as of revision 2:086272a2da1c:

#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;
}
    
void Odom::calc_odometry(){
    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(-world_.angle())) - (velocity_.y() * sinf(-world_.angle()))));
    world_velocity_.y(((velocity_.x() * sinf(-world_.angle())) + (velocity_.y() * cosf(-world_.angle()))));
      
    world_.x(world_.x() + world_velocity_.x() * 0.02f);
    world_.y(world_.y() + world_velocity_.y() * 0.02f);   
}

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

    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);
    }
    
    //オフセットを考慮して角度を設定
    world_.angle(actual_angle_ + offset_angle_);
    
    //値更新
    prev_angle = z_angle; 
}