駿 高世 / Mbed 2 deprecated abu2019_mr1

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?

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 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