aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: odometry/odom.h
- Revision:
- 11:375d474b8522
- Parent:
- 10:3b47050b1652
diff -r 3b47050b1652 -r 375d474b8522 odometry/odom.h --- 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(){