hidaka sato / Mbed 2 deprecated WRS2020_mecanum_node

WRS2020用にメカナム台車をROS化

Dependencies:   mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node

Dependents:   WRS2020_mecanum_node

MyOdometer.h@5:17ede03f1fa5, 2021-02-23 (annotated)

Committer:
sgrsn
Date:
Tue Feb 23 10:22:26 2021 +0000
Revision:
5:17ede03f1fa5
Parent:
4:40167450cdf0 
Lateset commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:c177452db984 1 #ifndef MY_ODOMETER_H
sgrsn 0:c177452db984 2 #define MY_ODOMETER_H
sgrsn 0:c177452db984 3
sgrsn 0:c177452db984 4 #include "JY901.h"
sgrsn 0:c177452db984 5 #include "define.h"
sgrsn 0:c177452db984 6
sgrsn 0:c177452db984 7 class CountWheel
sgrsn 0:c177452db984 8 {
sgrsn 0:c177452db984 9 public:
sgrsn 0:c177452db984 10 CountWheel(Timer *t)
sgrsn 0:c177452db984 11 {
sgrsn 0:c177452db984 12 _t = t;
sgrsn 0:c177452db984 13 _t -> start();
sgrsn 0:c177452db984 14 }
sgrsn 0:c177452db984 15 float getRadian(float frequency)
sgrsn 0:c177452db984 16 {
sgrsn 0:c177452db984 17 last_time = current_time;
sgrsn 0:c177452db984 18 current_time = _t -> read();
sgrsn 0:c177452db984 19 float dt = current_time - last_time;
sgrsn 0:c177452db984 20 float delta_rad = deg_per_pulse * frequency * dt * DEG_TO_RAD;
sgrsn 0:c177452db984 21 return delta_rad;
sgrsn 0:c177452db984 22 }
sgrsn 0:c177452db984 23
sgrsn 0:c177452db984 24 private:
sgrsn 0:c177452db984 25 Timer *_t;
sgrsn 0:c177452db984 26 float last_time;
sgrsn 0:c177452db984 27 float current_time;
sgrsn 0:c177452db984 28 };
sgrsn 0:c177452db984 29
sgrsn 0:c177452db984 30
sgrsn 0:c177452db984 31 Timer tmp_t;
sgrsn 0:c177452db984 32 CountWheel counter[4] = {CountWheel(&tmp_t), CountWheel(&tmp_t), CountWheel(&tmp_t), CountWheel(&tmp_t)};
sgrsn 0:c177452db984 33
sgrsn 0:c177452db984 34 class MyOdometer
sgrsn 0:c177452db984 35 {
sgrsn 0:c177452db984 36 public:
sgrsn 0:c177452db984 37 float x;
sgrsn 0:c177452db984 38 float y;
sgrsn 0:c177452db984 39 float theta;
sgrsn 0:c177452db984 40
sgrsn 0:c177452db984 41 float vel_x;
sgrsn 0:c177452db984 42 float vel_y;
sgrsn 0:c177452db984 43 float vel_theta;
sgrsn 0:c177452db984 44
sgrsn 0:c177452db984 45 float wheel_frequency[4];
sgrsn 0:c177452db984 46
sgrsn 0:c177452db984 47 MyOdometer(JY901 *jy901)
sgrsn 0:c177452db984 48 {
sgrsn 0:c177452db984 49 x = 0;
sgrsn 0:c177452db984 50 y = 0;
sgrsn 0:c177452db984 51 theta = 0;
sgrsn 0:c177452db984 52 vel_x = 0;
sgrsn 0:c177452db984 53 vel_y = 0;
sgrsn 0:c177452db984 54 vel_theta = 0;
sgrsn 0:c177452db984 55 jy901_ = jy901;
sgrsn 0:c177452db984 56 vel_timer.start();
sgrsn 0:c177452db984 57 }
sgrsn 0:c177452db984 58 void update()
sgrsn 0:c177452db984 59 {
sgrsn 0:c177452db984 60 theta = jy901_ -> calculateAngleOnlyGyro() * DEG_TO_RAD;
sgrsn 0:c177452db984 61 //theta = jy901_ -> getZaxisAngle() * DEG_TO_RAD;
sgrsn 0:c177452db984 62 for(int i = 0; i < MOTOR_NUM; i++)
sgrsn 0:c177452db984 63 {
sgrsn 0:c177452db984 64 wheel_rad[i] = counter[i].getRadian(wheel_frequency[i]);
sgrsn 0:c177452db984 65 }
sgrsn 4:40167450cdf0 66 float dx = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
sgrsn 4:40167450cdf0 67 float dy = ( wheel_rad[0] - wheel_rad[1] - wheel_rad[2] + wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
sgrsn 0:c177452db984 68
sgrsn 0:c177452db984 69 x += dx * cos(theta) + dy * sin(-theta);
sgrsn 0:c177452db984 70 y += dy * cos(theta) + dx * sin(theta);
sgrsn 0:c177452db984 71
sgrsn 0:c177452db984 72 vel_x = (-wheel_frequency[0] + wheel_frequency[1] + wheel_frequency[2] - wheel_frequency[3])/4.0 * deg_per_pulse * DEG_TO_RAD * wheel_r;//*0.3535534 * 0.67;
sgrsn 0:c177452db984 73
sgrsn 0:c177452db984 74 vel_y = (-wheel_frequency[0] - wheel_frequency[1] + wheel_frequency[2] + wheel_frequency[3])/4.0 * deg_per_pulse * DEG_TO_RAD * wheel_r;//*0.3535534 * 0.67;
sgrsn 0:c177452db984 75
sgrsn 0:c177452db984 76 float current_time = vel_timer.read();
sgrsn 0:c177452db984 77 float dt = (current_time - last_time);
sgrsn 0:c177452db984 78 vel_theta = (theta - last_theta) / dt;
sgrsn 0:c177452db984 79 last_theta = theta;
sgrsn 0:c177452db984 80 last_time = current_time;
sgrsn 0:c177452db984 81 }
sgrsn 0:c177452db984 82
sgrsn 0:c177452db984 83 private:
sgrsn 0:c177452db984 84 JY901 *jy901_;
sgrsn 0:c177452db984 85 Timer vel_timer;
sgrsn 0:c177452db984 86 float wheel_rad[MOTOR_NUM];
sgrsn 0:c177452db984 87 float last_time;
sgrsn 0:c177452db984 88 float last_theta;
sgrsn 0:c177452db984 89 };
sgrsn 0:c177452db984 90
sgrsn 0:c177452db984 91 #endif
sgrsn 0:c177452db984 92