Diff: main.cpp

Revision:

3:1ac506a96fd6

Parent:

2:886fac7f4399

Child:

4:f36986ceb73d

--- a/main.cpp	Wed May 13 06:04:47 2015 +0000
+++ b/main.cpp	Wed May 13 06:29:21 2015 +0000
@@ -256,6 +256,36 @@
     xbee.send(tx64request);
 }
 
+/////////////////////////////////////////
+//
+//auto_Move
+//
+//InAreaかOutAreaの判定
+//Kalmanを通した値を出力(Baseと自分)
+/////////////////////////////////////////
+
+void auto_Move(){
+ 
+ bool result;
+ int i;
+ 
+ result = agz.gpsAuto();
+ agz.set_agzAutoGPS();
+ agz.set_agzKalmanGPS();
+ 
+ if(result == true){
+        printf("Out Area\n");
+ }
+else if(result == false){
+        printf("In Area\n");
+}  
+for(i = 0; i < 4; i++){
+    printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
+ }
+ printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
+ 
+}
+
 
 /////////////////////////////////////////
 //
@@ -344,12 +374,11 @@
     Adafruit_GPS myGPS(gps_Serial); 
     Timer refresh_Timer;
     const int refresh_Time = 2000; //refresh time in ms
-    Timer collect_Timer;
-    const int collect_Time = 2000; //refresh time in ms
+    Timer auto_Timer;
+    const int auto_Time = 2000; //refresh time in ms
     myGPS.begin(GPS_BAUD_RATE); 
     
     char SenderIDc;
-    int basenumber=0;
     //GPS Send Command
     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
@@ -359,7 +388,7 @@
        
     //interrupt start
     refresh_Timer.start();
-    collect_Timer.start();
+    auto_Timer.start();
     
     printf("start\n");
     
@@ -423,13 +452,10 @@
             Get_GPS(&myGPS);
 
         }
-        
-        //一定時間ごとに基地局のGPSデータを取得し、AigamozuControlPacketsないのagzBasePointとagzBasePointKalmanに格納する
-        if (collect_Timer.read_ms() >= collect_Time) {
-            collect_Timer.reset();
-            Send_Request_Base(basenumber);
-            basenumber++;
-            if(basenumber > 4)basenumber=0;
+                
+        if(agz.nowMode == AUTO_MODE && auto_Timer.read_ms() >= auto_Time){
+            auto_Timer.reset();
+            auto_Move(); 
         }
     }
 }
\ No newline at end of file