aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
odometry/odom.cpp@3:a45557a0dcb8, 2018-12-11 (annotated)
- Committer:
- nakedt555
- Date:
- Tue Dec 11 17:51:47 2018 +0000
- Revision:
- 3:a45557a0dcb8
- Parent:
- odom.cpp@2:086272a2da1c
- Child:
- 5:e678f1ac6cdc
12/10iikanji
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nakedt555 | 1:bdd17feaa4ce | 1 | #include "odom.h" |
nakedt555 | 2:086272a2da1c | 2 | |
nakedt555 | 2:086272a2da1c | 3 | void Odom::initialize(){ |
nakedt555 | 2:086272a2da1c | 4 | //Create instance |
nakedt555 | 2:086272a2da1c | 5 | imu_ = new My_BNO055(PB_4, PA_8, PB_10, 0x29 << 1, MODE_NDOF); |
nakedt555 | 2:086272a2da1c | 6 | enc0_ = new TIM2Encoder; |
nakedt555 | 2:086272a2da1c | 7 | enc1_ = new TIM3Encoder; |
nakedt555 | 2:086272a2da1c | 8 | |
nakedt555 | 2:086272a2da1c | 9 | //Initialize BNO055 |
nakedt555 | 2:086272a2da1c | 10 | wait(0.3); |
nakedt555 | 2:086272a2da1c | 11 | if (imu_->chip_ready() == 0){ |
nakedt555 | 2:086272a2da1c | 12 | do { |
nakedt555 | 2:086272a2da1c | 13 | wait(0.1); |
nakedt555 | 2:086272a2da1c | 14 | } while(imu_->reset()); |
nakedt555 | 2:086272a2da1c | 15 | } |
nakedt555 | 2:086272a2da1c | 16 | |
nakedt555 | 2:086272a2da1c | 17 | offset_angle_ = 0; |
nakedt555 | 3:a45557a0dcb8 | 18 | |
nakedt555 | 3:a45557a0dcb8 | 19 | //Set TIM IT Callback |
nakedt555 | 3:a45557a0dcb8 | 20 | it_tick = new Ticker; |
nakedt555 | 3:a45557a0dcb8 | 21 | it_tick->attach(this, &Odom::update_it_cb, 0.02); |
nakedt555 | 2:086272a2da1c | 22 | } |
nakedt555 | 2:086272a2da1c | 23 | |
nakedt555 | 2:086272a2da1c | 24 | void Odom::calc_odometry(){ |
nakedt555 | 2:086272a2da1c | 25 | velocity_.x((enc0_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f))); |
nakedt555 | 2:086272a2da1c | 26 | velocity_.y((enc1_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f))); |
nakedt555 | 2:086272a2da1c | 27 | |
nakedt555 | 2:086272a2da1c | 28 | world_velocity_.x(((velocity_.x() * cosf(-world_.angle())) - (velocity_.y() * sinf(-world_.angle())))); |
nakedt555 | 2:086272a2da1c | 29 | world_velocity_.y(((velocity_.x() * sinf(-world_.angle())) + (velocity_.y() * cosf(-world_.angle())))); |
nakedt555 | 2:086272a2da1c | 30 | |
nakedt555 | 2:086272a2da1c | 31 | world_.x(world_.x() + world_velocity_.x() * 0.02f); |
nakedt555 | 2:086272a2da1c | 32 | world_.y(world_.y() + world_velocity_.y() * 0.02f); |
nakedt555 | 2:086272a2da1c | 33 | } |
nakedt555 | 2:086272a2da1c | 34 | |
nakedt555 | 2:086272a2da1c | 35 | void Odom::update_angle(){ |
nakedt555 | 2:086272a2da1c | 36 | static float prev_angle; |
nakedt555 | 2:086272a2da1c | 37 | static int rotation_flag;//180回転ごとに記録 |
nakedt555 | 2:086272a2da1c | 38 | |
nakedt555 | 3:a45557a0dcb8 | 39 | Vec3f angle = imu_->get_radian(); |
nakedt555 | 3:a45557a0dcb8 | 40 | float z_angle = angle.z(); |
nakedt555 | 2:086272a2da1c | 41 | |
nakedt555 | 2:086272a2da1c | 42 | if(prev_angle < -H_PI && z_angle > H_PI){ |
nakedt555 | 2:086272a2da1c | 43 | rotation_flag--; |
nakedt555 | 2:086272a2da1c | 44 | }else if(prev_angle > H_PI && z_angle < -H_PI){ |
nakedt555 | 2:086272a2da1c | 45 | rotation_flag++; |
nakedt555 | 2:086272a2da1c | 46 | }else if(rotation_flag && 0 < prev_angle && prev_angle < H_PI && -H_PI < z_angle && z_angle < 0){ |
nakedt555 | 2:086272a2da1c | 47 | rotation_flag--; |
nakedt555 | 2:086272a2da1c | 48 | }else if(rotation_flag && -H_PI < prev_angle && prev_angle < 0 && 0 < z_angle && z_angle < H_PI){ |
nakedt555 | 2:086272a2da1c | 49 | rotation_flag++; |
nakedt555 | 2:086272a2da1c | 50 | } |
nakedt555 | 2:086272a2da1c | 51 | |
nakedt555 | 2:086272a2da1c | 52 | if(!rotation_flag){ |
nakedt555 | 2:086272a2da1c | 53 | actual_angle_ = z_angle; |
nakedt555 | 2:086272a2da1c | 54 | }else if(ABS(rotation_flag) % 2){ |
nakedt555 | 2:086272a2da1c | 55 | actual_angle_ = -((rotation_flag > 0 ? -PI : PI) - z_angle) + (rotation_flag * PI); |
nakedt555 | 2:086272a2da1c | 56 | }else{ |
nakedt555 | 2:086272a2da1c | 57 | actual_angle_ = z_angle + (rotation_flag * PI); |
nakedt555 | 2:086272a2da1c | 58 | } |
nakedt555 | 2:086272a2da1c | 59 | |
nakedt555 | 2:086272a2da1c | 60 | //オフセットを考慮して角度を設定 |
nakedt555 | 2:086272a2da1c | 61 | world_.angle(actual_angle_ + offset_angle_); |
nakedt555 | 2:086272a2da1c | 62 | |
nakedt555 | 2:086272a2da1c | 63 | //値更新 |
nakedt555 | 2:086272a2da1c | 64 | prev_angle = z_angle; |
nakedt555 | 2:086272a2da1c | 65 | } |
nakedt555 | 2:086272a2da1c | 66 |