Diff: odometry/odom.cpp

Revision:

8:80708bacb5b5

Parent:

6:20a32baeff79

Child:

10:3b47050b1652

--- a/odometry/odom.cpp	Tue Dec 18 22:06:34 2018 +0000
+++ b/odometry/odom.cpp	Thu Dec 20 20:54:35 2018 +0000
@@ -15,6 +15,7 @@
     }
     
     offset_angle_ = 0;
+    enable_oled_ = true;
     
     //Set TIM IT Callback
     it_tick = new Ticker;
@@ -22,14 +23,17 @@
 }
     
 void Odom::calc_odometry(){
-    velocity_.x((enc0_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f)));
-    velocity_.y((enc1_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f)));
+    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(-world_.angle())) - (velocity_.y() * sinf(-world_.angle()))));
-    world_velocity_.y(((velocity_.x() * sinf(-world_.angle())) + (velocity_.y() * cosf(-world_.angle()))));
+    world_velocity.x(((velocity.x() * cosf(-odom_.angle())) - (velocity.y() * sinf(-odom_.angle()))));
+    world_velocity.y(((velocity.x() * sinf(-odom_.angle())) + (velocity.y() * cosf(-odom_.angle()))));
       
-    world_.x(world_.x() + world_velocity_.x() * 0.02f);
-    world_.y(world_.y() + world_velocity_.y() * 0.02f);   
+    odom_.x(odom_.x() + world_velocity.x() * 0.02f);
+    odom_.y(odom_.y() + world_velocity.y() * 0.02f);   
 }
 
 void Odom::update_angle(){
@@ -58,7 +62,7 @@
     }
     
     //オフセットを考慮して角度を設定
-    world_.angle(actual_angle_ - offset_angle_);
+    odom_.angle(actual_angle_ - offset_angle_);
     
     //値更新
     prev_angle = z_angle;