aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: odometry/odom.cpp
- Revision:
- 3:a45557a0dcb8
- Parent:
- 2:086272a2da1c
- Child:
- 5:e678f1ac6cdc
diff -r 086272a2da1c -r a45557a0dcb8 odometry/odom.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/odometry/odom.cpp Tue Dec 11 17:51:47 2018 +0000 @@ -0,0 +1,66 @@ +#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; + + //Set TIM IT Callback + it_tick = new Ticker; + it_tick->attach(this, &Odom::update_it_cb, 0.02); +} + +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_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); + } + + //オフセットを考慮して角度を設定 + world_.angle(actual_angle_ + offset_angle_); + + //値更新 + prev_angle = z_angle; +} + \ No newline at end of file