hidaka sato / Mbed 2 deprecated WRS2020_mecanum_node

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?

UserRevisionLine numberNew 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 }