aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
odometry/odom.cpp@8:80708bacb5b5, 2018-12-20 (annotated)
- Committer:
- nakedt555
- Date:
- Thu Dec 20 20:54:35 2018 +0000
- Revision:
- 8:80708bacb5b5
- Parent:
- 6:20a32baeff79
- Child:
- 10:3b47050b1652
amcl kakunin
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 | 8:80708bacb5b5 | 18 | enable_oled_ = true; |
nakedt555 | 3:a45557a0dcb8 | 19 | |
nakedt555 | 3:a45557a0dcb8 | 20 | //Set TIM IT Callback |
nakedt555 | 3:a45557a0dcb8 | 21 | it_tick = new Ticker; |
nakedt555 | 3:a45557a0dcb8 | 22 | it_tick->attach(this, &Odom::update_it_cb, 0.02); |
nakedt555 | 2:086272a2da1c | 23 | } |
nakedt555 | 2:086272a2da1c | 24 | |
nakedt555 | 2:086272a2da1c | 25 | void Odom::calc_odometry(){ |
nakedt555 | 8:80708bacb5b5 | 26 | Vec3f velocity; |
nakedt555 | 8:80708bacb5b5 | 27 | Vec3f world_velocity; |
nakedt555 | 8:80708bacb5b5 | 28 | |
nakedt555 | 8:80708bacb5b5 | 29 | velocity.x((enc0_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f))); |
nakedt555 | 8:80708bacb5b5 | 30 | velocity.y((enc1_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f))); |
nakedt555 | 2:086272a2da1c | 31 | |
nakedt555 | 8:80708bacb5b5 | 32 | world_velocity.x(((velocity.x() * cosf(-odom_.angle())) - (velocity.y() * sinf(-odom_.angle())))); |
nakedt555 | 8:80708bacb5b5 | 33 | world_velocity.y(((velocity.x() * sinf(-odom_.angle())) + (velocity.y() * cosf(-odom_.angle())))); |
nakedt555 | 2:086272a2da1c | 34 | |
nakedt555 | 8:80708bacb5b5 | 35 | odom_.x(odom_.x() + world_velocity.x() * 0.02f); |
nakedt555 | 8:80708bacb5b5 | 36 | odom_.y(odom_.y() + world_velocity.y() * 0.02f); |
nakedt555 | 2:086272a2da1c | 37 | } |
nakedt555 | 2:086272a2da1c | 38 | |
nakedt555 | 2:086272a2da1c | 39 | void Odom::update_angle(){ |
nakedt555 | 2:086272a2da1c | 40 | static float prev_angle; |
nakedt555 | 2:086272a2da1c | 41 | static int rotation_flag;//180回転ごとに記録 |
nakedt555 | 2:086272a2da1c | 42 | |
nakedt555 | 3:a45557a0dcb8 | 43 | Vec3f angle = imu_->get_radian(); |
nakedt555 | 3:a45557a0dcb8 | 44 | float z_angle = angle.z(); |
nakedt555 | 2:086272a2da1c | 45 | |
nakedt555 | 2:086272a2da1c | 46 | if(prev_angle < -H_PI && z_angle > H_PI){ |
nakedt555 | 2:086272a2da1c | 47 | rotation_flag--; |
nakedt555 | 2:086272a2da1c | 48 | }else if(prev_angle > H_PI && z_angle < -H_PI){ |
nakedt555 | 2:086272a2da1c | 49 | rotation_flag++; |
nakedt555 | 2:086272a2da1c | 50 | }else if(rotation_flag && 0 < prev_angle && prev_angle < H_PI && -H_PI < z_angle && z_angle < 0){ |
nakedt555 | 2:086272a2da1c | 51 | rotation_flag--; |
nakedt555 | 2:086272a2da1c | 52 | }else if(rotation_flag && -H_PI < prev_angle && prev_angle < 0 && 0 < z_angle && z_angle < H_PI){ |
nakedt555 | 2:086272a2da1c | 53 | rotation_flag++; |
nakedt555 | 2:086272a2da1c | 54 | } |
nakedt555 | 2:086272a2da1c | 55 | |
nakedt555 | 2:086272a2da1c | 56 | if(!rotation_flag){ |
nakedt555 | 2:086272a2da1c | 57 | actual_angle_ = z_angle; |
nakedt555 | 2:086272a2da1c | 58 | }else if(ABS(rotation_flag) % 2){ |
nakedt555 | 2:086272a2da1c | 59 | actual_angle_ = -((rotation_flag > 0 ? -PI : PI) - z_angle) + (rotation_flag * PI); |
nakedt555 | 2:086272a2da1c | 60 | }else{ |
nakedt555 | 2:086272a2da1c | 61 | actual_angle_ = z_angle + (rotation_flag * PI); |
nakedt555 | 2:086272a2da1c | 62 | } |
nakedt555 | 2:086272a2da1c | 63 | |
nakedt555 | 2:086272a2da1c | 64 | //オフセットを考慮して角度を設定 |
nakedt555 | 8:80708bacb5b5 | 65 | odom_.angle(actual_angle_ - offset_angle_); |
nakedt555 | 2:086272a2da1c | 66 | |
nakedt555 | 2:086272a2da1c | 67 | //値更新 |
nakedt555 | 2:086272a2da1c | 68 | prev_angle = z_angle; |
nakedt555 | 2:086272a2da1c | 69 | } |
nakedt555 | 2:086272a2da1c | 70 |