aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
odometry/odom.h
- Committer:
- nakedt555
- Date:
- 2018-12-18
- Revision:
- 6:20a32baeff79
- Parent:
- 5:e678f1ac6cdc
- Child:
- 8:80708bacb5b5
File content as of revision 6:20a32baeff79:
#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)) #define COURT_RED false #define COURT_BLUE true 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 bool court_color_; 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(); 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_; } Vec3f get_world(){ return world_; } Vec3f get_initialpose(){ return initialpose_; } Vec3f get_drift(){ return drift_; } //Setter void set_court_color(bool court_color){ court_color_ = court_color; } void set_initialpose(Vec3f initialpose){ initialpose_ = initialpose; } void set_world(Vec3f world){ world_ = world; } void set_drift(Vec3f drift){ drift_ = drift; } private: void initialize(); void calc_odometry(); }; class Odom_Abstract { private: Odom *odom_; public: Odom_Abstract(Odom *odom) : odom_(odom){ } virtual void loop() = 0; protected: void set_instance(Odom* odom){ odom_ = odom; } void reset_pose(){ odom_->reset_world(); odom_->set_offset_angle_zero(); } //Getter bool get_court_color(){ return odom_->get_court_color(); } Vec3f get_initial_pose(){ return odom_->get_initialpose(); } Vec3f get_pose(){ 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); } void set_pose(Vec3f pose){ odom_->set_world(pose); } void set_drift(Vec3f drift){ odom_->set_drift(drift); } }; #endif