駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

odometry/odom.cpp@6:20a32baeff79, 2018-12-18 (annotated)

Committer:
nakedt555
Date:
Tue Dec 18 21:51:13 2018 +0000
Revision:
6:20a32baeff79
Parent:
5:e678f1ac6cdc
Child:
8:80708bacb5b5
kita

Who changed what in which revision?

UserRevisionLine numberNew 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 6:20a32baeff79 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