aaa
Dependencies: mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic
odometry/odom.h@4:cf1a4e503974, 2018-12-12 (annotated)
- Committer:
- nakedt555
- Date:
- Wed Dec 12 03:35:52 2018 +0000
- Revision:
- 4:cf1a4e503974
- Parent:
- 3:a45557a0dcb8
- Child:
- 5:e678f1ac6cdc
toriaezu ok;
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 | 1:bdd17feaa4ce | 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 | 3:a45557a0dcb8 | 59 | //Getter |
nakedt555 | 4:cf1a4e503974 | 60 | bool get_court_color(){ |
nakedt555 | 4:cf1a4e503974 | 61 | return court_color_; |
nakedt555 | 4:cf1a4e503974 | 62 | } |
nakedt555 | 3:a45557a0dcb8 | 63 | Vec3f get_world(){ |
nakedt555 | 3:a45557a0dcb8 | 64 | return world_; |
nakedt555 | 3:a45557a0dcb8 | 65 | } |
nakedt555 | 3:a45557a0dcb8 | 66 | Vec3f get_initialpose(){ |
nakedt555 | 3:a45557a0dcb8 | 67 | return initialpose_; |
nakedt555 | 3:a45557a0dcb8 | 68 | } |
nakedt555 | 3:a45557a0dcb8 | 69 | Vec3f get_drift(){ |
nakedt555 | 3:a45557a0dcb8 | 70 | return drift_; |
nakedt555 | 3:a45557a0dcb8 | 71 | } |
nakedt555 | 3:a45557a0dcb8 | 72 | |
nakedt555 | 3:a45557a0dcb8 | 73 | //Setter |
nakedt555 | 4:cf1a4e503974 | 74 | void set_court_color(bool court_color){ |
nakedt555 | 4:cf1a4e503974 | 75 | court_color_ = court_color; |
nakedt555 | 4:cf1a4e503974 | 76 | } |
nakedt555 | 3:a45557a0dcb8 | 77 | void set_initialpose(Vec3f initialpose){ |
nakedt555 | 3:a45557a0dcb8 | 78 | initialpose_ = initialpose; |
nakedt555 | 3:a45557a0dcb8 | 79 | } |
nakedt555 | 4:cf1a4e503974 | 80 | void set_world(Vec3f world){ |
nakedt555 | 4:cf1a4e503974 | 81 | world_ = world; |
nakedt555 | 4:cf1a4e503974 | 82 | } |
nakedt555 | 3:a45557a0dcb8 | 83 | void set_drift(Vec3f drift){ |
nakedt555 | 3:a45557a0dcb8 | 84 | drift_ = drift; |
nakedt555 | 3:a45557a0dcb8 | 85 | } |
nakedt555 | 3:a45557a0dcb8 | 86 | |
nakedt555 | 2:086272a2da1c | 87 | private: |
nakedt555 | 2:086272a2da1c | 88 | void initialize(); |
nakedt555 | 2:086272a2da1c | 89 | void calc_odometry(); |
nakedt555 | 1:bdd17feaa4ce | 90 | }; |
nakedt555 | 1:bdd17feaa4ce | 91 | |
nakedt555 | 1:bdd17feaa4ce | 92 | class Odom_Abstract |
nakedt555 | 1:bdd17feaa4ce | 93 | { |
nakedt555 | 1:bdd17feaa4ce | 94 | private: |
nakedt555 | 1:bdd17feaa4ce | 95 | Odom *odom_; |
nakedt555 | 1:bdd17feaa4ce | 96 | |
nakedt555 | 1:bdd17feaa4ce | 97 | public: |
nakedt555 | 1:bdd17feaa4ce | 98 | Odom_Abstract(Odom *odom) : odom_(odom){ |
nakedt555 | 1:bdd17feaa4ce | 99 | |
nakedt555 | 1:bdd17feaa4ce | 100 | } |
nakedt555 | 3:a45557a0dcb8 | 101 | |
nakedt555 | 3:a45557a0dcb8 | 102 | virtual void loop() = 0; |
nakedt555 | 1:bdd17feaa4ce | 103 | |
nakedt555 | 3:a45557a0dcb8 | 104 | protected: |
nakedt555 | 3:a45557a0dcb8 | 105 | void set_instance(Odom* odom){ |
nakedt555 | 3:a45557a0dcb8 | 106 | odom_ = odom; |
nakedt555 | 3:a45557a0dcb8 | 107 | } |
nakedt555 | 3:a45557a0dcb8 | 108 | |
nakedt555 | 3:a45557a0dcb8 | 109 | void set_initial_pose(Vec3f initialpose){ |
nakedt555 | 3:a45557a0dcb8 | 110 | odom_->set_initialpose(initialpose); |
nakedt555 | 1:bdd17feaa4ce | 111 | } |
nakedt555 | 1:bdd17feaa4ce | 112 | |
nakedt555 | 4:cf1a4e503974 | 113 | //Getter |
nakedt555 | 4:cf1a4e503974 | 114 | bool get_court_color(){ |
nakedt555 | 4:cf1a4e503974 | 115 | return odom_->get_court_color(); |
nakedt555 | 4:cf1a4e503974 | 116 | } |
nakedt555 | 1:bdd17feaa4ce | 117 | Vec3f get_initial_pose(){ |
nakedt555 | 3:a45557a0dcb8 | 118 | return odom_->get_initialpose(); |
nakedt555 | 3:a45557a0dcb8 | 119 | } |
nakedt555 | 3:a45557a0dcb8 | 120 | Vec3f get_pose(){ |
nakedt555 | 3:a45557a0dcb8 | 121 | return odom_->get_world(); |
nakedt555 | 1:bdd17feaa4ce | 122 | } |
nakedt555 | 1:bdd17feaa4ce | 123 | Vec3f get_drift(){ |
nakedt555 | 3:a45557a0dcb8 | 124 | return odom_->get_drift(); |
nakedt555 | 1:bdd17feaa4ce | 125 | } |
nakedt555 | 1:bdd17feaa4ce | 126 | |
nakedt555 | 4:cf1a4e503974 | 127 | //Setter |
nakedt555 | 4:cf1a4e503974 | 128 | void set_court_color(bool court_color){ |
nakedt555 | 4:cf1a4e503974 | 129 | odom_->set_court_color(court_color); |
nakedt555 | 4:cf1a4e503974 | 130 | } |
nakedt555 | 4:cf1a4e503974 | 131 | void set_pose(Vec3f pose){ |
nakedt555 | 4:cf1a4e503974 | 132 | odom_->set_world(pose); |
nakedt555 | 4:cf1a4e503974 | 133 | } |
nakedt555 | 4:cf1a4e503974 | 134 | void set_drift(Vec3f drift){ |
nakedt555 | 4:cf1a4e503974 | 135 | odom_->set_drift(drift); |
nakedt555 | 4:cf1a4e503974 | 136 | } |
nakedt555 | 4:cf1a4e503974 | 137 | |
nakedt555 | 1:bdd17feaa4ce | 138 | }; |
nakedt555 | 1:bdd17feaa4ce | 139 | #endif |