WRS2020用にメカナム台車をROS化
Dependencies: mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node
Dependents: WRS2020_mecanum_node
robot_operator.h@0:c177452db984, 2020-11-02 (annotated)
- Committer:
- sgrsn
- Date:
- Mon Nov 02 09:00:16 2020 +0000
- Revision:
- 0:c177452db984
- Child:
- 1:b20c9bcfb254
First 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 | 0:c177452db984 | 72 | |
sgrsn | 0:c177452db984 | 73 | Timer timer2; |
sgrsn | 0:c177452db984 | 74 | PID pid_x(&timer2); |
sgrsn | 0:c177452db984 | 75 | PID pid_y(&timer2); |
sgrsn | 0:c177452db984 | 76 | PID pid_theta(&timer2); |
sgrsn | 0:c177452db984 | 77 | |
sgrsn | 0:c177452db984 | 78 | void robot_vel_controll(float vel_x, float vel_y, float vel_theta); |
sgrsn | 0:c177452db984 | 79 | void moveStart(float pose_x, float pose_y, float pose_theta); |
sgrsn | 0:c177452db984 | 80 | void moveStop(); |
sgrsn | 0:c177452db984 | 81 | void pose_controll(); |
sgrsn | 0:c177452db984 | 82 | |
sgrsn | 0:c177452db984 | 83 | |
sgrsn | 0:c177452db984 | 84 | void robot_vel_controll(float vel_x, float vel_y, float vel_theta) |
sgrsn | 0:c177452db984 | 85 | { |
sgrsn | 0:c177452db984 | 86 | float v[MOTOR_NUM] = {}; |
sgrsn | 0:c177452db984 | 87 | // 各車輪の速度 |
sgrsn | 0:c177452db984 | 88 | v[0] = -vel_x - vel_y - L * vel_theta; // m/sec |
sgrsn | 0:c177452db984 | 89 | v[1] = vel_x - vel_y - L * vel_theta; |
sgrsn | 0:c177452db984 | 90 | v[2] = vel_x + vel_y - L * vel_theta; |
sgrsn | 0:c177452db984 | 91 | v[3] = -vel_x + vel_y - L * vel_theta; |
sgrsn | 0:c177452db984 | 92 | |
sgrsn | 0:c177452db984 | 93 | // 本当はこうするべき |
sgrsn | 0:c177452db984 | 94 | // f = v * ppr / ( 2*PI * r); |
sgrsn | 0:c177452db984 | 95 | // controlMotor(i, (int)f[i]); |
sgrsn | 0:c177452db984 | 96 | |
sgrsn | 0:c177452db984 | 97 | for(int i = 0; i < MOTOR_NUM; i++) |
sgrsn | 0:c177452db984 | 98 | { |
sgrsn | 0:c177452db984 | 99 | float f = v[i] / wheel_r * RAD_TO_DEG / deg_per_pulse; |
sgrsn | 0:c177452db984 | 100 | controlMotor(i, (int)f); |
sgrsn | 0:c177452db984 | 101 | } |
sgrsn | 0:c177452db984 | 102 | } |
sgrsn | 0:c177452db984 | 103 | |
sgrsn | 0:c177452db984 | 104 | void moveStart(float pose_x, float pose_y, float pose_theta) |
sgrsn | 0:c177452db984 | 105 | { |
sgrsn | 0:c177452db984 | 106 | pid_x.setParameter(0.40, 0.0, 0.0); |
sgrsn | 0:c177452db984 | 107 | pid_y.setParameter(0.40, 0.0, 0.0); |
sgrsn | 0:c177452db984 | 108 | pid_theta.setParameter(0.10, 0.0, 0.0); |
sgrsn | 0:c177452db984 | 109 | |
sgrsn | 0:c177452db984 | 110 | pathSize = myPath.makePath(pose_x*1000, pose_y*1000, pose_theta*RAD_TO_DEG, |
sgrsn | 0:c177452db984 | 111 | my_odometry.x*1000, my_odometry.y*1000, my_odometry.theta*RAD_TO_DEG); |
sgrsn | 0:c177452db984 | 112 | ticker_pose_controll.attach(&pose_controll, 0.010); |
sgrsn | 0:c177452db984 | 113 | path = 0; |
sgrsn | 0:c177452db984 | 114 | } |
sgrsn | 0:c177452db984 | 115 | |
sgrsn | 0:c177452db984 | 116 | void moveStop() |
sgrsn | 0:c177452db984 | 117 | { |
sgrsn | 0:c177452db984 | 118 | ticker_pose_controll.detach(); |
sgrsn | 0:c177452db984 | 119 | } |
sgrsn | 0:c177452db984 | 120 | |
sgrsn | 0:c177452db984 | 121 | void pose_controll() |
sgrsn | 0:c177452db984 | 122 | { |
sgrsn | 0:c177452db984 | 123 | float x_robot = my_odometry.x*1000; |
sgrsn | 0:c177452db984 | 124 | float y_robot = my_odometry.y*1000; |
sgrsn | 0:c177452db984 | 125 | float theta_robot = my_odometry.theta; |
sgrsn | 0:c177452db984 | 126 | |
sgrsn | 0:c177452db984 | 127 | // 目標位置判定 |
sgrsn | 0:c177452db984 | 128 | float err_x = myPath.target_x - x_robot; |
sgrsn | 0:c177452db984 | 129 | float err_y = myPath.target_y - y_robot; |
sgrsn | 0:c177452db984 | 130 | float err_theta = myPath.target_z - theta_robot; |
sgrsn | 0:c177452db984 | 131 | |
sgrsn | 0:c177452db984 | 132 | // 目標速度計算 |
sgrsn | 0:c177452db984 | 133 | // 慣性座標での速度 |
sgrsn | 0:c177452db984 | 134 | float xI_dot = pid_x.controlPID(myPath.getPathX(path), x_robot); |
sgrsn | 0:c177452db984 | 135 | float yI_dot = pid_y.controlPID(myPath.getPathY(path), y_robot); |
sgrsn | 0:c177452db984 | 136 | float theta_dot = pid_theta.controlPID( float( myPath.getPathZ(path))*DEG_TO_RAD, theta_robot); |
sgrsn | 0:c177452db984 | 137 | path++; |
sgrsn | 0:c177452db984 | 138 | if(path >= pathSize) path = pathSize-1; |
sgrsn | 0:c177452db984 | 139 | |
sgrsn | 0:c177452db984 | 140 | // ロボット座標での速度 |
sgrsn | 0:c177452db984 | 141 | float x_dot = cos(theta_robot) * xI_dot + sin(theta_robot) * yI_dot; |
sgrsn | 0:c177452db984 | 142 | float y_dot = -sin(theta_robot) * xI_dot + cos(theta_robot) * yI_dot; |
sgrsn | 0:c177452db984 | 143 | |
sgrsn | 0:c177452db984 | 144 | robot_vel_controll(x_dot/1000, y_dot/1000, theta_dot); |
sgrsn | 0:c177452db984 | 145 | |
sgrsn | 0:c177452db984 | 146 | // 目標位置到達した場合 |
sgrsn | 0:c177452db984 | 147 | if ( abs(err_x) < threshold_x && abs(err_y) < threshold_y && abs(err_theta) < threshold_theta) |
sgrsn | 0:c177452db984 | 148 | { |
sgrsn | 0:c177452db984 | 149 | // 車輪を停止 |
sgrsn | 0:c177452db984 | 150 | coastAllMotor(); |
sgrsn | 0:c177452db984 | 151 | //pid_x.reset(); |
sgrsn | 0:c177452db984 | 152 | //pid_y.reset(); |
sgrsn | 0:c177452db984 | 153 | //pid_theta.reset(); |
sgrsn | 0:c177452db984 | 154 | wait_ms(500); |
sgrsn | 0:c177452db984 | 155 | jy901.reset(); |
sgrsn | 0:c177452db984 | 156 | |
sgrsn | 0:c177452db984 | 157 | // 制御を停止 |
sgrsn | 0:c177452db984 | 158 | ticker_pose_controll.detach(); |
sgrsn | 0:c177452db984 | 159 | } |
sgrsn | 0:c177452db984 | 160 | } |