aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
odometry/odom.cpp
- Committer:
- nakedt555
- Date:
- 2019-08-30
- Revision:
- 12:f726eb78b54c
- Parent:
- 10:3b47050b1652
File content as of revision 12:f726eb78b54c:
#include "odom.h" void Odom::initialize(){ //Create instance imu_ = new My_BNO055(PB_4, PA_8, PB_10, 0x29 << 1, MODE_NDOF); enc0_ = new TIM2Encoder; enc1_ = new TIM3Encoder; //Initialize BNO055 wait(0.3); if (imu_->chip_ready() == 0){ do { wait(0.1); } while(imu_->reset()); } offset_angle_ = 0; enable_oled_ = true; //Set TIM IT Callback it_tick = new Ticker; it_tick->attach(this, &Odom::update_it_cb, 0.02); } void Odom::calc_odometry(){ Vec3f velocity; Vec3f world_velocity; velocity.x((enc0_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f))); velocity.y((enc1_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f))); world_velocity.x(((velocity.x() * cosf(odom_.angle() + initialpose_.angle() - H_PI)) - (velocity.y() * sinf(odom_.angle() + initialpose_.angle() - H_PI)))); world_velocity.y(((velocity.x() * sinf(odom_.angle() + initialpose_.angle() - H_PI)) + (velocity.y() * cosf(odom_.angle() + initialpose_.angle() - H_PI)))); odom_.x(odom_.x() + world_velocity.x() * 0.02f); odom_.y(odom_.y() + world_velocity.y() * 0.02f); } void Odom::update_angle(){ static float prev_angle; static int rotation_flag;//180回転ごとに記録 Vec3f angle = imu_->get_radian(); float z_angle = angle.z(); if(prev_angle < -H_PI && z_angle > H_PI){ rotation_flag--; }else if(prev_angle > H_PI && z_angle < -H_PI){ rotation_flag++; }else if(rotation_flag && 0 < prev_angle && prev_angle < H_PI && -H_PI < z_angle && z_angle < 0){ rotation_flag--; }else if(rotation_flag && -H_PI < prev_angle && prev_angle < 0 && 0 < z_angle && z_angle < H_PI){ rotation_flag++; } if(!rotation_flag){ actual_angle_ = z_angle; }else if(ABS(rotation_flag) % 2){ actual_angle_ = -((rotation_flag > 0 ? -PI : PI) - z_angle) + (rotation_flag * PI); }else{ actual_angle_ = z_angle + (rotation_flag * PI); } //オフセットを考慮して角度を設定 odom_.angle(actual_angle_ - offset_angle_); //値更新 prev_angle = z_angle; }