駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

odom.h@2:086272a2da1c, 2018-12-07 (annotated)

Committer:
nakedt555
Date:
Fri Dec 07 22:34:31 2018 +0000
Revision:
2:086272a2da1c
Parent:
1:bdd17feaa4ce 
12/08 work log;

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 1:bdd17feaa4ce 35 public:
nakedt555 2:086272a2da1c 36 //Constructor
nakedt555 1:bdd17feaa4ce 37 Odom(){
nakedt555 2:086272a2da1c 38 initialize();
nakedt555 2:086272a2da1c 39 }
nakedt555 2:086272a2da1c 40 //TIM IT Callback function
nakedt555 2:086272a2da1c 41 void update_it_cb(){
nakedt555 2:086272a2da1c 42 calc_odometry();
nakedt555 2:086272a2da1c 43 }
nakedt555 2:086272a2da1c 44
nakedt555 2:086272a2da1c 45 //Update world angle
nakedt555 2:086272a2da1c 46 void update_angle();
nakedt555 2:086272a2da1c 47
nakedt555 2:086272a2da1c 48 private:
nakedt555 2:086272a2da1c 49 void initialize();
nakedt555 2:086272a2da1c 50 void calc_odometry();
nakedt555 1:bdd17feaa4ce 51 };
nakedt555 1:bdd17feaa4ce 52
nakedt555 1:bdd17feaa4ce 53 class Odom_Abstract
nakedt555 1:bdd17feaa4ce 54 {
nakedt555 1:bdd17feaa4ce 55 private:
nakedt555 1:bdd17feaa4ce 56 Odom *odom_;
nakedt555 1:bdd17feaa4ce 57 Vec3f initial_pose_;
nakedt555 1:bdd17feaa4ce 58 Vec3f drift_;
nakedt555 1:bdd17feaa4ce 59
nakedt555 1:bdd17feaa4ce 60 public:
nakedt555 1:bdd17feaa4ce 61 Odom_Abstract(){
nakedt555 1:bdd17feaa4ce 62
nakedt555 1:bdd17feaa4ce 63 }
nakedt555 1:bdd17feaa4ce 64
nakedt555 1:bdd17feaa4ce 65 Odom_Abstract(Odom *odom) : odom_(odom){
nakedt555 1:bdd17feaa4ce 66
nakedt555 1:bdd17feaa4ce 67 }
nakedt555 1:bdd17feaa4ce 68
nakedt555 1:bdd17feaa4ce 69 void set_initial_pose(Vec3f initial_pose){
nakedt555 1:bdd17feaa4ce 70 initial_pose_ = initial_pose;
nakedt555 1:bdd17feaa4ce 71 }
nakedt555 1:bdd17feaa4ce 72
nakedt555 1:bdd17feaa4ce 73 Vec3f get_initial_pose(){
nakedt555 1:bdd17feaa4ce 74 return initial_pose_;
nakedt555 1:bdd17feaa4ce 75 }
nakedt555 1:bdd17feaa4ce 76
nakedt555 1:bdd17feaa4ce 77 void set_drift(Vec3f drift){
nakedt555 1:bdd17feaa4ce 78 drift_ = drift;
nakedt555 1:bdd17feaa4ce 79 }
nakedt555 1:bdd17feaa4ce 80
nakedt555 1:bdd17feaa4ce 81 Vec3f get_drift(){
nakedt555 1:bdd17feaa4ce 82 return drift_;
nakedt555 1:bdd17feaa4ce 83 }
nakedt555 1:bdd17feaa4ce 84
nakedt555 1:bdd17feaa4ce 85 // virtual void loop() = 0;
nakedt555 1:bdd17feaa4ce 86 };
nakedt555 1:bdd17feaa4ce 87 #endif