駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

odometry/odom.h@11:375d474b8522, 2019-08-30 (annotated)

Committer:
nakedt555
Date:
Fri Aug 30 07:53:21 2019 +0000
Revision:
11:375d474b8522
Parent:
10:3b47050b1652 
test

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 11:375d474b8522 8 #include "math.h"
nakedt555 2:086272a2da1c 9
nakedt555 2:086272a2da1c 10 #define PI (3.14159265f)
nakedt555 2:086272a2da1c 11 #define H_PI (1.57079632f)
nakedt555 11:375d474b8522 12 #define ENCODER_WHEEL_DIAMETER (0.054f)
nakedt555 2:086272a2da1c 13 #define EDGE_EVALUATION (4.0f)
nakedt555 2:086272a2da1c 14 #define PULSE_ENCODER (500.0f)
nakedt555 2:086272a2da1c 15 #define ENCODER_COEFFICIENT ((ENCODER_WHEEL_DIAMETER * PI) / (EDGE_EVALUATION * PULSE_ENCODER))
nakedt555 2:086272a2da1c 16
nakedt555 2:086272a2da1c 17 #define DEG2RAD(x) (((x) * 3.1416f) / 180.0f)
nakedt555 2:086272a2da1c 18 #define RAD2DEG(x) (((x) / 3.1416f) * 180.0f)
nakedt555 2:086272a2da1c 19 #define ABS(x) (((x) > 0) ? (x) : -(x))
nakedt555 1:bdd17feaa4ce 20
nakedt555 4:cf1a4e503974 21 #define COURT_RED false
nakedt555 4:cf1a4e503974 22 #define COURT_BLUE true
nakedt555 4:cf1a4e503974 23
nakedt555 1:bdd17feaa4ce 24 class Odom
nakedt555 1:bdd17feaa4ce 25 {
nakedt555 1:bdd17feaa4ce 26 private:
nakedt555 2:086272a2da1c 27 //class instance
nakedt555 2:086272a2da1c 28 My_BNO055 *imu_;
nakedt555 2:086272a2da1c 29 TIM2Encoder *enc0_;
nakedt555 2:086272a2da1c 30 TIM3Encoder *enc1_;
nakedt555 5:e678f1ac6cdc 31
nakedt555 2:086272a2da1c 32 float offset_angle_;
nakedt555 2:086272a2da1c 33 float actual_angle_;
nakedt555 8:80708bacb5b5 34 Vec3f odom_;
nakedt555 2:086272a2da1c 35
nakedt555 8:80708bacb5b5 36 //For ROS
nakedt555 4:cf1a4e503974 37 bool court_color_;
nakedt555 3:a45557a0dcb8 38 Vec3f initialpose_;
nakedt555 3:a45557a0dcb8 39 Vec3f drift_;
nakedt555 3:a45557a0dcb8 40
nakedt555 3:a45557a0dcb8 41 //For TIM IT
nakedt555 3:a45557a0dcb8 42 Ticker *it_tick;
nakedt555 3:a45557a0dcb8 43
nakedt555 8:80708bacb5b5 44 //For OLED
nakedt555 8:80708bacb5b5 45 bool enable_oled_;
nakedt555 8:80708bacb5b5 46
nakedt555 1:bdd17feaa4ce 47 public:
nakedt555 2:086272a2da1c 48 //Constructor
nakedt555 1:bdd17feaa4ce 49 Odom(){
nakedt555 2:086272a2da1c 50 initialize();
nakedt555 2:086272a2da1c 51 }
nakedt555 2:086272a2da1c 52 //TIM IT Callback function
nakedt555 2:086272a2da1c 53 void update_it_cb(){
nakedt555 2:086272a2da1c 54 calc_odometry();
nakedt555 2:086272a2da1c 55 }
nakedt555 2:086272a2da1c 56
nakedt555 2:086272a2da1c 57 //Update world angle
nakedt555 2:086272a2da1c 58 void update_angle();
nakedt555 2:086272a2da1c 59
nakedt555 8:80708bacb5b5 60 void reset_odom(){
nakedt555 8:80708bacb5b5 61 odom_.x(0.0);
nakedt555 8:80708bacb5b5 62 odom_.y(0.0);
nakedt555 8:80708bacb5b5 63 odom_.angle(0.0);
nakedt555 8:80708bacb5b5 64 }
nakedt555 8:80708bacb5b5 65
nakedt555 8:80708bacb5b5 66 void reset_drift(){
nakedt555 8:80708bacb5b5 67 drift_.x(0.0);
nakedt555 8:80708bacb5b5 68 drift_.y(0.0);
nakedt555 8:80708bacb5b5 69 drift_.angle(0.0);
nakedt555 5:e678f1ac6cdc 70 }
nakedt555 5:e678f1ac6cdc 71
nakedt555 5:e678f1ac6cdc 72 //現在角度を0度に設定
nakedt555 5:e678f1ac6cdc 73 void set_offset_angle_zero(){
nakedt555 6:20a32baeff79 74 offset_angle_ = actual_angle_;
nakedt555 5:e678f1ac6cdc 75 }
nakedt555 10:3b47050b1652 76
nakedt555 10:3b47050b1652 77 //現在角度を0度に設定
nakedt555 10:3b47050b1652 78 void set_offset_angle_initialpose(){
nakedt555 10:3b47050b1652 79 offset_angle_ = actual_angle_ - initialpose_.angle();
nakedt555 10:3b47050b1652 80 }
nakedt555 5:e678f1ac6cdc 81
nakedt555 3:a45557a0dcb8 82 //Getter
nakedt555 4:cf1a4e503974 83 bool get_court_color(){
nakedt555 4:cf1a4e503974 84 return court_color_;
nakedt555 4:cf1a4e503974 85 }
nakedt555 8:80708bacb5b5 86 Vec3f get_odom(){
nakedt555 8:80708bacb5b5 87 return odom_;
nakedt555 3:a45557a0dcb8 88 }
nakedt555 3:a45557a0dcb8 89 Vec3f get_initialpose(){
nakedt555 3:a45557a0dcb8 90 return initialpose_;
nakedt555 3:a45557a0dcb8 91 }
nakedt555 3:a45557a0dcb8 92 Vec3f get_drift(){
nakedt555 3:a45557a0dcb8 93 return drift_;
nakedt555 3:a45557a0dcb8 94 }
nakedt555 8:80708bacb5b5 95 bool get_enable_oled(){
nakedt555 8:80708bacb5b5 96 return enable_oled_;
nakedt555 8:80708bacb5b5 97 }
nakedt555 8:80708bacb5b5 98
nakedt555 3:a45557a0dcb8 99 //Setter
nakedt555 4:cf1a4e503974 100 void set_court_color(bool court_color){
nakedt555 4:cf1a4e503974 101 court_color_ = court_color;
nakedt555 4:cf1a4e503974 102 }
nakedt555 3:a45557a0dcb8 103 void set_initialpose(Vec3f initialpose){
nakedt555 3:a45557a0dcb8 104 initialpose_ = initialpose;
nakedt555 3:a45557a0dcb8 105 }
nakedt555 3:a45557a0dcb8 106 void set_drift(Vec3f drift){
nakedt555 3:a45557a0dcb8 107 drift_ = drift;
nakedt555 3:a45557a0dcb8 108 }
nakedt555 8:80708bacb5b5 109 void set_enable_oled(bool enable_oled){
nakedt555 8:80708bacb5b5 110 enable_oled_ = enable_oled;
nakedt555 8:80708bacb5b5 111 }
nakedt555 3:a45557a0dcb8 112
nakedt555 2:086272a2da1c 113 private:
nakedt555 2:086272a2da1c 114 void initialize();
nakedt555 2:086272a2da1c 115 void calc_odometry();
nakedt555 1:bdd17feaa4ce 116 };
nakedt555 1:bdd17feaa4ce 117
nakedt555 1:bdd17feaa4ce 118 class Odom_Abstract
nakedt555 1:bdd17feaa4ce 119 {
nakedt555 1:bdd17feaa4ce 120 private:
nakedt555 1:bdd17feaa4ce 121 Odom *odom_;
nakedt555 1:bdd17feaa4ce 122
nakedt555 1:bdd17feaa4ce 123 public:
nakedt555 1:bdd17feaa4ce 124 Odom_Abstract(Odom *odom) : odom_(odom){
nakedt555 1:bdd17feaa4ce 125
nakedt555 1:bdd17feaa4ce 126 }
nakedt555 3:a45557a0dcb8 127
nakedt555 3:a45557a0dcb8 128 virtual void loop() = 0;
nakedt555 1:bdd17feaa4ce 129
nakedt555 3:a45557a0dcb8 130 protected:
nakedt555 3:a45557a0dcb8 131 void set_instance(Odom* odom){
nakedt555 3:a45557a0dcb8 132 odom_ = odom;
nakedt555 3:a45557a0dcb8 133 }
nakedt555 3:a45557a0dcb8 134
nakedt555 8:80708bacb5b5 135 void reset_odometry(){
nakedt555 8:80708bacb5b5 136 odom_->reset_odom();
nakedt555 8:80708bacb5b5 137 odom_->reset_drift();
nakedt555 5:e678f1ac6cdc 138 odom_->set_offset_angle_zero();
nakedt555 10:3b47050b1652 139 // odom_->set_offset_angle_initialpose();
nakedt555 1:bdd17feaa4ce 140 }
nakedt555 1:bdd17feaa4ce 141
nakedt555 4:cf1a4e503974 142 //Getter
nakedt555 4:cf1a4e503974 143 bool get_court_color(){
nakedt555 4:cf1a4e503974 144 return odom_->get_court_color();
nakedt555 4:cf1a4e503974 145 }
nakedt555 8:80708bacb5b5 146
nakedt555 1:bdd17feaa4ce 147 Vec3f get_initial_pose(){
nakedt555 3:a45557a0dcb8 148 return odom_->get_initialpose();
nakedt555 3:a45557a0dcb8 149 }
nakedt555 8:80708bacb5b5 150 Vec3f get_odom(){
nakedt555 8:80708bacb5b5 151 return odom_->get_odom();
nakedt555 1:bdd17feaa4ce 152 }
nakedt555 1:bdd17feaa4ce 153 Vec3f get_drift(){
nakedt555 3:a45557a0dcb8 154 return odom_->get_drift();
nakedt555 1:bdd17feaa4ce 155 }
nakedt555 1:bdd17feaa4ce 156
nakedt555 8:80708bacb5b5 157 Vec3f get_pose(){
nakedt555 8:80708bacb5b5 158 return get_initial_pose() + get_odom();
nakedt555 8:80708bacb5b5 159 }
nakedt555 8:80708bacb5b5 160 Vec3f get_world(){
nakedt555 11:375d474b8522 161 // return get_pose() + get_drift();
nakedt555 11:375d474b8522 162 Vec3f ret;
nakedt555 11:375d474b8522 163 float t = get_drift().angle();
nakedt555 11:375d474b8522 164 ret.x(get_pose().x() * cos(t) - get_pose().y() * sin(t) + get_drift().x());
nakedt555 11:375d474b8522 165 ret.y(get_pose().x() * sin(t) + get_pose().y() * cos(t) + get_drift().y());
nakedt555 11:375d474b8522 166 ret.angle(get_pose().angle() + t);
nakedt555 11:375d474b8522 167 return ret;
nakedt555 8:80708bacb5b5 168 }
nakedt555 8:80708bacb5b5 169
nakedt555 8:80708bacb5b5 170 bool get_enable_oled(){
nakedt555 8:80708bacb5b5 171 return odom_->get_enable_oled();
nakedt555 8:80708bacb5b5 172 }
nakedt555 8:80708bacb5b5 173
nakedt555 4:cf1a4e503974 174 //Setter
nakedt555 4:cf1a4e503974 175 void set_court_color(bool court_color){
nakedt555 4:cf1a4e503974 176 odom_->set_court_color(court_color);
nakedt555 4:cf1a4e503974 177 }
nakedt555 8:80708bacb5b5 178
nakedt555 8:80708bacb5b5 179 void set_initial_pose(Vec3f initialpose){
nakedt555 8:80708bacb5b5 180 odom_->set_initialpose(initialpose);//CANから受け取った初期位置の情報を格納するためのメソッド
nakedt555 4:cf1a4e503974 181 }
nakedt555 4:cf1a4e503974 182 void set_drift(Vec3f drift){
nakedt555 8:80708bacb5b5 183 odom_->set_drift(drift);//ROSからサブスクライブしたデータを格納するためのメソッド
nakedt555 8:80708bacb5b5 184 }
nakedt555 8:80708bacb5b5 185
nakedt555 8:80708bacb5b5 186 void set_enable_oled(bool enable_oled){
nakedt555 8:80708bacb5b5 187 odom_->set_enable_oled(enable_oled);
nakedt555 4:cf1a4e503974 188 }
nakedt555 4:cf1a4e503974 189
nakedt555 1:bdd17feaa4ce 190 };
nakedt555 1:bdd17feaa4ce 191 #endif