駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

odometry/odom.cpp@12:f726eb78b54c, 2019-08-30 (annotated)

Committer:
nakedt555
Date:
Fri Aug 30 07:55:38 2019 +0000
Revision:
12:f726eb78b54c
Parent:
10:3b47050b1652 
aaa

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 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 10:3b47050b1652 32 world_velocity.x(((velocity.x() * cosf(odom_.angle() + initialpose_.angle() - H_PI)) - (velocity.y() * sinf(odom_.angle() + initialpose_.angle() - H_PI))));
nakedt555 10:3b47050b1652 33 world_velocity.y(((velocity.x() * sinf(odom_.angle() + initialpose_.angle() - H_PI)) + (velocity.y() * cosf(odom_.angle() + initialpose_.angle() - H_PI))));
nakedt555 10:3b47050b1652 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