Diff: main.cpp

Revision:

1:d0d24c45ce32

Parent:

0:02e4c332f5b4

--- a/main.cpp	Thu Sep 22 11:45:34 2022 +0000
+++ b/main.cpp	Fri Sep 23 07:23:45 2022 +0000
@@ -24,26 +24,30 @@
 double D_Y = 0;//偏差の微分値
 double pre_hennsa_X;//Xのひとつ前の偏差
 double pre_hennsa_Y;//Yのひとつ前の偏差
-double Kp = 250;//PID制御のPの係数
-double Ki = 10;//PID制御のIの係数
-double Kd = 10;//PID制御のDの係数
+double Kp_X = 100;//PID制御のPの係数
+double Ki_X = 10;//PID制御のIの係数
+double Kd_X = 10;//PID制御のDの係数
+double Kp_Y = 100;//PID制御のPの係数
+double Ki_Y = 10;//PID制御のIの係数
+double Kd_Y = 10;//PID制御のDの係数
 double P_X = 0;//PID制御の計算値
 double P_Y = 0;//PID制御の計算値
 double PW1 = 0;//モーターの力
 double PW2 = 0;//モーターの力
 double PW3 = 0;//モーターの力
 double PW4 = 0;//モーターの力
-double dst_Y = 1154;//目的地の座標
-double dst_X = 643.3;//目的地の座標
+double dst_Y = 334.5;//X目的地の座標
+double dst_X = 600;//Y目的地の座標
 double taiyanoennshuu = 150.8;
 int bunnkainou = 200;
 double ball;//ボールセンサーの値
 int exist;//while文の条件のballの値
 #define HAZIMARI 1
-#define HENNKOU 2
-#define TENNKAI 3
+#define NIKAIME 2
+#define HENNKOU 3
+#define TEISI 4
+int mokuteki = HAZIMARI;
 bool flag = false;
-int mokuteki = HENNKOU;
 
 void flip()//タイマー割込み
 {
@@ -55,21 +59,60 @@
     hennsa_Y = dst_Y - taiyanoennshuu * (cnt_Y / (bunnkainou * 4));//距離の偏差
     I_X += (pre_hennsa_X + hennsa_X) * 0.01  / 2;//積分計算
     I_Y += (pre_hennsa_Y + hennsa_Y) * 0.01  / 2;//積分計算
-    D_X =  (pre_hennsa_X - hennsa_X) * 0.01  / 2;//微分計算
-    D_X =  (pre_hennsa_X - hennsa_X) * 0.01  / 2;//微分計算
+    D_X =  (hennsa_X - pre_hennsa_X) / 0.01 ;//微分計算
+    D_X =  (hennsa_X - pre_hennsa_X) / 0.01 ;//微分計算
     pre_hennsa_X = hennsa_X;//一個前の値に設定
     pre_hennsa_Y = hennsa_Y;//一個前の値に設定
-    P_X = Kp * hennsa_X / (bunnkainou * 4) + Ki * I_X / (bunnkainou * 4) + Kd * D_X / (bunnkainou * 4);//PID制御の計算
-    P_Y = Kp * hennsa_Y / (bunnkainou * 4) + Ki * I_Y / (bunnkainou * 4) + Kd * D_Y / (bunnkainou * 4);//PID制御の計算
-    PW3 = 1.5 * (P_Y - P_X);
-    PW4 = 1.5 * (P_Y + P_X);
-    PW1 = -1.0 * 1.5 * (P_Y - P_X);
-    PW2 = -1.0 * 1.5 * (P_Y + P_X);
+    P_X = Kp_X * hennsa_X / (bunnkainou * 4) + Ki_X * I_X / (bunnkainou * 4) + Kd_X * D_X / (bunnkainou * 4);//PID制御の計算
+    P_Y = Kp_Y * hennsa_Y / (bunnkainou * 4) + Ki_Y * I_Y / (bunnkainou * 4) + Kd_Y * D_Y / (bunnkainou * 4);//PID制御の計算
+    PW3 =2.0 * (P_Y - P_X);
+    PW4 = 2.0 * (P_Y + P_X);
+    PW1 = -1.0 * 2.0 * (P_Y - P_X);
+    PW2 = -1.0 * 2.0 * (P_Y + P_X);
+    if(abs(hennsa_X) <= 10 && abs(hennsa_Y)) {
+        PW3 = 0;
+        PW4 = 0;
+        PW1 = 0;
+        PW2 = 0;
+    }
+    switch(mokuteki) {
+        case HAZIMARI:
+            if(abs(hennsa_X) <= 100 && abs(hennsa_Y) <= 100){
+                dst_Y = 643.3;
+                dst_X = 1154;
+                mokuteki = NIKAIME;
+                }  
+        case NIKAIME:
+            if(abs(hennsa_X) <= 10 && abs(hennsa_Y) <= 10) {
+                dst_X = 2135;
+                dst_Y = 96.5;
+                Kp_X = 50;
+                Ki_X = 5;
+                Kd_X = 5;
+                Kp_Y = 125;
+                Ki_Y = 12.5;
+                Kd_Y = 10;
+                mokuteki = HENNKOU;
+            }
+            break;
+        case HENNKOU:
+            if(abs(hennsa_X) <= 10 && abs(hennsa_Y) <= 10) {
+                dst_X = 2135;
+                dst_Y = 437;
+                Kp_X = 50;
+                Ki_X = 5;
+                Kd_X = 5;
+                Kp_Y = 200;
+                Ki_Y = 20;
+                Kd_Y = 10;
+                mokuteki = TEISI;
+            }
+            break;
+    }
     flag = true;
 }
 int main()
 {
-    flipper.attach(&flip,0.01);
     while(i) {
         S = swich.read();
         pc.printf("%d \r\n",i);
@@ -79,26 +122,6 @@
     }
     flipper.attach(&flip,0.01);
     while(1) {
-        switch(mokuteki) {
-            case HAZIMARI:
-                dst_X = 643.3;
-                dst_Y = 1154;
-                if(abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) {
-                    mokuteki = HENNKOU;
-                }
-                break;
-            case HENNKOU:
-                dst_X = 96.5;
-                dst_Y = 2135;
-                if(abs(hennsa_X) <= 1 && abs(hennsa_Y) <= 1) {
-                    mokuteki = TENNKAI;
-                }
-                break;
-            case TENNKAI:
-                dst_X = 437;
-                dst_Y = 2135;
-                break;
-        }
         pc.printf("%d %lf %lf %lf %lf %lf %lf %lf %lf \r\n",i,kyori_X,kyori_Y,P_X,P_Y,PW1,PW2,PW3,PW4);
         sabertooth1.printf("M1:%d\r\n",(int)PW1);
         sabertooth1.printf("M2:%d\r\n",(int)PW2);