aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
odometry/odom.h
- Committer:
- nakedt555
- Date:
- 2018-12-20
- Revision:
- 8:80708bacb5b5
- Parent:
- 6:20a32baeff79
- Child:
- 10:3b47050b1652
File content as of revision 8:80708bacb5b5:
#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 odom_; //For ROS bool court_color_; Vec3f initialpose_; Vec3f drift_; //For TIM IT Ticker *it_tick; //For OLED bool enable_oled_; public: //Constructor Odom(){ initialize(); } //TIM IT Callback function void update_it_cb(){ calc_odometry(); } //Update world angle void update_angle(); void reset_odom(){ odom_.x(0.0); odom_.y(0.0); odom_.angle(0.0); } void reset_drift(){ drift_.x(0.0); drift_.y(0.0); drift_.angle(0.0); } //現在角度を0度に設定 void set_offset_angle_zero(){ offset_angle_ = actual_angle_; } //Getter bool get_court_color(){ return court_color_; } Vec3f get_odom(){ return odom_; } Vec3f get_initialpose(){ return initialpose_; } Vec3f get_drift(){ return drift_; } bool get_enable_oled(){ return enable_oled_; } //Setter void set_court_color(bool court_color){ court_color_ = court_color; } void set_initialpose(Vec3f initialpose){ initialpose_ = initialpose; } void set_drift(Vec3f drift){ drift_ = drift; } void set_enable_oled(bool enable_oled){ enable_oled_ = enable_oled; } 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_odometry(){ odom_->reset_odom(); odom_->reset_drift(); 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_odom(){ return odom_->get_odom(); } Vec3f get_drift(){ return odom_->get_drift(); } Vec3f get_pose(){ return get_initial_pose() + get_odom(); } Vec3f get_world(){ return get_pose() + get_drift(); } bool get_enable_oled(){ return odom_->get_enable_oled(); } //Setter void set_court_color(bool court_color){ odom_->set_court_color(court_color); } void set_initial_pose(Vec3f initialpose){ odom_->set_initialpose(initialpose);//CANから受け取った初期位置の情報を格納するためのメソッド } void set_drift(Vec3f drift){ odom_->set_drift(drift);//ROSからサブスクライブしたデータを格納するためのメソッド } void set_enable_oled(bool enable_oled){ odom_->set_enable_oled(enable_oled); } }; #endif