WRS2020用にメカナム台車をROS化
Dependencies: mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node
Dependents: WRS2020_mecanum_node
MyOdometer.h
- Committer:
- sgrsn
- Date:
- 2020-11-02
- Revision:
- 0:c177452db984
- Child:
- 4:40167450cdf0
File content as of revision 0:c177452db984:
#ifndef MY_ODOMETER_H #define MY_ODOMETER_H #include "JY901.h" #include "define.h" class CountWheel { public: CountWheel(Timer *t) { _t = t; _t -> start(); } float getRadian(float frequency) { last_time = current_time; current_time = _t -> read(); float dt = current_time - last_time; float delta_rad = deg_per_pulse * frequency * dt * DEG_TO_RAD; return delta_rad; } private: Timer *_t; float last_time; float current_time; }; Timer tmp_t; CountWheel counter[4] = {CountWheel(&tmp_t), CountWheel(&tmp_t), CountWheel(&tmp_t), CountWheel(&tmp_t)}; class MyOdometer { public: float x; float y; float theta; float vel_x; float vel_y; float vel_theta; float wheel_frequency[4]; MyOdometer(JY901 *jy901) { x = 0; y = 0; theta = 0; vel_x = 0; vel_y = 0; vel_theta = 0; jy901_ = jy901; vel_timer.start(); } void update() { theta = jy901_ -> calculateAngleOnlyGyro() * DEG_TO_RAD; //theta = jy901_ -> getZaxisAngle() * DEG_TO_RAD; for(int i = 0; i < MOTOR_NUM; i++) { wheel_rad[i] = counter[i].getRadian(wheel_frequency[i]); } float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67; float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67; x += dx * cos(theta) + dy * sin(-theta); y += dy * cos(theta) + dx * sin(theta); 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; 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; float current_time = vel_timer.read(); float dt = (current_time - last_time); vel_theta = (theta - last_theta) / dt; last_theta = theta; last_time = current_time; } private: JY901 *jy901_; Timer vel_timer; float wheel_rad[MOTOR_NUM]; float last_time; float last_theta; }; #endif