aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
odometry/odom.h@6:20a32baeff79, 2018-12-18 (annotated)
- Committer:
- nakedt555
- Date:
- Tue Dec 18 21:51:13 2018 +0000
- Revision:
- 6:20a32baeff79
- Parent:
- 5:e678f1ac6cdc
- Child:
- 8:80708bacb5b5
kita
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nakedt555 | 1:bdd17feaa4ce | 1 | #ifndef _ODOM_H_ |
nakedt555 | 1:bdd17feaa4ce | 2 | #define _ODOM_H_ |
nakedt555 | 1:bdd17feaa4ce | 3 | |
nakedt555 | 1:bdd17feaa4ce | 4 | #include "mbed.h" |
nakedt555 | 2:086272a2da1c | 5 | #include "encoder.h" |
nakedt555 | 2:086272a2da1c | 6 | #include "myBNO055.h" |
nakedt555 | 1:bdd17feaa4ce | 7 | #include "type.h" |
nakedt555 | 2:086272a2da1c | 8 | |
nakedt555 | 2:086272a2da1c | 9 | #define PI (3.14159265f) |
nakedt555 | 2:086272a2da1c | 10 | #define H_PI (1.57079632f) |
nakedt555 | 2:086272a2da1c | 11 | #define ENCODER_WHEEL_DIAMETER (0.0508f) |
nakedt555 | 2:086272a2da1c | 12 | #define EDGE_EVALUATION (4.0f) |
nakedt555 | 2:086272a2da1c | 13 | #define PULSE_ENCODER (500.0f) |
nakedt555 | 2:086272a2da1c | 14 | #define ENCODER_COEFFICIENT ((ENCODER_WHEEL_DIAMETER * PI) / (EDGE_EVALUATION * PULSE_ENCODER)) |
nakedt555 | 2:086272a2da1c | 15 | |
nakedt555 | 2:086272a2da1c | 16 | #define DEG2RAD(x) (((x) * 3.1416f) / 180.0f) |
nakedt555 | 2:086272a2da1c | 17 | #define RAD2DEG(x) (((x) / 3.1416f) * 180.0f) |
nakedt555 | 2:086272a2da1c | 18 | #define ABS(x) (((x) > 0) ? (x) : -(x)) |
nakedt555 | 1:bdd17feaa4ce | 19 | |
nakedt555 | 4:cf1a4e503974 | 20 | #define COURT_RED false |
nakedt555 | 4:cf1a4e503974 | 21 | #define COURT_BLUE true |
nakedt555 | 4:cf1a4e503974 | 22 | |
nakedt555 | 1:bdd17feaa4ce | 23 | class Odom |
nakedt555 | 1:bdd17feaa4ce | 24 | { |
nakedt555 | 1:bdd17feaa4ce | 25 | private: |
nakedt555 | 2:086272a2da1c | 26 | //class instance |
nakedt555 | 2:086272a2da1c | 27 | My_BNO055 *imu_; |
nakedt555 | 2:086272a2da1c | 28 | TIM2Encoder *enc0_; |
nakedt555 | 2:086272a2da1c | 29 | TIM3Encoder *enc1_; |
nakedt555 | 5:e678f1ac6cdc | 30 | |
nakedt555 | 2:086272a2da1c | 31 | float offset_angle_; |
nakedt555 | 2:086272a2da1c | 32 | float actual_angle_; |
nakedt555 | 2:086272a2da1c | 33 | |
nakedt555 | 2:086272a2da1c | 34 | Vec3f world_; |
nakedt555 | 2:086272a2da1c | 35 | Vec3f velocity_; |
nakedt555 | 2:086272a2da1c | 36 | Vec3f world_velocity_; |
nakedt555 | 2:086272a2da1c | 37 | |
nakedt555 | 3:a45557a0dcb8 | 38 | //For amcl |
nakedt555 | 4:cf1a4e503974 | 39 | bool court_color_; |
nakedt555 | 3:a45557a0dcb8 | 40 | Vec3f initialpose_; |
nakedt555 | 3:a45557a0dcb8 | 41 | Vec3f drift_; |
nakedt555 | 3:a45557a0dcb8 | 42 | |
nakedt555 | 3:a45557a0dcb8 | 43 | //For TIM IT |
nakedt555 | 3:a45557a0dcb8 | 44 | Ticker *it_tick; |
nakedt555 | 3:a45557a0dcb8 | 45 | |
nakedt555 | 1:bdd17feaa4ce | 46 | public: |
nakedt555 | 2:086272a2da1c | 47 | //Constructor |
nakedt555 | 1:bdd17feaa4ce | 48 | Odom(){ |
nakedt555 | 2:086272a2da1c | 49 | initialize(); |
nakedt555 | 2:086272a2da1c | 50 | } |
nakedt555 | 2:086272a2da1c | 51 | //TIM IT Callback function |
nakedt555 | 2:086272a2da1c | 52 | void update_it_cb(){ |
nakedt555 | 2:086272a2da1c | 53 | calc_odometry(); |
nakedt555 | 2:086272a2da1c | 54 | } |
nakedt555 | 2:086272a2da1c | 55 | |
nakedt555 | 2:086272a2da1c | 56 | //Update world angle |
nakedt555 | 2:086272a2da1c | 57 | void update_angle(); |
nakedt555 | 2:086272a2da1c | 58 | |
nakedt555 | 5:e678f1ac6cdc | 59 | void reset_world(){ |
nakedt555 | 5:e678f1ac6cdc | 60 | world_ = Vec3f(0, 0, 0); |
nakedt555 | 5:e678f1ac6cdc | 61 | } |
nakedt555 | 5:e678f1ac6cdc | 62 | |
nakedt555 | 5:e678f1ac6cdc | 63 | //現在角度を0度に設定 |
nakedt555 | 5:e678f1ac6cdc | 64 | void set_offset_angle_zero(){ |
nakedt555 | 6:20a32baeff79 | 65 | offset_angle_ = actual_angle_; |
nakedt555 | 5:e678f1ac6cdc | 66 | } |
nakedt555 | 5:e678f1ac6cdc | 67 | |
nakedt555 | 3:a45557a0dcb8 | 68 | //Getter |
nakedt555 | 4:cf1a4e503974 | 69 | bool get_court_color(){ |
nakedt555 | 4:cf1a4e503974 | 70 | return court_color_; |
nakedt555 | 4:cf1a4e503974 | 71 | } |
nakedt555 | 3:a45557a0dcb8 | 72 | Vec3f get_world(){ |
nakedt555 | 3:a45557a0dcb8 | 73 | return world_; |
nakedt555 | 3:a45557a0dcb8 | 74 | } |
nakedt555 | 3:a45557a0dcb8 | 75 | Vec3f get_initialpose(){ |
nakedt555 | 3:a45557a0dcb8 | 76 | return initialpose_; |
nakedt555 | 3:a45557a0dcb8 | 77 | } |
nakedt555 | 3:a45557a0dcb8 | 78 | Vec3f get_drift(){ |
nakedt555 | 3:a45557a0dcb8 | 79 | return drift_; |
nakedt555 | 3:a45557a0dcb8 | 80 | } |
nakedt555 | 3:a45557a0dcb8 | 81 | |
nakedt555 | 3:a45557a0dcb8 | 82 | //Setter |
nakedt555 | 4:cf1a4e503974 | 83 | void set_court_color(bool court_color){ |
nakedt555 | 4:cf1a4e503974 | 84 | court_color_ = court_color; |
nakedt555 | 4:cf1a4e503974 | 85 | } |
nakedt555 | 3:a45557a0dcb8 | 86 | void set_initialpose(Vec3f initialpose){ |
nakedt555 | 3:a45557a0dcb8 | 87 | initialpose_ = initialpose; |
nakedt555 | 3:a45557a0dcb8 | 88 | } |
nakedt555 | 4:cf1a4e503974 | 89 | void set_world(Vec3f world){ |
nakedt555 | 4:cf1a4e503974 | 90 | world_ = world; |
nakedt555 | 4:cf1a4e503974 | 91 | } |
nakedt555 | 3:a45557a0dcb8 | 92 | void set_drift(Vec3f drift){ |
nakedt555 | 3:a45557a0dcb8 | 93 | drift_ = drift; |
nakedt555 | 3:a45557a0dcb8 | 94 | } |
nakedt555 | 3:a45557a0dcb8 | 95 | |
nakedt555 | 2:086272a2da1c | 96 | private: |
nakedt555 | 2:086272a2da1c | 97 | void initialize(); |
nakedt555 | 2:086272a2da1c | 98 | void calc_odometry(); |
nakedt555 | 1:bdd17feaa4ce | 99 | }; |
nakedt555 | 1:bdd17feaa4ce | 100 | |
nakedt555 | 1:bdd17feaa4ce | 101 | class Odom_Abstract |
nakedt555 | 1:bdd17feaa4ce | 102 | { |
nakedt555 | 1:bdd17feaa4ce | 103 | private: |
nakedt555 | 1:bdd17feaa4ce | 104 | Odom *odom_; |
nakedt555 | 1:bdd17feaa4ce | 105 | |
nakedt555 | 1:bdd17feaa4ce | 106 | public: |
nakedt555 | 1:bdd17feaa4ce | 107 | Odom_Abstract(Odom *odom) : odom_(odom){ |
nakedt555 | 1:bdd17feaa4ce | 108 | |
nakedt555 | 1:bdd17feaa4ce | 109 | } |
nakedt555 | 3:a45557a0dcb8 | 110 | |
nakedt555 | 3:a45557a0dcb8 | 111 | virtual void loop() = 0; |
nakedt555 | 1:bdd17feaa4ce | 112 | |
nakedt555 | 3:a45557a0dcb8 | 113 | protected: |
nakedt555 | 3:a45557a0dcb8 | 114 | void set_instance(Odom* odom){ |
nakedt555 | 3:a45557a0dcb8 | 115 | odom_ = odom; |
nakedt555 | 3:a45557a0dcb8 | 116 | } |
nakedt555 | 3:a45557a0dcb8 | 117 | |
nakedt555 | 5:e678f1ac6cdc | 118 | void reset_pose(){ |
nakedt555 | 5:e678f1ac6cdc | 119 | odom_->reset_world(); |
nakedt555 | 5:e678f1ac6cdc | 120 | odom_->set_offset_angle_zero(); |
nakedt555 | 1:bdd17feaa4ce | 121 | } |
nakedt555 | 1:bdd17feaa4ce | 122 | |
nakedt555 | 4:cf1a4e503974 | 123 | //Getter |
nakedt555 | 4:cf1a4e503974 | 124 | bool get_court_color(){ |
nakedt555 | 4:cf1a4e503974 | 125 | return odom_->get_court_color(); |
nakedt555 | 4:cf1a4e503974 | 126 | } |
nakedt555 | 1:bdd17feaa4ce | 127 | Vec3f get_initial_pose(){ |
nakedt555 | 3:a45557a0dcb8 | 128 | return odom_->get_initialpose(); |
nakedt555 | 3:a45557a0dcb8 | 129 | } |
nakedt555 | 3:a45557a0dcb8 | 130 | Vec3f get_pose(){ |
nakedt555 | 5:e678f1ac6cdc | 131 | return odom_->get_world() + odom_->get_initialpose() + odom_->get_drift(); |
nakedt555 | 1:bdd17feaa4ce | 132 | } |
nakedt555 | 1:bdd17feaa4ce | 133 | Vec3f get_drift(){ |
nakedt555 | 3:a45557a0dcb8 | 134 | return odom_->get_drift(); |
nakedt555 | 1:bdd17feaa4ce | 135 | } |
nakedt555 | 1:bdd17feaa4ce | 136 | |
nakedt555 | 4:cf1a4e503974 | 137 | //Setter |
nakedt555 | 5:e678f1ac6cdc | 138 | void set_initial_pose(Vec3f initialpose){ |
nakedt555 | 5:e678f1ac6cdc | 139 | odom_->set_initialpose(initialpose); |
nakedt555 | 5:e678f1ac6cdc | 140 | } |
nakedt555 | 4:cf1a4e503974 | 141 | void set_court_color(bool court_color){ |
nakedt555 | 4:cf1a4e503974 | 142 | odom_->set_court_color(court_color); |
nakedt555 | 4:cf1a4e503974 | 143 | } |
nakedt555 | 4:cf1a4e503974 | 144 | void set_pose(Vec3f pose){ |
nakedt555 | 4:cf1a4e503974 | 145 | odom_->set_world(pose); |
nakedt555 | 4:cf1a4e503974 | 146 | } |
nakedt555 | 4:cf1a4e503974 | 147 | void set_drift(Vec3f drift){ |
nakedt555 | 4:cf1a4e503974 | 148 | odom_->set_drift(drift); |
nakedt555 | 4:cf1a4e503974 | 149 | } |
nakedt555 | 4:cf1a4e503974 | 150 | |
nakedt555 | 1:bdd17feaa4ce | 151 | }; |
nakedt555 | 1:bdd17feaa4ce | 152 | #endif |