Diff: main.cpp

Revision:

19:bdb503dd1e8c

Parent:

18:268ab2ab0b2a

Child:

20:1957c67ab740

--- a/main.cpp	Thu Sep 27 05:28:37 2018 +0000
+++ b/main.cpp	Wed Oct 03 08:10:17 2018 +0000
@@ -15,6 +15,7 @@
 elements getRobotVelocity(state);
 state getTargetState();
 int updateStateNum();
+int changeNextTable(int);
 int updateTopsNum(bool);
 bool isConvergenceCatapult(int);
 bool isConvergenceSupply(int);
@@ -43,9 +44,12 @@
                       {0, 0, 0}
                      };
 Odometer odm(matrix, 0.050);
-    
+
+int table_sw[9];
+
 int main()
 {
+    pc.baud(115200);
     //タイマー３の優先度を最低にする
     NVIC_SetPriority(TIMER3_IRQn, 3);
 
@@ -113,7 +117,10 @@
         if(now_time - pre_time >= 0.01)
         {
             if(can.send())
+            {
                 pre_time = now_time;
+                LED[0] = !LED[0];
+            }
         }
         
         //ロボット速度を取得
@@ -128,11 +135,19 @@
         //モーターの駆動
         for(int i = 0; i < 4; i++)
             Motor[i].drive(mecanum.wheel_vel[i]);
-        
+        /*
         pc.printf("%.2f\t", odm.x);
         pc.printf("%.2f\t", odm.y);
         pc.printf("%.2f\t", imu.angle[2]);
         
+        pc.printf("%.2f\t", uss[0].distance);
+        pc.printf("%.2f\t", uss[1].distance);
+        pc.printf("%.2f\t", enc[0].rotations);
+        pc.printf("%.2f\t", enc[1].rotations);
+        
+        for(int i = 0; i < 9; i++)
+            pc.printf("%d\t", table_sw[i]);
+        */
         pc.printf("\n");
     }
 }
@@ -147,21 +162,24 @@
     
     /*
     スタートボタン待機
-    whileにするとほかの処理がすべて止めるためこれを回避
+    whileにするとほかの処理がすべて止まるためこれを回避
     */
     if(sw1 == 1)
         start_flag = 1;
     
     //sw1が1度も押されていないとき
     if(!start_flag)
-    {
-        state_num = odm.x = odm.y = imu.angle[2] = have_ever_shot = 0;
-    }
+        state_num = tops_num = odm.x = odm.y = imu.angle[2] = have_ever_shot = 0;
     else state_num = updateStateNum();
     
     //再びスタート待機
     if(state_num == 0)
-        start_flag = 0;   
+        start_flag = 0;
+    
+    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(7, i);
     
     int target_table = state_lib[state_num][3];
     bool is_shoot = state_lib[state_num][4];
@@ -174,14 +192,24 @@
     
     tops_lib[3][0] = para_lib[target_table].vel;
     tops_lib[2][1] = tops_lib[3][1] = para_lib[target_table].angle;
-
+    
     tops_num = updateTopsNum(is_shoot);
     
+    pc.printf("%d\t", state_num);
+    pc.printf("%d\t", tops_num);
+    
     state a;
-    a.x = state_lib[state_num][0];
+    
+    //changeZoneの処理
+    if(table_sw[8] == 1)
+    {
+        a.x = -1 * state_lib[state_num][0];
+        a.theta = -1 * state_lib[state_num][2];
+    } else {
+        a.x = state_lib[state_num][0];
+        a.theta = state_lib[state_num][2];
+    }
     a.y = state_lib[state_num][1];
-    a.theta = state_lib[state_num][2];
-    
     a.shoot = tops_lib[tops_num][0];
     a.angle = tops_lib[tops_num][1];
     a.supply = tops_lib[tops_num][2];
@@ -210,7 +238,7 @@
     }
     else if(pidRobotX.isConvergence(t) == 1)
         flag_x = 1;
-        
+    
     //超音波で近づくとき
     if(state_lib[num][1] >= 10 || state_lib[num][1] <= -10)
     {
@@ -223,45 +251,106 @@
     if(state_lib[num][4] == 1)
     {
         if(is_shot)
-        {
             flag_tops = 1;
-            is_shot = 0;
-        }
     }
     else flag_tops = 1;
     
     //全部が収束してるか
     if(flag_x && flag_y && flag_yow && flag_tops)
+    {
         num++;
-    
+        is_shot = 0;
+    }
     //振り出しに戻る
     if(num >= LIBNUM)
         num = 0;
     
-    pc.printf("%d\t", num);
+    num = changeNextTable(num);
+    
     pc.printf("%d\t", flag_x);
     pc.printf("%d\t", flag_y);
     pc.printf("%d\t", flag_yow);
     pc.printf("%d\t", flag_tops);
-        
+    pc.printf("%d\t", pidRobotX.isConvergence(0.1));
+    pc.printf("%d\t", pidRobotY.isConvergence(0.1));
+    pc.printf("%d\t", pidRobotYow.isConvergence(0.1));
+    
     return num;
 }
 
 int updateTopsNum(bool is_shoot)
 {
+    static bool pre_is_shoot = 0;
     static int num = 0;
     if(isConvergenceCatapult(num) && isConvergenceSupply(num))
     {
         if(num != 2)
             num++;
-        else if(is_shoot)
+        else if(is_shoot && !pre_is_shoot)
+        {
             num++;
+            pre_is_shoot = 1;
+        }
     }
     if(num >= 4)
     {
         num = 0;
         is_shot = 1;
     }
+    if(!is_shoot)    
+        pre_is_shoot = 0;
+    
+    return num;
+}
+int changeNextTable(int num)
+{
+    if(table_sw[3] == 1 && table_sw[4] == 0)
+    {
+        if(num > 4 && num < 17)
+            num = 17;
+    }
+    else if(table_sw[3] == 0 && table_sw[4] == 1)
+    {
+        if(num > 0 && num < 5)
+            num = 5;
+        if(num > 8 && num < 17)
+            num = 17;
+    }
+    else if(table_sw[3] == 1 && table_sw[4] == 1)
+    {
+        if(num > 0 && num < 9)
+            num = 9;
+    }
+    else if(num > 0 && num < 18)
+            num = 18;
+    if(table_sw[5] == 0 && table_sw[6] == 0 && table_sw[7] == 0)
+    {
+        if(num > 16 && num < 30)
+            num = 30;
+    }
+    else
+    {
+        if(table_sw[5] == 0 && num > 17 && num < 22)
+            num = 22;
+        if(table_sw[6] == 0 && num > 21 && num < 26)
+            num = 26;
+        if(table_sw[7] == 0 && num > 25 && num < 30)
+            num = 30;
+    }
+    if(table_sw[0] == 1 || table_sw[1] == 1 || table_sw[2] == 1)
+    {
+        if(table_sw[0] == 0 && num > 30 && num < 35)
+            num = 35;
+        if(table_sw[1] == 0 && num < 34 && num > 39)
+            num = 39;
+        if(table_sw[2] == 0 && num > 38 && num < 43)
+            num = 42;
+        if(table_sw[1] == 0 && table_sw[2] == 0 && num == 43)
+            num = 44;
+    }
+    else if(num > 29 && num < 44)
+        num = 44;
+    
     return num;
 }