Diff: main.cpp

Revision:

29:524684a1198f

Parent:

28:4ccd7cfc6b1b

Child:

30:7f6ebe2121d9

--- a/main.cpp	Mon May 18 02:36:59 2015 +0000
+++ b/main.cpp	Tue May 19 10:27:43 2015 +0000
@@ -50,7 +50,7 @@
 //Base    ID: 'a' ~ 'a'
 //manager ID: '0'(数字のゼロ)
 //
-const char MyID = 'A';
+const char MyID = 'D';
 //************ID Number*****************
 
 /////////////////////////////////////////
@@ -408,6 +408,10 @@
     Timer auto_Timer;
     const int auto_Time = 2000; //refresh time in ms
     int count = 0;
+    
+    Timer Move_Timer;
+    const int straight = 30000, turning = 34000, wait = 31000;
+    
     myGPS.begin(GPS_BAUD_RATE); 
     
     Timer collect_Timer;
@@ -420,14 +424,15 @@
     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
     myGPS.sendCommand(PGCMD_ANTENNA);
     
-    wait(2);
+    wait_ms(2000);
        
     //interrupt start
     refresh_Timer.start();
     auto_Timer.start();
+    Move_Timer.start();
     collect_Timer.start();
+    printf("start\n");
     
-    printf("start\n");
     
     while (true) {
         
@@ -509,8 +514,25 @@
          } 
             
         if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){
-            auto_Timer.reset();
-            auto_Move(); 
+           auto_Timer.reset();
+           auto_Move(); 
+        }
+        
+     if(agz.nowMode == AUTO_GPS_MODE){
+        if(Move_Timer.read_ms() < straight){
+           agz.test_Auto(0); //straight
+        }
+        if(Move_Timer.read_ms() > straight && Move_Timer.read_ms() < wait){
+            agz.test_Auto(1);
+        }
+        if(Move_Timer.read_ms() > wait && Move_Timer.read_ms() < turning){
+            agz.test_Auto(2); //Turn Right
+        }   
+        if(Move_Timer.read_ms() > turning){
+            agz.test_Auto(3);
+            wait_ms(200);
+            Move_Timer.reset();
         }
     }
+    }
 }
\ No newline at end of file