Diff: main.cpp

Revision:

15:ae2043fde152

Parent:

14:8334c241bb0a

Child:

16:621f04b15f86

--- a/main.cpp	Mon Sep 17 08:52:05 2018 +0000
+++ b/main.cpp	Sat Sep 22 13:41:19 2018 +0000
@@ -14,18 +14,21 @@
 
 elements getRobotVelocity(state);
 state getTargetState();
-int updateStateNum();
+int updateStateNum(int tops_num);
+int updateTopsNum(int num);
+int changeNextTable(int num);
+state updateTopsState(float val, int tops_num);
 bool isConvergenceTops(int);
 bool isConvergenceSupply(int);
 controller getPropoData();
 void riseUssTriger();
 
 //x軸補正用 PID
-PID pidRobotX(4, 0, 0, 0.01, 0.3, &timer);
+PID pidRobotX(4, 0, 0, 0.01, 0.5, &timer);
 float target_x = 0;
 
 //y軸補正用 PID
-PID pidRobotY(2, 0, 0, 0.01, 0.3, &timer);
+PID pidRobotY(2, 0, 0, 0.01, 0.5, &timer);
 float target_y = 0;
 
 //yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
@@ -42,6 +45,8 @@
                       {0, 0, 0}
                      };
 Odometer odm(matrix, 0.050);
+
+int table_sw[9];
     
 int main()
 {
@@ -98,25 +103,14 @@
         riseUssTriger();
         can.read();
         
-        /*
-        int table_sw[9];
-        
-        for(int i = 0; i < 6; i++)
-            table_sw[i] = can.get(6, i + 1);
-        for(int i = 1; i < 4; i++)
-            table_sw[i + 5] = can.get(2, i);
-        
-        if(table_sw[8] == 0)
-            changeToBlueZone();
-        */
-        
         //目指すべきロボットの状態を取得
         state tar_state = getTargetState();
         
+        int can_angle = 900 - int(tar_state.angle * 10);
         //canに置く
         //角度は2倍して送って、2で割って受け取る  角度可変が0.5度刻みにできる
         can.set(1, 1, tar_state.shoot);
-        can.set(1, 2, int (tar_state.angle * 2));
+        can.set(1, 2, int(can_angle));
         can.set(1, 3, tar_state.supply);
         
         //送信周期調整
@@ -145,10 +139,19 @@
         pc.printf("%.2f\t", odm.x);
         pc.printf("%.2f\t", odm.y);
         pc.printf("%.2f\t", imu.angle[2]);
-        
+        /*
         int s1 = sw1, s2 = sw2;
         pc.printf("%d\t", s1);
         pc.printf("%d\t", s2);
+        */
+        int angle_pid = can.get(3, 2);
+        float angle_val = can.get(3, 4);
+        angle_val = 90 - (angle_val / 10.0);
+    
+        pc.printf("%d\t", angle_pid);
+        pc.printf("%.2f\t", angle_val);
+        pc.printf("%d\t", can_angle);
+        pc.printf("%.2f\t", tar_state.angle);
         
         pc.printf("%.2f\t", uss[0].distance);
         pc.printf("%.2f\t", uss[1].distance);
@@ -164,8 +167,8 @@
 state getTargetState()
 {
     static bool start_flag = 0;
-        
-    int num;
+    
+    int num[2];
     
     /*
     スタートボタン待機
@@ -176,32 +179,38 @@
     
     //sw1が1度も押されていないとき
     if(!start_flag)
-        num = odm.x = odm.y = imu.angle[2] = 0;
-    else num = updateStateNum();
+        num[0] = num[1] = odm.x = odm.y = imu.angle[2] = 0;
+    else 
+    {
+        num[0] = updateStateNum(num[1]);
+        num[1] = updateTopsNum(num[0]);
+    }
     
     //再びスタート待機
-    if(num == 0)
-        start_flag = 0;    
+    if(num[0] == 0)
+        start_flag = 0;
+    if(num[1] == 0)
+        num[0] = changeNextTable(num[0]);
     
     state a;
     //states辞典から値を引っ張る
-    a.x = state_lib[num][0];
-    a.y = state_lib[num][1];
-    a.theta = state_lib[num][2];
-    a.shoot = state_lib[num][3];
-    a.angle = state_lib[num][4];
-    a.supply = state_lib[num][5];
+    a.x = state_lib[num[0]][0];
+    a.y = state_lib[num[0]][1];
+    a.theta = state_lib[num[0]][2];
+    
+    updateTopsState(state_lib[num[0]][3], num[1]);
+    
     return a;
 }
 
-int updateStateNum()
+int updateStateNum(int tops_num)
 {
     /*
     状態収束を判定して次のステップに進む
     */
     float t = 0.1;
     static int num = 0;
-    //x方向、y方向、yow方向の判定をはっきりしておく
+    //x方向、y方向、yow方向、topsの判定をはっきりしておく
     bool flag_x = 0, flag_y = 0, flag_yow = 0;
     
     if(pidRobotYow.isConvergence(t))
@@ -209,7 +218,7 @@
     
     if(state_lib[num][0] >= 10 || state_lib[num][0] <= -10)
     {
-        if(pidUss.isConvergence(t+1) && isConvergenceTops(num) && isConvergenceSupply(num))
+        if(pidUss.isConvergence(t+1) && isConvergenceTops(tops_num) && isConvergenceSupply(tops_num))
             flag_x = flag_y = 1;
     }
     else if(pidRobotX.isConvergence(t) == 1)
@@ -218,7 +227,7 @@
     //超音波で近づくとき
     if(state_lib[num][1] >= 10 || state_lib[num][1] <= -10)
     {
-        if(pidUss.isConvergence(t+1) && isConvergenceTops(num) && isConvergenceSupply(num))
+        if(pidUss.isConvergence(t+1) && isConvergenceTops(tops_num) && isConvergenceSupply(tops_num))
             flag_y = flag_x = 1;
     }
     else if(pidRobotY.isConvergence(t) == 1)
@@ -236,6 +245,167 @@
     return num;
 }
 
+int updateTopsNum(int num)
+{
+    static int tops_num = 0;
+    bool flag_tops = 0;
+    
+    if(isConvergenceTops(tops_num) && isConvergenceSupply(tops_num))
+    {
+        if(state_lib[num][4] == 1)
+            flag_tops = 1;
+        else
+        {
+            if(tops_num == 1)
+                flag_tops = 0;
+            else flag_tops = 1;
+        }
+    }
+    
+    if(flag_tops)
+        tops_num++;
+    
+    if(tops_num >= 4)
+        tops_num = 0;
+    
+    return tops_num;
+}
+
+int changeNextTable(int num)
+{
+    for(int i = 0; i < 6; i++)
+            table_sw[i] = can.get(6, i + 1);
+    for(int i = 1; i < 4; i++)
+        table_sw[i + 5] = can.get(2, i);
+    
+    if(table_sw[8] == 0)
+        changeToBlueZone();
+    
+    
+    if(table_sw[3] == 1 && table_sw[4] == 0)
+    {
+        if(num == 5)
+         num = 17;
+    }
+    else if(table_sw[3] == 0 && table_sw[4] == 1)
+    {
+        if(num == 1)
+            num = 5;
+        if(num == 9)
+            num = 17;
+    }
+    else if(table_sw[3] == 1 && table_sw[4] == 1)
+    {
+        if(num == 1)
+            num = 9;
+    }
+    if(table_sw[5] == 0 && table_sw[6] == 0 && table_sw[7] == 0)
+    {
+        if(num == 17)
+            num = 43;
+    }
+    if(table_sw[5] == 0)
+    {
+        if(num == 18)
+            num = 22;
+    }
+    if(table_sw[6] == 0)
+    {
+        if(num == 22)
+            num = 26;
+    }
+    if(table_sw[7] == 0)
+    {
+        if(num == 26)
+            num = 30;
+    }
+    if(table_sw[0] == 0)
+    {
+        if(num == 30)
+            num = 33;
+    }
+    if(table_sw[1] == 0)
+    {
+        if(num == 34)
+            num = 38;
+    }
+    if(table_sw[2] == 0)
+    {
+        if(num == 38)
+            num = 42;
+    }
+    if(table_sw[1] == 0 || table_sw[2] == 0)
+    {
+        if(num == 42)
+            num = 43;
+    }
+    
+    return num;
+}
+
+state updateTopsState(float val, int tops_num)
+{
+    state a;
+    if(val == 1)
+    {
+        for(int i = 0; i < 3; i++)
+            state_tops[tops_num][i] = A_1[tops_num][i];
+    }
+    else if(val == 2)
+    {
+        for(int i = 0; i < 3; i++)
+            state_tops[tops_num][i] = A_2[tops_num][i];
+    }
+    else if(val == 3)
+    {
+        for(int i = 0; i < 3; i++)
+            state_tops[tops_num][i] = A_3[tops_num][i];
+    }
+    else if(val == 4)
+    {
+        for(int i = 0; i < 3; i++)
+            state_tops[tops_num][i] = B_1[tops_num][i];
+    }
+    else if(val == 5)
+    {
+        for(int i = 0; i < 3; i++)
+            state_tops[tops_num][i] = B_2[tops_num][i];
+    }
+    else if(val == 6)
+    {
+        for(int i = 0; i < 3; i++)
+            state_tops[tops_num][i] = C_1[tops_num][i];
+    }
+    else if(val == 7)
+    {
+        for(int i = 0; i < 3; i++)
+            state_tops[tops_num][i] = C_2[tops_num][i];
+    }
+    else if(val == 8)
+    {
+        for(int i = 0; i < 3; i++)
+            state_tops[tops_num][i] = C_3[tops_num][i];
+    }
+    else if(val == 9)
+    {
+        state_tops[0][0] = 0;
+        state_tops[0][1] = 90;
+        state_tops[0][2] = 1;
+    }
+    else
+    {
+        state_tops[0][0] = 0;
+        state_tops[0][1] = 90;
+        state_tops[0][2] = 0;
+    }
+    
+    a.shoot = state_tops[tops_num][0];
+    a.angle = state_tops[tops_num][1];
+    a.supply = state_tops[tops_num][2];
+    
+    return a;
+}
+
 elements getRobotVelocity(state a)
 {
     elements vel;
@@ -308,23 +478,23 @@
     }
     return vel;
 }
-bool isConvergenceTops(int state_num)
+bool isConvergenceTops(int tops_num)
 {
     int velocity_pid = can.get(3, 1);
     int angle_pid    = can.get(3, 2);
     int velocity_val = can.get(3, 3);
     float angle_val    = can.get(3, 4);
-    angle_val /= 2.0;
+    angle_val = 90 - (angle_val / 10.0);
         
-    if(angle_pid == 1 && velocity_pid == 1 && velocity_val == state_lib[state_num][3] && angle_val == state_lib[state_num][4])
+    if(angle_pid == 1 && velocity_pid == 1 && velocity_val == state_tops[tops_num][0] && angle_val == state_tops[tops_num][1])
         return 1;
     else return 0;
 }
-bool isConvergenceSupply(int state_num)
+bool isConvergenceSupply(int num)
 {
     int is_supply_done = can.get(3, 5);
         
-    if(state_lib[state_num][5] == 1)
+    if(state_tops[num][2] == 1)
     {
         if(is_supply_done == 1)
             return 1;
@@ -368,7 +538,7 @@
     static double start_time = timer.read();
     static bool flag = 1;
     double now_time = timer.read();
-    if(now_time - start_time > 0.05)
+    if(now_time - start_time > 0.025)
     {
         if(flag)
         {