駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

odometry/odom.h@3:a45557a0dcb8, 2018-12-11 (annotated)

Committer:
nakedt555
Date:
Tue Dec 11 17:51:47 2018 +0000
Revision:
3:a45557a0dcb8
Parent:
odom.h@2:086272a2da1c
Child:
4:cf1a4e503974
12/10iikanji

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 1:bdd17feaa4ce 20 class Odom
nakedt555 1:bdd17feaa4ce 21 {
nakedt555 1:bdd17feaa4ce 22 private:
nakedt555 2:086272a2da1c 23 //class instance
nakedt555 2:086272a2da1c 24 My_BNO055 *imu_;
nakedt555 2:086272a2da1c 25 TIM2Encoder *enc0_;
nakedt555 2:086272a2da1c 26 TIM3Encoder *enc1_;
nakedt555 1:bdd17feaa4ce 27
nakedt555 2:086272a2da1c 28 float offset_angle_;
nakedt555 2:086272a2da1c 29 float actual_angle_;
nakedt555 2:086272a2da1c 30
nakedt555 2:086272a2da1c 31 Vec3f world_;
nakedt555 2:086272a2da1c 32 Vec3f velocity_;
nakedt555 2:086272a2da1c 33 Vec3f world_velocity_;
nakedt555 2:086272a2da1c 34
nakedt555 3:a45557a0dcb8 35 //For amcl
nakedt555 3:a45557a0dcb8 36 Vec3f initialpose_;
nakedt555 3:a45557a0dcb8 37 Vec3f drift_;
nakedt555 3:a45557a0dcb8 38
nakedt555 3:a45557a0dcb8 39 //For TIM IT
nakedt555 3:a45557a0dcb8 40 Ticker *it_tick;
nakedt555 3:a45557a0dcb8 41
nakedt555 1:bdd17feaa4ce 42 public:
nakedt555 2:086272a2da1c 43 //Constructor
nakedt555 1:bdd17feaa4ce 44 Odom(){
nakedt555 2:086272a2da1c 45 initialize();
nakedt555 2:086272a2da1c 46 }
nakedt555 2:086272a2da1c 47 //TIM IT Callback function
nakedt555 2:086272a2da1c 48 void update_it_cb(){
nakedt555 2:086272a2da1c 49 calc_odometry();
nakedt555 2:086272a2da1c 50 }
nakedt555 2:086272a2da1c 51
nakedt555 2:086272a2da1c 52 //Update world angle
nakedt555 2:086272a2da1c 53 void update_angle();
nakedt555 2:086272a2da1c 54
nakedt555 3:a45557a0dcb8 55 //Getter
nakedt555 3:a45557a0dcb8 56 Vec3f get_world(){
nakedt555 3:a45557a0dcb8 57 return world_;
nakedt555 3:a45557a0dcb8 58 }
nakedt555 3:a45557a0dcb8 59 Vec3f get_initialpose(){
nakedt555 3:a45557a0dcb8 60 return initialpose_;
nakedt555 3:a45557a0dcb8 61 }
nakedt555 3:a45557a0dcb8 62 Vec3f get_drift(){
nakedt555 3:a45557a0dcb8 63 return drift_;
nakedt555 3:a45557a0dcb8 64 }
nakedt555 3:a45557a0dcb8 65
nakedt555 3:a45557a0dcb8 66 //Setter
nakedt555 3:a45557a0dcb8 67 void set_initialpose(Vec3f initialpose){
nakedt555 3:a45557a0dcb8 68 initialpose_ = initialpose;
nakedt555 3:a45557a0dcb8 69 }
nakedt555 3:a45557a0dcb8 70 void set_drift(Vec3f drift){
nakedt555 3:a45557a0dcb8 71 drift_ = drift;
nakedt555 3:a45557a0dcb8 72 }
nakedt555 3:a45557a0dcb8 73
nakedt555 2:086272a2da1c 74 private:
nakedt555 2:086272a2da1c 75 void initialize();
nakedt555 2:086272a2da1c 76 void calc_odometry();
nakedt555 1:bdd17feaa4ce 77 };
nakedt555 1:bdd17feaa4ce 78
nakedt555 1:bdd17feaa4ce 79 class Odom_Abstract
nakedt555 1:bdd17feaa4ce 80 {
nakedt555 1:bdd17feaa4ce 81 private:
nakedt555 1:bdd17feaa4ce 82 Odom *odom_;
nakedt555 1:bdd17feaa4ce 83
nakedt555 1:bdd17feaa4ce 84 public:
nakedt555 1:bdd17feaa4ce 85 Odom_Abstract(){
nakedt555 1:bdd17feaa4ce 86
nakedt555 1:bdd17feaa4ce 87 }
nakedt555 1:bdd17feaa4ce 88
nakedt555 1:bdd17feaa4ce 89 Odom_Abstract(Odom *odom) : odom_(odom){
nakedt555 1:bdd17feaa4ce 90
nakedt555 1:bdd17feaa4ce 91 }
nakedt555 3:a45557a0dcb8 92
nakedt555 3:a45557a0dcb8 93 virtual void loop() = 0;
nakedt555 1:bdd17feaa4ce 94
nakedt555 3:a45557a0dcb8 95 protected:
nakedt555 3:a45557a0dcb8 96 void set_instance(Odom* odom){
nakedt555 3:a45557a0dcb8 97 odom_ = odom;
nakedt555 3:a45557a0dcb8 98 }
nakedt555 3:a45557a0dcb8 99
nakedt555 3:a45557a0dcb8 100 void set_initial_pose(Vec3f initialpose){
nakedt555 3:a45557a0dcb8 101 odom_->set_initialpose(initialpose);
nakedt555 1:bdd17feaa4ce 102 }
nakedt555 1:bdd17feaa4ce 103
nakedt555 1:bdd17feaa4ce 104 Vec3f get_initial_pose(){
nakedt555 3:a45557a0dcb8 105 return odom_->get_initialpose();
nakedt555 3:a45557a0dcb8 106 }
nakedt555 3:a45557a0dcb8 107
nakedt555 3:a45557a0dcb8 108 Vec3f get_pose(){
nakedt555 3:a45557a0dcb8 109 return odom_->get_world();
nakedt555 1:bdd17feaa4ce 110 }
nakedt555 1:bdd17feaa4ce 111
nakedt555 1:bdd17feaa4ce 112 void set_drift(Vec3f drift){
nakedt555 3:a45557a0dcb8 113 odom_->set_drift(drift);
nakedt555 1:bdd17feaa4ce 114 }
nakedt555 1:bdd17feaa4ce 115
nakedt555 1:bdd17feaa4ce 116 Vec3f get_drift(){
nakedt555 3:a45557a0dcb8 117 return odom_->get_drift();
nakedt555 1:bdd17feaa4ce 118 }
nakedt555 1:bdd17feaa4ce 119
nakedt555 1:bdd17feaa4ce 120 };
nakedt555 1:bdd17feaa4ce 121 #endif