odom.cpp

00001 #include "odom.h"
00002 
00003 void Odom::initialize(){
00004     //Create instance
00005     imu_ = new My_BNO055(PB_4, PA_8, PB_10, 0x29 << 1, MODE_NDOF);
00006     enc0_ = new TIM2Encoder;
00007     enc1_ = new TIM3Encoder;
00008     
00009     //Initialize BNO055
00010     wait(0.3);
00011     if (imu_->chip_ready() == 0){
00012         do {
00013             wait(0.1);
00014         } while(imu_->reset());
00015     }
00016     
00017     offset_angle_ = 0;
00018     enable_oled_ = true;
00019     
00020     //Set TIM IT Callback
00021     it_tick = new Ticker;
00022     it_tick->attach(this, &Odom::update_it_cb, 0.02);
00023 }
00024     
00025 void Odom::calc_odometry(){
00026     Vec3f velocity;
00027     Vec3f world_velocity;
00028         
00029     velocity.x((enc0_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f)));
00030     velocity.y((enc1_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f)));
00031     
00032     world_velocity.x(((velocity.x() * cosf(odom_.angle() + initialpose_.angle() - H_PI)) - (velocity.y() * sinf(odom_.angle() + initialpose_.angle() - H_PI))));
00033     world_velocity.y(((velocity.x() * sinf(odom_.angle() + initialpose_.angle() - H_PI)) + (velocity.y() * cosf(odom_.angle() + initialpose_.angle() - H_PI))));
00034         
00035     odom_.x(odom_.x() + world_velocity.x() * 0.02f);
00036     odom_.y(odom_.y() + world_velocity.y() * 0.02f);   
00037 }
00038 
00039 void Odom::update_angle(){
00040     static float prev_angle;
00041     static int rotation_flag;//180回転ごとに記録
00042     
00043     Vec3f angle = imu_->get_radian();
00044     float z_angle = angle.z();
00045 
00046     if(prev_angle < -H_PI && z_angle > H_PI){
00047         rotation_flag--;
00048     }else if(prev_angle > H_PI && z_angle < -H_PI){
00049         rotation_flag++;   
00050     }else if(rotation_flag && 0 < prev_angle && prev_angle < H_PI && -H_PI < z_angle && z_angle < 0){
00051         rotation_flag--;
00052     }else if(rotation_flag && -H_PI < prev_angle && prev_angle < 0 && 0 < z_angle && z_angle < H_PI){
00053         rotation_flag++;
00054     }
00055     
00056     if(!rotation_flag){
00057         actual_angle_ = z_angle;
00058     }else if(ABS(rotation_flag) % 2){
00059         actual_angle_ = -((rotation_flag > 0 ? -PI : PI) - z_angle) + (rotation_flag * PI);        
00060     }else{
00061         actual_angle_ = z_angle + (rotation_flag * PI);
00062     }
00063     
00064     //オフセットを考慮して角度を設定
00065     odom_.angle(actual_angle_ - offset_angle_);
00066     
00067     //値更新
00068     prev_angle = z_angle; 
00069 }
00070