![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: odom.cpp
- Revision:
- 2:086272a2da1c
- Parent:
- 1:bdd17feaa4ce
diff -r bdd17feaa4ce -r 086272a2da1c odom.cpp --- a/odom.cpp Fri Dec 07 20:59:56 2018 +0000 +++ b/odom.cpp Fri Dec 07 22:34:31 2018 +0000 @@ -1,1 +1,62 @@ #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; +} + \ No newline at end of file