WRS2020用にメカナム台車をROS化
Dependencies: mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node
Dependents: WRS2020_mecanum_node
robot_operator.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?
User | Revision | Line number | New contents of line |
---|---|---|---|
sgrsn | 0:c177452db984 | 1 | #include "motor_controller.h" |
sgrsn | 0:c177452db984 | 2 | #include "MyOdometer.h" |
sgrsn | 0:c177452db984 | 3 | #include "define.h" |
sgrsn | 0:c177452db984 | 4 | #include "PID.h" |
sgrsn | 0:c177452db984 | 5 | |
sgrsn | 0:c177452db984 | 6 | // 目標位置制御用の制御周期 |
sgrsn | 0:c177452db984 | 7 | const float pose_tick = 0.010; // sec |
sgrsn | 0:c177452db984 | 8 | const float L = 0.233; //233 [mm] |
sgrsn | 0:c177452db984 | 9 | |
sgrsn | 0:c177452db984 | 10 | class MakePath |
sgrsn | 0:c177452db984 | 11 | { |
sgrsn | 0:c177452db984 | 12 | public: |
sgrsn | 0:c177452db984 | 13 | int target_x; |
sgrsn | 0:c177452db984 | 14 | int target_y; |
sgrsn | 0:c177452db984 | 15 | int target_z; |
sgrsn | 0:c177452db984 | 16 | |
sgrsn | 0:c177452db984 | 17 | MakePath() |
sgrsn | 0:c177452db984 | 18 | { |
sgrsn | 0:c177452db984 | 19 | } |
sgrsn | 0:c177452db984 | 20 | int makePath(int targetX, int targetY, int targetZ, int x, int y, int z) |
sgrsn | 0:c177452db984 | 21 | { |
sgrsn | 0:c177452db984 | 22 | target_x = targetX; |
sgrsn | 0:c177452db984 | 23 | target_y = targetY; |
sgrsn | 0:c177452db984 | 24 | target_z = targetZ; |
sgrsn | 0:c177452db984 | 25 | int num = float( sqrt( double( abs(targetX-x)*abs(targetX-x) + abs(targetY-y)*abs(targetY-y) ) )+ abs(targetZ-z) ) / 5.0; //5mm間隔 |
sgrsn | 0:c177452db984 | 26 | //printf("num = %d\r\n", num); |
sgrsn | 0:c177452db984 | 27 | for(int i = 1; i <= num; i++) |
sgrsn | 0:c177452db984 | 28 | { |
sgrsn | 0:c177452db984 | 29 | float a = float(i) / num; |
sgrsn | 0:c177452db984 | 30 | PATH[i-1][0] = x + (float(targetX - x) * a);// * cos( last_angleZ*DEG_TO_RAD); |
sgrsn | 0:c177452db984 | 31 | PATH[i-1][1] = y + (float(targetY - y)* a);// * sin(-last_angleZ*DEG_TO_RAD); |
sgrsn | 0:c177452db984 | 32 | PATH[i-1][2] = z + float(targetZ - z) * a; |
sgrsn | 0:c177452db984 | 33 | } |
sgrsn | 0:c177452db984 | 34 | if(num > 0) |
sgrsn | 0:c177452db984 | 35 | { |
sgrsn | 0:c177452db984 | 36 | PATH[num-1][0] = targetX; |
sgrsn | 0:c177452db984 | 37 | PATH[num-1][1] = targetY; |
sgrsn | 0:c177452db984 | 38 | PATH[num-1][2] = targetZ; |
sgrsn | 0:c177452db984 | 39 | } |
sgrsn | 0:c177452db984 | 40 | else |
sgrsn | 0:c177452db984 | 41 | { |
sgrsn | 0:c177452db984 | 42 | PATH[0][0] = targetX; |
sgrsn | 0:c177452db984 | 43 | PATH[0][1] = targetY; |
sgrsn | 0:c177452db984 | 44 | PATH[0][2] = targetZ; |
sgrsn | 0:c177452db984 | 45 | num = 1; |
sgrsn | 0:c177452db984 | 46 | } |
sgrsn | 0:c177452db984 | 47 | return num; |
sgrsn | 0:c177452db984 | 48 | } |
sgrsn | 0:c177452db984 | 49 | |
sgrsn | 0:c177452db984 | 50 | int getPathX(int i) |
sgrsn | 0:c177452db984 | 51 | { |
sgrsn | 0:c177452db984 | 52 | return PATH[i][0]; |
sgrsn | 0:c177452db984 | 53 | } |
sgrsn | 0:c177452db984 | 54 | int getPathY(int i) |
sgrsn | 0:c177452db984 | 55 | { |
sgrsn | 0:c177452db984 | 56 | return PATH[i][1]; |
sgrsn | 0:c177452db984 | 57 | } |
sgrsn | 0:c177452db984 | 58 | int getPathZ(int i) |
sgrsn | 0:c177452db984 | 59 | { |
sgrsn | 0:c177452db984 | 60 | return PATH[i][2]; |
sgrsn | 0:c177452db984 | 61 | } |
sgrsn | 0:c177452db984 | 62 | |
sgrsn | 0:c177452db984 | 63 | private: |
sgrsn | 0:c177452db984 | 64 | int PATH[500][3]; |
sgrsn | 0:c177452db984 | 65 | }; |
sgrsn | 0:c177452db984 | 66 | |
sgrsn | 0:c177452db984 | 67 | Ticker ticker_pose_controll; |
sgrsn | 0:c177452db984 | 68 | MakePath myPath; |
sgrsn | 0:c177452db984 | 69 | int path = 0; |
sgrsn | 0:c177452db984 | 70 | int pathSize; |
sgrsn | 0:c177452db984 | 71 | |
sgrsn | 1:b20c9bcfb254 | 72 | bool bool_tmp = true; |
sgrsn | 1:b20c9bcfb254 | 73 | bool *is_stop_ptr_ = &bool_tmp; |
sgrsn | 0:c177452db984 | 74 | |
sgrsn | 0:c177452db984 | 75 | Timer timer2; |
sgrsn | 0:c177452db984 | 76 | PID pid_x(&timer2); |
sgrsn | 0:c177452db984 | 77 | PID pid_y(&timer2); |
sgrsn | 0:c177452db984 | 78 | PID pid_theta(&timer2); |
sgrsn | 0:c177452db984 | 79 | |
sgrsn | 0:c177452db984 | 80 | void robot_vel_controll(float vel_x, float vel_y, float vel_theta); |
sgrsn | 0:c177452db984 | 81 | void moveStart(float pose_x, float pose_y, float pose_theta); |
sgrsn | 0:c177452db984 | 82 | void moveStop(); |
sgrsn | 0:c177452db984 | 83 | void pose_controll(); |
sgrsn | 0:c177452db984 | 84 | |
sgrsn | 0:c177452db984 | 85 | |
sgrsn | 0:c177452db984 | 86 | void robot_vel_controll(float vel_x, float vel_y, float vel_theta) |
sgrsn | 0:c177452db984 | 87 | { |
sgrsn | 0:c177452db984 | 88 | float v[MOTOR_NUM] = {}; |
sgrsn | 0:c177452db984 | 89 | // 各車輪の速度 |
sgrsn | 4:40167450cdf0 | 90 | v[0] = -vel_x + vel_y - L * vel_theta; // m/sec |
sgrsn | 4:40167450cdf0 | 91 | v[1] = -vel_x - vel_y - L * vel_theta; |
sgrsn | 4:40167450cdf0 | 92 | v[2] = vel_x - vel_y - L * vel_theta; |
sgrsn | 4:40167450cdf0 | 93 | v[3] = vel_x + vel_y - L * vel_theta; |
sgrsn | 0:c177452db984 | 94 | |
sgrsn | 0:c177452db984 | 95 | // 本当はこうするべき |
sgrsn | 0:c177452db984 | 96 | // f = v * ppr / ( 2*PI * r); |
sgrsn | 0:c177452db984 | 97 | // controlMotor(i, (int)f[i]); |
sgrsn | 0:c177452db984 | 98 | |
sgrsn | 0:c177452db984 | 99 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 0:c177452db984 | 100 | { |
sgrsn | 0:c177452db984 | 101 | float f = v[i] / wheel_r * RAD_TO_DEG / deg_per_pulse; |
sgrsn | 0:c177452db984 | 102 | controlMotor(i, (int)f); |
sgrsn | 0:c177452db984 | 103 | } |
sgrsn | 0:c177452db984 | 104 | } |
sgrsn | 0:c177452db984 | 105 | |
sgrsn | 1:b20c9bcfb254 | 106 | void moveStart(float pose_x, float pose_y, float pose_theta, bool* is_stop_pointer) |
sgrsn | 0:c177452db984 | 107 | { |
sgrsn | 1:b20c9bcfb254 | 108 | is_stop_ptr_ = is_stop_pointer; |
sgrsn | 1:b20c9bcfb254 | 109 | *is_stop_ptr_ = false; |
sgrsn | 0:c177452db984 | 110 | pid_x.setParameter(0.40, 0.0, 0.0); |
sgrsn | 0:c177452db984 | 111 | pid_y.setParameter(0.40, 0.0, 0.0); |
sgrsn | 0:c177452db984 | 112 | pid_theta.setParameter(0.10, 0.0, 0.0); |
sgrsn | 0:c177452db984 | 113 | |
sgrsn | 0:c177452db984 | 114 | pathSize = myPath.makePath(pose_x*1000, pose_y*1000, pose_theta*RAD_TO_DEG, |
sgrsn | 0:c177452db984 | 115 | my_odometry.x*1000, my_odometry.y*1000, my_odometry.theta*RAD_TO_DEG); |
sgrsn | 0:c177452db984 | 116 | ticker_pose_controll.attach(&pose_controll, 0.010); |
sgrsn | 0:c177452db984 | 117 | path = 0; |
sgrsn | 0:c177452db984 | 118 | } |
sgrsn | 0:c177452db984 | 119 | |
sgrsn | 0:c177452db984 | 120 | void moveStop() |
sgrsn | 0:c177452db984 | 121 | { |
sgrsn | 0:c177452db984 | 122 | ticker_pose_controll.detach(); |
sgrsn | 1:b20c9bcfb254 | 123 | *is_stop_ptr_ = true; |
sgrsn | 0:c177452db984 | 124 | } |
sgrsn | 0:c177452db984 | 125 | |
sgrsn | 0:c177452db984 | 126 | void pose_controll() |
sgrsn | 0:c177452db984 | 127 | { |
sgrsn | 0:c177452db984 | 128 | float x_robot = my_odometry.x*1000; |
sgrsn | 0:c177452db984 | 129 | float y_robot = my_odometry.y*1000; |
sgrsn | 0:c177452db984 | 130 | float theta_robot = my_odometry.theta; |
sgrsn | 0:c177452db984 | 131 | |
sgrsn | 0:c177452db984 | 132 | // 目標位置判定 |
sgrsn | 0:c177452db984 | 133 | float err_x = myPath.target_x - x_robot; |
sgrsn | 0:c177452db984 | 134 | float err_y = myPath.target_y - y_robot; |
sgrsn | 0:c177452db984 | 135 | float err_theta = myPath.target_z - theta_robot; |
sgrsn | 0:c177452db984 | 136 | |
sgrsn | 0:c177452db984 | 137 | // 目標速度計算 |
sgrsn | 0:c177452db984 | 138 | // 慣性座標での速度 |
sgrsn | 0:c177452db984 | 139 | float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot); |
sgrsn | 0:c177452db984 | 140 | float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot); |
sgrsn | 0:c177452db984 | 141 | float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot); |
sgrsn | 3:cf06189a7511 | 142 | |
sgrsn | 3:cf06189a7511 | 143 | if(path < pathSize-1) |
sgrsn | 3:cf06189a7511 | 144 | path++; |
sgrsn | 3:cf06189a7511 | 145 | else |
sgrsn | 3:cf06189a7511 | 146 | path = pathSize-1; |
sgrsn | 0:c177452db984 | 147 | |
sgrsn | 0:c177452db984 | 148 | // ロボット座標での速度 |
sgrsn | 0:c177452db984 | 149 | float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot; |
sgrsn | 0:c177452db984 | 150 | float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot; |
sgrsn | 0:c177452db984 | 151 | |
sgrsn | 0:c177452db984 | 152 | robot_vel_controll(x_dot/1000, y_dot/1000, theta_dot); |
sgrsn | 0:c177452db984 | 153 | |
sgrsn | 0:c177452db984 | 154 | // 目標位置到達した場合 |
sgrsn | 0:c177452db984 | 155 | if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta) |
sgrsn | 0:c177452db984 | 156 | { |
sgrsn | 0:c177452db984 | 157 | // 車輪を停止 |
sgrsn | 0:c177452db984 | 158 | coastAllMotor(); |
sgrsn | 0:c177452db984 | 159 | //pid_x.reset(); |
sgrsn | 0:c177452db984 | 160 | //pid_y.reset(); |
sgrsn | 0:c177452db984 | 161 | //pid_theta.reset(); |
sgrsn | 0:c177452db984 | 162 | wait_ms(500); |
sgrsn | 0:c177452db984 | 163 | jy901.reset(); |
sgrsn | 0:c177452db984 | 164 | |
sgrsn | 0:c177452db984 | 165 | // 制御を停止 |
sgrsn | 1:b20c9bcfb254 | 166 | moveStop(); |
sgrsn | 0:c177452db984 | 167 | } |
sgrsn | 0:c177452db984 | 168 | } |