Diff: robot_operator.h

Revision:

0:c177452db984

Child:

1:b20c9bcfb254

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot_operator.h	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,160 @@
+#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;
+
+
+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)
+{
+    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();
+}
+
+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);
+    path++;
+    if(path >= pathSize) 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();
+        
+        // 制御を停止
+        ticker_pose_controll.detach();
+    }
+}