駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

odometry/odom.h@8:80708bacb5b5, 2018-12-20 (annotated)

Committer:
nakedt555
Date:
Thu Dec 20 20:54:35 2018 +0000
Revision:
8:80708bacb5b5
Parent:
6:20a32baeff79
Child:
10:3b47050b1652
amcl kakunin

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 5:e678f1ac6cdc 75
nakedt555 3:a45557a0dcb8 76 //Getter
nakedt555 4:cf1a4e503974 77 bool get_court_color(){
nakedt555 4:cf1a4e503974 78 return court_color_;
nakedt555 4:cf1a4e503974 79 }
nakedt555 8:80708bacb5b5 80 Vec3f get_odom(){
nakedt555 8:80708bacb5b5 81 return odom_;
nakedt555 3:a45557a0dcb8 82 }
nakedt555 3:a45557a0dcb8 83 Vec3f get_initialpose(){
nakedt555 3:a45557a0dcb8 84 return initialpose_;
nakedt555 3:a45557a0dcb8 85 }
nakedt555 3:a45557a0dcb8 86 Vec3f get_drift(){
nakedt555 3:a45557a0dcb8 87 return drift_;
nakedt555 3:a45557a0dcb8 88 }
nakedt555 8:80708bacb5b5 89 bool get_enable_oled(){
nakedt555 8:80708bacb5b5 90 return enable_oled_;
nakedt555 8:80708bacb5b5 91 }
nakedt555 8:80708bacb5b5 92
nakedt555 3:a45557a0dcb8 93 //Setter
nakedt555 4:cf1a4e503974 94 void set_court_color(bool court_color){
nakedt555 4:cf1a4e503974 95 court_color_ = court_color;
nakedt555 4:cf1a4e503974 96 }
nakedt555 3:a45557a0dcb8 97 void set_initialpose(Vec3f initialpose){
nakedt555 3:a45557a0dcb8 98 initialpose_ = initialpose;
nakedt555 3:a45557a0dcb8 99 }
nakedt555 3:a45557a0dcb8 100 void set_drift(Vec3f drift){
nakedt555 3:a45557a0dcb8 101 drift_ = drift;
nakedt555 3:a45557a0dcb8 102 }
nakedt555 8:80708bacb5b5 103 void set_enable_oled(bool enable_oled){
nakedt555 8:80708bacb5b5 104 enable_oled_ = enable_oled;
nakedt555 8:80708bacb5b5 105 }
nakedt555 3:a45557a0dcb8 106
nakedt555 2:086272a2da1c 107 private:
nakedt555 2:086272a2da1c 108 void initialize();
nakedt555 2:086272a2da1c 109 void calc_odometry();
nakedt555 1:bdd17feaa4ce 110 };
nakedt555 1:bdd17feaa4ce 111
nakedt555 1:bdd17feaa4ce 112 class Odom_Abstract
nakedt555 1:bdd17feaa4ce 113 {
nakedt555 1:bdd17feaa4ce 114 private:
nakedt555 1:bdd17feaa4ce 115 Odom *odom_;
nakedt555 1:bdd17feaa4ce 116
nakedt555 1:bdd17feaa4ce 117 public:
nakedt555 1:bdd17feaa4ce 118 Odom_Abstract(Odom *odom) : odom_(odom){
nakedt555 1:bdd17feaa4ce 119
nakedt555 1:bdd17feaa4ce 120 }
nakedt555 3:a45557a0dcb8 121
nakedt555 3:a45557a0dcb8 122 virtual void loop() = 0;
nakedt555 1:bdd17feaa4ce 123
nakedt555 3:a45557a0dcb8 124 protected:
nakedt555 3:a45557a0dcb8 125 void set_instance(Odom* odom){
nakedt555 3:a45557a0dcb8 126 odom_ = odom;
nakedt555 3:a45557a0dcb8 127 }
nakedt555 3:a45557a0dcb8 128
nakedt555 8:80708bacb5b5 129 void reset_odometry(){
nakedt555 8:80708bacb5b5 130 odom_->reset_odom();
nakedt555 8:80708bacb5b5 131 odom_->reset_drift();
nakedt555 5:e678f1ac6cdc 132 odom_->set_offset_angle_zero();
nakedt555 1:bdd17feaa4ce 133 }
nakedt555 1:bdd17feaa4ce 134
nakedt555 4:cf1a4e503974 135 //Getter
nakedt555 4:cf1a4e503974 136 bool get_court_color(){
nakedt555 4:cf1a4e503974 137 return odom_->get_court_color();
nakedt555 4:cf1a4e503974 138 }
nakedt555 8:80708bacb5b5 139
nakedt555 1:bdd17feaa4ce 140 Vec3f get_initial_pose(){
nakedt555 3:a45557a0dcb8 141 return odom_->get_initialpose();
nakedt555 3:a45557a0dcb8 142 }
nakedt555 8:80708bacb5b5 143 Vec3f get_odom(){
nakedt555 8:80708bacb5b5 144 return odom_->get_odom();
nakedt555 1:bdd17feaa4ce 145 }
nakedt555 1:bdd17feaa4ce 146 Vec3f get_drift(){
nakedt555 3:a45557a0dcb8 147 return odom_->get_drift();
nakedt555 1:bdd17feaa4ce 148 }
nakedt555 1:bdd17feaa4ce 149
nakedt555 8:80708bacb5b5 150 Vec3f get_pose(){
nakedt555 8:80708bacb5b5 151 return get_initial_pose() + get_odom();
nakedt555 8:80708bacb5b5 152 }
nakedt555 8:80708bacb5b5 153 Vec3f get_world(){
nakedt555 8:80708bacb5b5 154 return get_pose() + get_drift();
nakedt555 8:80708bacb5b5 155 }
nakedt555 8:80708bacb5b5 156
nakedt555 8:80708bacb5b5 157 bool get_enable_oled(){
nakedt555 8:80708bacb5b5 158 return odom_->get_enable_oled();
nakedt555 8:80708bacb5b5 159 }
nakedt555 8:80708bacb5b5 160
nakedt555 4:cf1a4e503974 161 //Setter
nakedt555 4:cf1a4e503974 162 void set_court_color(bool court_color){
nakedt555 4:cf1a4e503974 163 odom_->set_court_color(court_color);
nakedt555 4:cf1a4e503974 164 }
nakedt555 8:80708bacb5b5 165
nakedt555 8:80708bacb5b5 166 void set_initial_pose(Vec3f initialpose){
nakedt555 8:80708bacb5b5 167 odom_->set_initialpose(initialpose);//CANから受け取った初期位置の情報を格納するためのメソッド
nakedt555 4:cf1a4e503974 168 }
nakedt555 4:cf1a4e503974 169 void set_drift(Vec3f drift){
nakedt555 8:80708bacb5b5 170 odom_->set_drift(drift);//ROSからサブスクライブしたデータを格納するためのメソッド
nakedt555 8:80708bacb5b5 171 }
nakedt555 8:80708bacb5b5 172
nakedt555 8:80708bacb5b5 173 void set_enable_oled(bool enable_oled){
nakedt555 8:80708bacb5b5 174 odom_->set_enable_oled(enable_oled);
nakedt555 4:cf1a4e503974 175 }
nakedt555 4:cf1a4e503974 176
nakedt555 1:bdd17feaa4ce 177 };
nakedt555 1:bdd17feaa4ce 178 #endif