駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

odometry/odom.h@10:3b47050b1652, 2019-03-22 (annotated)

Committer:
nakedt555
Date:
Fri Mar 22 12:29:18 2019 +0000
Revision:
10:3b47050b1652
Parent:
8:80708bacb5b5
Child:
11:375d474b8522
check ok

Who changed what in which revision?

UserRevisionLine numberNew 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 8:80708bacb5b5 33 Vec3f odom_;
nakedt555 2:086272a2da1c 34
nakedt555 8:80708bacb5b5 35 //For ROS
nakedt555 4:cf1a4e503974 36 bool court_color_;
nakedt555 3:a45557a0dcb8 37 Vec3f initialpose_;
nakedt555 3:a45557a0dcb8 38 Vec3f drift_;
nakedt555 3:a45557a0dcb8 39
nakedt555 3:a45557a0dcb8 40 //For TIM IT
nakedt555 3:a45557a0dcb8 41 Ticker *it_tick;
nakedt555 3:a45557a0dcb8 42
nakedt555 8:80708bacb5b5 43 //For OLED
nakedt555 8:80708bacb5b5 44 bool enable_oled_;
nakedt555 8:80708bacb5b5 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 8:80708bacb5b5 59 void reset_odom(){
nakedt555 8:80708bacb5b5 60 odom_.x(0.0);
nakedt555 8:80708bacb5b5 61 odom_.y(0.0);
nakedt555 8:80708bacb5b5 62 odom_.angle(0.0);
nakedt555 8:80708bacb5b5 63 }
nakedt555 8:80708bacb5b5 64
nakedt555 8:80708bacb5b5 65 void reset_drift(){
nakedt555 8:80708bacb5b5 66 drift_.x(0.0);
nakedt555 8:80708bacb5b5 67 drift_.y(0.0);
nakedt555 8:80708bacb5b5 68 drift_.angle(0.0);
nakedt555 5:e678f1ac6cdc 69 }
nakedt555 5:e678f1ac6cdc 70
nakedt555 5:e678f1ac6cdc 71 //現在角度を0度に設定
nakedt555 5:e678f1ac6cdc 72 void set_offset_angle_zero(){
nakedt555 6:20a32baeff79 73 offset_angle_ = actual_angle_;
nakedt555 5:e678f1ac6cdc 74 }
nakedt555 10:3b47050b1652 75
nakedt555 10:3b47050b1652 76 //現在角度を0度に設定
nakedt555 10:3b47050b1652 77 void set_offset_angle_initialpose(){
nakedt555 10:3b47050b1652 78 offset_angle_ = actual_angle_ - initialpose_.angle();
nakedt555 10:3b47050b1652 79 }
nakedt555 5:e678f1ac6cdc 80
nakedt555 3:a45557a0dcb8 81 //Getter
nakedt555 4:cf1a4e503974 82 bool get_court_color(){
nakedt555 4:cf1a4e503974 83 return court_color_;
nakedt555 4:cf1a4e503974 84 }
nakedt555 8:80708bacb5b5 85 Vec3f get_odom(){
nakedt555 8:80708bacb5b5 86 return odom_;
nakedt555 3:a45557a0dcb8 87 }
nakedt555 3:a45557a0dcb8 88 Vec3f get_initialpose(){
nakedt555 3:a45557a0dcb8 89 return initialpose_;
nakedt555 3:a45557a0dcb8 90 }
nakedt555 3:a45557a0dcb8 91 Vec3f get_drift(){
nakedt555 3:a45557a0dcb8 92 return drift_;
nakedt555 3:a45557a0dcb8 93 }
nakedt555 8:80708bacb5b5 94 bool get_enable_oled(){
nakedt555 8:80708bacb5b5 95 return enable_oled_;
nakedt555 8:80708bacb5b5 96 }
nakedt555 8:80708bacb5b5 97
nakedt555 3:a45557a0dcb8 98 //Setter
nakedt555 4:cf1a4e503974 99 void set_court_color(bool court_color){
nakedt555 4:cf1a4e503974 100 court_color_ = court_color;
nakedt555 4:cf1a4e503974 101 }
nakedt555 3:a45557a0dcb8 102 void set_initialpose(Vec3f initialpose){
nakedt555 3:a45557a0dcb8 103 initialpose_ = initialpose;
nakedt555 3:a45557a0dcb8 104 }
nakedt555 3:a45557a0dcb8 105 void set_drift(Vec3f drift){
nakedt555 3:a45557a0dcb8 106 drift_ = drift;
nakedt555 3:a45557a0dcb8 107 }
nakedt555 8:80708bacb5b5 108 void set_enable_oled(bool enable_oled){
nakedt555 8:80708bacb5b5 109 enable_oled_ = enable_oled;
nakedt555 8:80708bacb5b5 110 }
nakedt555 3:a45557a0dcb8 111
nakedt555 2:086272a2da1c 112 private:
nakedt555 2:086272a2da1c 113 void initialize();
nakedt555 2:086272a2da1c 114 void calc_odometry();
nakedt555 1:bdd17feaa4ce 115 };
nakedt555 1:bdd17feaa4ce 116
nakedt555 1:bdd17feaa4ce 117 class Odom_Abstract
nakedt555 1:bdd17feaa4ce 118 {
nakedt555 1:bdd17feaa4ce 119 private:
nakedt555 1:bdd17feaa4ce 120 Odom *odom_;
nakedt555 1:bdd17feaa4ce 121
nakedt555 1:bdd17feaa4ce 122 public:
nakedt555 1:bdd17feaa4ce 123 Odom_Abstract(Odom *odom) : odom_(odom){
nakedt555 1:bdd17feaa4ce 124
nakedt555 1:bdd17feaa4ce 125 }
nakedt555 3:a45557a0dcb8 126
nakedt555 3:a45557a0dcb8 127 virtual void loop() = 0;
nakedt555 1:bdd17feaa4ce 128
nakedt555 3:a45557a0dcb8 129 protected:
nakedt555 3:a45557a0dcb8 130 void set_instance(Odom* odom){
nakedt555 3:a45557a0dcb8 131 odom_ = odom;
nakedt555 3:a45557a0dcb8 132 }
nakedt555 3:a45557a0dcb8 133
nakedt555 8:80708bacb5b5 134 void reset_odometry(){
nakedt555 8:80708bacb5b5 135 odom_->reset_odom();
nakedt555 8:80708bacb5b5 136 odom_->reset_drift();
nakedt555 5:e678f1ac6cdc 137 odom_->set_offset_angle_zero();
nakedt555 10:3b47050b1652 138 // odom_->set_offset_angle_initialpose();
nakedt555 1:bdd17feaa4ce 139 }
nakedt555 1:bdd17feaa4ce 140
nakedt555 4:cf1a4e503974 141 //Getter
nakedt555 4:cf1a4e503974 142 bool get_court_color(){
nakedt555 4:cf1a4e503974 143 return odom_->get_court_color();
nakedt555 4:cf1a4e503974 144 }
nakedt555 8:80708bacb5b5 145
nakedt555 1:bdd17feaa4ce 146 Vec3f get_initial_pose(){
nakedt555 3:a45557a0dcb8 147 return odom_->get_initialpose();
nakedt555 3:a45557a0dcb8 148 }
nakedt555 8:80708bacb5b5 149 Vec3f get_odom(){
nakedt555 8:80708bacb5b5 150 return odom_->get_odom();
nakedt555 1:bdd17feaa4ce 151 }
nakedt555 1:bdd17feaa4ce 152 Vec3f get_drift(){
nakedt555 3:a45557a0dcb8 153 return odom_->get_drift();
nakedt555 1:bdd17feaa4ce 154 }
nakedt555 1:bdd17feaa4ce 155
nakedt555 8:80708bacb5b5 156 Vec3f get_pose(){
nakedt555 8:80708bacb5b5 157 return get_initial_pose() + get_odom();
nakedt555 8:80708bacb5b5 158 }
nakedt555 8:80708bacb5b5 159 Vec3f get_world(){
nakedt555 8:80708bacb5b5 160 return get_pose() + get_drift();
nakedt555 8:80708bacb5b5 161 }
nakedt555 8:80708bacb5b5 162
nakedt555 8:80708bacb5b5 163 bool get_enable_oled(){
nakedt555 8:80708bacb5b5 164 return odom_->get_enable_oled();
nakedt555 8:80708bacb5b5 165 }
nakedt555 8:80708bacb5b5 166
nakedt555 4:cf1a4e503974 167 //Setter
nakedt555 4:cf1a4e503974 168 void set_court_color(bool court_color){
nakedt555 4:cf1a4e503974 169 odom_->set_court_color(court_color);
nakedt555 4:cf1a4e503974 170 }
nakedt555 8:80708bacb5b5 171
nakedt555 8:80708bacb5b5 172 void set_initial_pose(Vec3f initialpose){
nakedt555 8:80708bacb5b5 173 odom_->set_initialpose(initialpose);//CANから受け取った初期位置の情報を格納するためのメソッド
nakedt555 4:cf1a4e503974 174 }
nakedt555 4:cf1a4e503974 175 void set_drift(Vec3f drift){
nakedt555 8:80708bacb5b5 176 odom_->set_drift(drift);//ROSからサブスクライブしたデータを格納するためのメソッド
nakedt555 8:80708bacb5b5 177 }
nakedt555 8:80708bacb5b5 178
nakedt555 8:80708bacb5b5 179 void set_enable_oled(bool enable_oled){
nakedt555 8:80708bacb5b5 180 odom_->set_enable_oled(enable_oled);
nakedt555 4:cf1a4e503974 181 }
nakedt555 4:cf1a4e503974 182
nakedt555 1:bdd17feaa4ce 183 };
nakedt555 1:bdd17feaa4ce 184 #endif