WRS2020用にメカナム台車をROS化
Dependencies: mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node
Dependents: WRS2020_mecanum_node
robot_operator.h
- Committer:
- sgrsn
- Date:
- 2020-11-12
- Revision:
- 4:40167450cdf0
- Parent:
- 3:cf06189a7511
File content as of revision 4:40167450cdf0:
#include "motor_controller.h" #include "MyOdometer.h" #include "define.h" #include "PID.h" // 目標位置制御用の制御周期 const float pose_tick = 0.010; // sec const float L = 0.233; //233 [mm] class MakePath { public: int target_x; int target_y; int target_z; MakePath() { } int makePath(int targetX, int targetY, int targetZ, int x, int y, int z) { target_x = targetX; target_y = targetY; target_z = targetZ; int num = float( sqrt( double( abs(targetX-x)*abs(targetX-x) + abs(targetY-y)*abs(targetY-y) ) )+ abs(targetZ-z) ) / 5.0; //5mm間隔 //printf("num = %d\r\n", num); for(int i = 1; i <= num; i++) { float a = float(i) / num; PATH[i-1][0] = x + (float(targetX - x) * a);// * cos( last_angleZ*DEG_TO_RAD); PATH[i-1][1] = y + (float(targetY - y)* a);// * sin(-last_angleZ*DEG_TO_RAD); PATH[i-1][2] = z + float(targetZ - z) * a; } if(num > 0) { PATH[num-1][0] = targetX; PATH[num-1][1] = targetY; PATH[num-1][2] = targetZ; } else { PATH[0][0] = targetX; PATH[0][1] = targetY; PATH[0][2] = targetZ; num = 1; } return num; } int getPathX(int i) { return PATH[i][0]; } int getPathY(int i) { return PATH[i][1]; } int getPathZ(int i) { return PATH[i][2]; } private: int PATH[500][3]; }; Ticker ticker_pose_controll; MakePath myPath; int path = 0; int pathSize; bool bool_tmp = true; bool *is_stop_ptr_ = &bool_tmp; Timer timer2; PID pid_x(&timer2); PID pid_y(&timer2); PID pid_theta(&timer2); void robot_vel_controll(float vel_x, float vel_y, float vel_theta); void moveStart(float pose_x, float pose_y, float pose_theta); void moveStop(); void pose_controll(); void robot_vel_controll(float vel_x, float vel_y, float vel_theta) { float v[MOTOR_NUM] = {}; // 各車輪の速度 v[0] = -vel_x + vel_y - L * vel_theta; // m/sec v[1] = -vel_x - vel_y - L * vel_theta; v[2] = vel_x - vel_y - L * vel_theta; v[3] = vel_x + vel_y - L * vel_theta; // 本当はこうするべき // f = v * ppr / ( 2*PI * r); // controlMotor(i, (int)f[i]); for(int i = 0; i < MOTOR_NUM; i++) { float f = v[i] / wheel_r * RAD_TO_DEG / deg_per_pulse; controlMotor(i, (int)f); } } void moveStart(float pose_x, float pose_y, float pose_theta, bool* is_stop_pointer) { is_stop_ptr_ = is_stop_pointer; *is_stop_ptr_ = false; pid_x.setParameter(0.40, 0.0, 0.0); pid_y.setParameter(0.40, 0.0, 0.0); pid_theta.setParameter(0.10, 0.0, 0.0); pathSize = myPath.makePath(pose_x*1000, pose_y*1000, pose_theta*RAD_TO_DEG, my_odometry.x*1000, my_odometry.y*1000, my_odometry.theta*RAD_TO_DEG); ticker_pose_controll.attach(&pose_controll, 0.010); path = 0; } void moveStop() { ticker_pose_controll.detach(); *is_stop_ptr_ = true; } void pose_controll() { float x_robot = my_odometry.x*1000; float y_robot = my_odometry.y*1000; float theta_robot = my_odometry.theta; // 目標位置判定 float err_x = myPath.target_x - x_robot; float err_y = myPath.target_y - y_robot; float err_theta = myPath.target_z - theta_robot; // 目標速度計算 // 慣性座標での速度 float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot); float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot); float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot); if(path < pathSize-1) path++; else path = pathSize-1; // ロボット座標での速度 float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot; float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot; robot_vel_controll(x_dot/1000, y_dot/1000, theta_dot); // 目標位置到達した場合 if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta) { // 車輪を停止 coastAllMotor(); //pid_x.reset(); //pid_y.reset(); //pid_theta.reset(); wait_ms(500); jy901.reset(); // 制御を停止 moveStop(); } }