Diff: main.cpp

Revision:

1:d10d72b1d29b

Parent:

0:eee5e3d906ce

--- a/main.cpp	Tue May 12 01:03:31 2015 +0000
+++ b/main.cpp	Tue May 12 10:21:05 2015 +0000
@@ -13,6 +13,8 @@
 //2015/05/11
 //ベースプログラムの作成
 //
+//void Get_GPS()内でLongitudeL, LatitudeL それぞれに+10する処理。
+//main()内でAutoModeのときでかつ2000ms立った時にGPSを取得する。
 //
 //
 /**********************************************/
@@ -83,6 +85,7 @@
 XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L);
 
 
+
 /////////////////////////////////////////
 //
 //Send_Status
@@ -102,13 +105,13 @@
     }
     //send normal data
     //Create GPS Infomation Packet
-    agz.createReceiveStatusCommand(MyID,SenderIDc,agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                                    agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                                    agz.get_agzCov_lati(),agz.get_agzCov_longi());
+//    agz.createReceiveStatusCommand(MyID,SenderIDc,agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+//                                    agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+//                                    agz.get_agzCov_lati(),agz.get_agzCov_longi());
     //Select Destination
-    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
+//    ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
     //Send -> Base
-    xbee.send(tx64request);
+//    xbee.send(tx64request);
 }
 
 /////////////////////////////////////////
@@ -134,6 +137,7 @@
         if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
             return;
         }
+        
         //Kalman Filter
         Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
                 
@@ -212,6 +216,27 @@
     agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
 }
 
+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());
+ 
+}
 
 /////////////////////////////////////////
 //
@@ -235,6 +260,10 @@
     myGPS.begin(GPS_BAUD_RATE); 
     
     char SenderIDc;
+    
+    Timer auto_Timer;
+    const int auto_Time = 2000;
+    
     //GPS Send Command
     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
@@ -244,6 +273,7 @@
        
     //interrupt start
     refresh_Timer.start();
+    auto_Timer.start();
 
     printf("start\n");
     
@@ -276,6 +306,8 @@
                 }//endswitch
             }//endifZB_RX_RESPONSE
         }//endifisAvailable
+        
+        
                        
         myGPS.read();
         //recive gps module
@@ -298,5 +330,11 @@
                     );
             
         }
+        
+        if(agz.nowMode == AUTO_MODE && auto_Timer.read_ms() >= auto_Time){
+            auto_Timer.reset();
+            auto_Move(); 
+        }
+        
     }
 }
\ No newline at end of file