Diff: odometry/odom.cpp

Revision:

3:a45557a0dcb8

Parent:

2:086272a2da1c

Child:

5:e678f1ac6cdc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/odometry/odom.cpp	Tue Dec 11 17:51:47 2018 +0000
@@ -0,0 +1,66 @@
+#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;
+    
+    //Set TIM IT Callback
+    it_tick = new Ticker;
+    it_tick->attach(this, &Odom::update_it_cb, 0.02);
+}
+    
+void Odom::calc_odometry(){
+    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(-world_.angle())) - (velocity_.y() * sinf(-world_.angle()))));
+    world_velocity_.y(((velocity_.x() * sinf(-world_.angle())) + (velocity_.y() * cosf(-world_.angle()))));
+      
+    world_.x(world_.x() + world_velocity_.x() * 0.02f);
+    world_.y(world_.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);
+    }
+    
+    //オフセットを考慮して角度を設定
+    world_.angle(actual_angle_ + offset_angle_);
+    
+    //値更新
+    prev_angle = z_angle; 
+}
+    
\ No newline at end of file