駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

odometry/odom.h@5:e678f1ac6cdc, 2018-12-18 (annotated)

Committer:
nakedt555
Date:
Tue Dec 18 21:00:30 2018 +0000
Revision:
5:e678f1ac6cdc
Parent:
4:cf1a4e503974
Child:
6:20a32baeff79
before can it

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 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 5:e678f1ac6cdc 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