Diff: odometry/odom.h

Revision:

5:e678f1ac6cdc

Parent:

4:cf1a4e503974

Child:

6:20a32baeff79

--- a/odometry/odom.h	Wed Dec 12 03:35:52 2018 +0000
+++ b/odometry/odom.h	Tue Dec 18 21:00:30 2018 +0000
@@ -27,7 +27,7 @@
         My_BNO055 *imu_;
         TIM2Encoder *enc0_;
         TIM3Encoder *enc1_; 
-
+        
         float offset_angle_;
         float actual_angle_;
 
@@ -56,6 +56,15 @@
         //Update world angle
         void update_angle();
         
+        void reset_world(){
+            world_ = Vec3f(0, 0, 0);   
+        }
+        
+        //現在角度を0度に設定
+        void set_offset_angle_zero(){
+            offset_angle_ = -actual_angle_;
+        }
+                
         //Getter
         bool get_court_color(){
             return court_color_;   
@@ -106,8 +115,9 @@
             odom_ = odom;   
         }
         
-        void set_initial_pose(Vec3f initialpose){
-            odom_->set_initialpose(initialpose);
+        void reset_pose(){
+            odom_->reset_world(); 
+            odom_->set_offset_angle_zero();  
         }
         
         //Getter
@@ -118,13 +128,16 @@
             return odom_->get_initialpose();
         }
         Vec3f get_pose(){
-            return odom_->get_world();   
+            return odom_->get_world() + odom_->get_initialpose() + odom_->get_drift();   
         }
         Vec3f get_drift(){
             return odom_->get_drift();
         }
         
         //Setter
+        void set_initial_pose(Vec3f initialpose){
+            odom_->set_initialpose(initialpose);
+        }
         void set_court_color(bool court_color){
             odom_->set_court_color(court_color);   
         }