aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
Diff: odometry/odom.h
- Revision:
- 3:a45557a0dcb8
- Parent:
- 2:086272a2da1c
- Child:
- 4:cf1a4e503974
diff -r 086272a2da1c -r a45557a0dcb8 odometry/odom.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/odometry/odom.h Tue Dec 11 17:51:47 2018 +0000 @@ -0,0 +1,121 @@ +#ifndef _ODOM_H_ +#define _ODOM_H_ + +#include "mbed.h" +#include "encoder.h" +#include "myBNO055.h" +#include "type.h" + +#define PI (3.14159265f) +#define H_PI (1.57079632f) +#define ENCODER_WHEEL_DIAMETER (0.0508f) +#define EDGE_EVALUATION (4.0f) +#define PULSE_ENCODER (500.0f) +#define ENCODER_COEFFICIENT ((ENCODER_WHEEL_DIAMETER * PI) / (EDGE_EVALUATION * PULSE_ENCODER)) + +#define DEG2RAD(x) (((x) * 3.1416f) / 180.0f) +#define RAD2DEG(x) (((x) / 3.1416f) * 180.0f) +#define ABS(x) (((x) > 0) ? (x) : -(x)) + +class Odom +{ + private: + //class instance + My_BNO055 *imu_; + TIM2Encoder *enc0_; + TIM3Encoder *enc1_; + + float offset_angle_; + float actual_angle_; + + Vec3f world_; + Vec3f velocity_; + Vec3f world_velocity_; + + //For amcl + Vec3f initialpose_; + Vec3f drift_; + + //For TIM IT + Ticker *it_tick; + + public: + //Constructor + Odom(){ + initialize(); + } + //TIM IT Callback function + void update_it_cb(){ + calc_odometry(); + } + + //Update world angle + void update_angle(); + + //Getter + Vec3f get_world(){ + return world_; + } + Vec3f get_initialpose(){ + return initialpose_; + } + Vec3f get_drift(){ + return drift_; + } + + //Setter + void set_initialpose(Vec3f initialpose){ + initialpose_ = initialpose; + } + void set_drift(Vec3f drift){ + drift_ = drift; + } + + private: + void initialize(); + void calc_odometry(); +}; + +class Odom_Abstract +{ + private: + Odom *odom_; + + public: + Odom_Abstract(){ + + } + + Odom_Abstract(Odom *odom) : odom_(odom){ + + } + + virtual void loop() = 0; + + protected: + void set_instance(Odom* odom){ + odom_ = odom; + } + + void set_initial_pose(Vec3f initialpose){ + odom_->set_initialpose(initialpose); + } + + Vec3f get_initial_pose(){ + return odom_->get_initialpose(); + } + + Vec3f get_pose(){ + return odom_->get_world(); + } + + void set_drift(Vec3f drift){ + odom_->set_drift(drift); + } + + Vec3f get_drift(){ + return odom_->get_drift(); + } + +}; +#endif \ No newline at end of file