駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

odom.cpp@2:086272a2da1c, 2018-12-07 (annotated)

Committer:
nakedt555
Date:
Fri Dec 07 22:34:31 2018 +0000
Revision:
2:086272a2da1c
Parent:
1:bdd17feaa4ce 
12/08 work log;

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 2:086272a2da1c 18 }
nakedt555 2:086272a2da1c 19
nakedt555 2:086272a2da1c 20 void Odom::calc_odometry(){
nakedt555 2:086272a2da1c 21 velocity_.x((enc0_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f)));
nakedt555 2:086272a2da1c 22 velocity_.y((enc1_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f)));
nakedt555 2:086272a2da1c 23
nakedt555 2:086272a2da1c 24 world_velocity_.x(((velocity_.x() * cosf(-world_.angle())) - (velocity_.y() * sinf(-world_.angle()))));
nakedt555 2:086272a2da1c 25 world_velocity_.y(((velocity_.x() * sinf(-world_.angle())) + (velocity_.y() * cosf(-world_.angle()))));
nakedt555 2:086272a2da1c 26
nakedt555 2:086272a2da1c 27 world_.x(world_.x() + world_velocity_.x() * 0.02f);
nakedt555 2:086272a2da1c 28 world_.y(world_.y() + world_velocity_.y() * 0.02f);
nakedt555 2:086272a2da1c 29 }
nakedt555 2:086272a2da1c 30
nakedt555 2:086272a2da1c 31 void Odom::update_angle(){
nakedt555 2:086272a2da1c 32 static float prev_angle;
nakedt555 2:086272a2da1c 33 static int rotation_flag;//180回転ごとに記録
nakedt555 2:086272a2da1c 34
nakedt555 2:086272a2da1c 35 Vec3f angle = imu_->get_angle();
nakedt555 2:086272a2da1c 36 float z_angle = angle.z() * (-1);
nakedt555 2:086272a2da1c 37
nakedt555 2:086272a2da1c 38 if(prev_angle < -H_PI && z_angle > H_PI){
nakedt555 2:086272a2da1c 39 rotation_flag--;
nakedt555 2:086272a2da1c 40 }else if(prev_angle > H_PI && z_angle < -H_PI){
nakedt555 2:086272a2da1c 41 rotation_flag++;
nakedt555 2:086272a2da1c 42 }else if(rotation_flag && 0 < prev_angle && prev_angle < H_PI && -H_PI < z_angle && z_angle < 0){
nakedt555 2:086272a2da1c 43 rotation_flag--;
nakedt555 2:086272a2da1c 44 }else if(rotation_flag && -H_PI < prev_angle && prev_angle < 0 && 0 < z_angle && z_angle < H_PI){
nakedt555 2:086272a2da1c 45 rotation_flag++;
nakedt555 2:086272a2da1c 46 }
nakedt555 2:086272a2da1c 47
nakedt555 2:086272a2da1c 48 if(!rotation_flag){
nakedt555 2:086272a2da1c 49 actual_angle_ = z_angle;
nakedt555 2:086272a2da1c 50 }else if(ABS(rotation_flag) % 2){
nakedt555 2:086272a2da1c 51 actual_angle_ = -((rotation_flag > 0 ? -PI : PI) - z_angle) + (rotation_flag * PI);
nakedt555 2:086272a2da1c 52 }else{
nakedt555 2:086272a2da1c 53 actual_angle_ = z_angle + (rotation_flag * PI);
nakedt555 2:086272a2da1c 54 }
nakedt555 2:086272a2da1c 55
nakedt555 2:086272a2da1c 56 //オフセットを考慮して角度を設定
nakedt555 2:086272a2da1c 57 world_.angle(actual_angle_ + offset_angle_);
nakedt555 2:086272a2da1c 58
nakedt555 2:086272a2da1c 59 //値更新
nakedt555 2:086272a2da1c 60 prev_angle = z_angle;
nakedt555 2:086272a2da1c 61 }
nakedt555 2:086272a2da1c 62