Diff: main.cpp

Revision:

1:b2b950b916ce

Parent:

0:daab5accfd83

Child:

2:886fac7f4399

diff -r daab5accfd83 -r b2b950b916ce main.cpp
--- a/main.cpp	Tue May 12 11:49:24 2015 +0000
+++ b/main.cpp	Wed May 13 04:09:16 2015 +0000
@@ -23,7 +23,6 @@
 #include "AigamozuControlPackets.h"
 #include "agzIDLIST.h"
 #include "aigamozuSetting.h"
-#include "agz_common.h"
 #include "Kalman.h"
 
 //************ID Number*****************
@@ -31,7 +30,7 @@
 //Base    ID: 'a' ~ 'a'
 //manager ID: '0'(数字のゼロ)
 //
-const char MyID = 'D';
+const char MyID = 'd';
 //************ID Number*****************
 
 /////////////////////////////////////////
@@ -159,6 +158,53 @@
     else agz.nowStatus = GPS_UNAVAIL;
     
 } 
+  
+/////////////////////////////////////////
+//
+//Get Status
+//
+/////////////////////////////////////////
+void Get_Status(char SenderIDc,uint8_t *packetdata){
+    
+    //マネージャからデータが来たとき
+    if(SenderIDc == '0'){
+        printf("get manager Status\n");
+    }
+    //他のロボットからデータが来たとき
+    if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
+        printf("get other robots Status\n");
+    }
+    //基地局からデータが来たとき
+    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
+        printf("Get Base data\n");
+        int id = SenderIDc - 'a';
+        agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
+        agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
+        
+        //debug
+        for(int i = 0;i < 4;i++){
+        printf("BASE:%d\n",i);
+        printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
+            agz.get_basePoint_lati(i),agz.get_basePoint_longi(i),
+            agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)
+            );
+        }
+    }
+}
+
+/////////////////////////////////////////
+//
+//Send_Request_to_base
+//
+/////////////////////////////////////////
+void Send_Request_Base(int basenumber){
+    printf("send\n");
+    agz.createRequestCommand(MyID, basenumber);
+    //Select Destination
+    ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength());
+    //Send -> Base
+    xbee.send(tx64request);
+}
 
 
 /////////////////////////////////////////
@@ -248,9 +294,12 @@
     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
     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);
@@ -260,7 +309,8 @@
        
     //interrupt start
     refresh_Timer.start();
-
+    collect_Timer.start();
+    
     printf("start\n");
     
     while (true) {
@@ -286,6 +336,10 @@
                         Send_Status(SenderIDc);                     
                         break;          
                     }
+                    case RECEIVE_STATUS:{
+                        Get_Status(SenderIDc,buf1);
+                        break;
+                    }
                     default:{
                         break;
                     }
@@ -301,12 +355,19 @@
                 continue;   
             }       
         }
-        
-        
+        //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
         if (refresh_Timer.read_ms() >= refresh_Time) {
             refresh_Timer.reset();
             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;
+        }
     }
 }
\ No newline at end of file