Diff: main.cpp

Revision:

8:123cd1f07aea

Parent:

7:1ee46b2e8dce

Child:

9:ce5a1315fe0d

--- a/main.cpp	Mon Aug 20 08:23:55 2018 +0000
+++ b/main.cpp	Mon Aug 20 13:39:26 2018 +0000
@@ -129,9 +129,11 @@
     while(1)
     {
         can.read();
+        /*
         int supply_pid = can.get(2, 1);
         int supply_wait = can.get(2, 2);
         int angle_wait = can.get(3, 5);
+        */
         int sw[8] = {can.get(4, 1),
                      can.get(4, 2),
                      can.get(4, 3),
@@ -144,10 +146,11 @@
         
         can.set(1, 1, int(position[posi_num][4]));
         can.set(1, 2, int(position[posi_num][5]));
+        /*
         if(supply_wait == 1 && angle_wait == 1)
             can.set(1, 3, 1);
         else can.set(1, 3, 0);
-        
+        */
         while(can.send() == 0);
         
         //ポジション収束判定
@@ -164,12 +167,15 @@
             //超音波使わないとき
             else if(pidRobotY.isConvergence(1) == 1)
             {
+                /*
                 if(posi_num == 2 || posi_num == 6 || posi_num == 10 || posi_num == 14 || posi_num == 17)
                 {
                     if(supply_pid == 1)
                         posi_num++;
                 }
-                else posi_num++;
+                else 
+                */
+                posi_num++;
             }
         }
         if(sw[0] == 1 && posi_num == 0)
@@ -185,6 +191,12 @@
         if(sw[4] == 1 && posi_num == 17)
             posi_num = 18;
         
+        if(posi_num < 19)
+        {
+            posi_num = 0;
+            while(sw1 == 0);
+        }
+        
         //ロボットの移動速度(LX, LY, RX)
         float robot_velocity[3];