Diff: main.cpp

Revision:

1:d7ceb38da3d8

Parent:

0:6db16ad02a1b

--- a/main.cpp	Sat Aug 04 13:07:23 2018 +0000
+++ b/main.cpp	Mon Aug 06 03:04:47 2018 +0000
@@ -15,16 +15,16 @@
 //(X, Y, θ, speed, angle, injection, 補給昇降)
 float position[10][7] = {{0.0, 0.0, 0.0, 0, 90, 0, 0.0},//x移動, 角度変化
                          {-0.3, 0.0, 0.0, 0, 90, 0, 0.0},//y移動
-                         {-0.3, 5.0, 0.0, 0, 90, 0, 0.0},//発射
+                         {-0.3, 15.0, 0.0, 0, 90, 0, 0.0},//発射
                          {-0.3, 0.0, 0.0, 0, 90, 0, 0.0},//y移動,角度戻す, P上昇
                          
                          {-0.6, 0.0, 0.0, 0, 90, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
-                         {-0.6, 5.0, 0.0, 0, 90, 0, 0.0},//y移動
+                         {-0.6, 15.0, 0.0, 0, 90, 0, 0.0},//y移動
                          {-0.6, 0.0, 0.0, 0, 90, 0, 0.0},//発射
                          //{6.0, 0.0, 0.0, 20, 90, 0, 6.8},//y移動,角度戻す, P上昇
                          
                          {-0.9, 0.0, 0.0, 20, 90, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
-                         {-0.9, 5.0, 0.0, 20, 90, 0, 0.0},//y移動
+                         {-0.9, 15.0, 0.0, 20, 90, 0, 0.0},//y移動
                          {-0.9, 0.0, 0.0, 20, 90, 0, 0.0},//発射
                          //{7.0, 0.0, 0.0, 20, 90, 0, 6.8},//y移動,角度戻す, P上昇
                          /*
@@ -42,15 +42,19 @@
 //bool isConvergence(int num);
 
 //x軸補正用 PID
-PID pidRobotX(2, 0, 0, 0.01, 0.3);
+PID pidRobotX(2, 0, 0, 0.01, 0.3, &timer);
 float target_x = 0;
 
 //y軸補正用 PID
-PID pidRobotY(2, 0, 0, 0.01, 0.3);
+PID pidRobotY(2, 0, 0, 0.01, 0.3, &timer);
 float target_y = 0;
 
+//yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
+PID pidRobotYow(0.05, 0, 0, 0.01, 0.95, &timer);
+float target_yow = 0;
+    
 //USS用
-PID pidUss(0.02, 0, 0, 0.01, 0.3);
+PID pidUss(0.02, 0, 0, 0.01, 0.3, &timer);
 float target_uss = 25.0;
 
 int main()
@@ -62,14 +66,6 @@
     imu.performCalibration();
     imu.startAngleComputing();
 
-    //yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
-    PID pidRobotYow(0.05, 0, 0, 0.01, 0.95);
-    float target = 0;
-    mecanum.imu_yow = &imu.angle[2];
-    pidRobotYow.sensor = &imu.angle[2];
-    pidRobotYow.target = ⌖
-    pidRobotYow.start();
-
     for(int i; i < 3; i++)
         enc[i].changeDirection();
 
@@ -93,18 +89,44 @@
     pidRobotY.sensor = &odm.y;
     pidRobotY.target = &target_y;
     pidRobotY.start();
-
+    
+    //IMUyow
+    mecanum.imu_yow = &imu.angle[2];
+    pidRobotYow.sensor = &imu.angle[2];
+    pidRobotYow.target = &target_yow;
+    pidRobotYow.start();
+    
     //USS  pid設定
     uss.startTriger();
     pidUss.sensor = &uss.distance;
     pidUss.target = &target_uss;
     pidUss.start();
-
+    
+    pidRobotX.allowable_error = 0.1;
+    pidRobotY.allowable_error = 0.1;
+    pidRobotYow.allowable_error = 2;
+    pidUss.allowable_error = 3;
+    
     while(1) 
     {
         controller cmd = getPropoData();   //getPropoData & getCanData
         
         static int posi_num = 0;
+        
+        if(position[posi_num][1] >= 10)
+        {
+            if(pidRobotX.isConvergence(1) == 1 && pidRobotYow.isConvergence(1) == 1 && pidUss.isConvergence(1) == 1)
+                posi_num++;
+        }
+        else if(pidRobotX.isConvergence(1) + pidRobotYow.isConvergence(1) + pidRobotY.isConvergence(1) == 3)
+                posi_num++;
+        
+        
+        if(posi_num >= 9)
+            posi_num = 0;
+        
+        
+        /*
         static int reset_swich = 0;
         if(cmd.H == 2 && reset_swich == 0)
         {
@@ -115,7 +137,8 @@
         }
         else if(cmd.H == 0 && reset_swich == 1)
             reset_swich = 0;
-
+        */
+        /*
         if(position[posi_num][5] == 1)
             can.set(1, 1, position[posi_num][3]);
         else
@@ -123,6 +146,7 @@
         can.set(1, 2, position[posi_num][4]);
         can.set(2, 1, position[posi_num][6]);
         can.send();
+        */
         
         //ロボットの移動速度(LX, LY, RX)
         float robot_velocity[3];
@@ -136,9 +160,9 @@
         robot_velocity[0] = pidRobotX.output;
         
         //USS距離調整処理
-        if(posi_num == 2 || posi_num == 5 || posi_num == 8)
+        if(position[posi_num][1] >= 10)
         {
-            *pidUss.target = position[posi_num][1];
+            *pidUss.target = position[posi_num][1] - 10;
             robot_velocity[1] = -pidUss.output;
         }
         else 
@@ -167,12 +191,22 @@
         //モーターの駆動
         for(int i = 0; i < 4; i++)
             Motor[i].drive(mecanum.wheel_vel[i]);
-        
-        //posi_num += isConvergence(posi_num);
-        
+
         pc.printf("%.2f\t", odm.x);
         pc.printf("%.2f\t", odm.y);
+        pc.printf("%.2f\t", *pidRobotYow.sensor);
         
+        pc.printf("%.2f\t", *pidRobotX.target);
+        pc.printf("%.2f\t", *pidRobotY.target);
+        pc.printf("%.2f\t", *pidRobotYow.target);
+        
+        pc.printf("%d\t", pidRobotX.isConvergence(1));
+        pc.printf("%d\t", pidRobotY.isConvergence(1));
+        pc.printf("%d\t", pidRobotYow.isConvergence(1));
+        pc.printf("%d\t", pidUss.isConvergence(1));
+        
+        pc.printf("%d\t", posi_num);
+                
         pc.printf("\n");
         wait(0.02);
     }