WRS2020用にメカナム台車をROS化
Dependencies: mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node
Dependents: WRS2020_mecanum_node
MyOdometer.h@0:c177452db984, 2020-11-02 (annotated)
- Committer:
- sgrsn
- Date:
- Mon Nov 02 09:00:16 2020 +0000
- Revision:
- 0:c177452db984
- Child:
- 4:40167450cdf0
First commit
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:c177452db984 | 66 | float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67; |
sgrsn | 0:c177452db984 | 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 |