Diff: odometry/odom.h

Revision:

11:375d474b8522

Parent:

10:3b47050b1652

--- a/odometry/odom.h	Fri Mar 22 12:29:18 2019 +0000
+++ b/odometry/odom.h	Fri Aug 30 07:53:21 2019 +0000
@@ -5,10 +5,11 @@
 #include "encoder.h"
 #include "myBNO055.h"
 #include "type.h"
+#include "math.h"
 
 #define PI                          (3.14159265f)
 #define H_PI                        (1.57079632f)
-#define ENCODER_WHEEL_DIAMETER      (0.0508f)
+#define ENCODER_WHEEL_DIAMETER      (0.054f)
 #define EDGE_EVALUATION             (4.0f)
 #define PULSE_ENCODER               (500.0f)
 #define ENCODER_COEFFICIENT         ((ENCODER_WHEEL_DIAMETER * PI) / (EDGE_EVALUATION * PULSE_ENCODER))
@@ -157,7 +158,13 @@
             return get_initial_pose() + get_odom();
         }
         Vec3f get_world(){
-            return get_pose() + get_drift();   
+//            return get_pose() + get_drift();   
+            Vec3f ret;
+            float t = get_drift().angle();
+            ret.x(get_pose().x() * cos(t) - get_pose().y() * sin(t) + get_drift().x());
+            ret.y(get_pose().x() * sin(t) + get_pose().y() * cos(t) + get_drift().y());
+            ret.angle(get_pose().angle() + t); 
+            return ret;
         }
         
         bool get_enable_oled(){