Diff: main.cpp

Revision:

1:b684ee7d282a

Parent:

0:0d2f5546f51a

Child:

2:ed97da657b4b

--- a/main.cpp	Fri May 15 07:05:06 2015 +0000
+++ b/main.cpp	Fri May 15 08:22:51 2015 +0000
@@ -2,20 +2,26 @@
 //  
 //    
 //
-//  Program name: Aigamozu BASE
+//  Program name: Aigamozu_collect_data
 //  Author: Mineta Kizuku
+//
+//  Yokokawa Mami
 //  
 //
 /**********************************************/
 
 /**********************************************/
 //更新情報
-//2015/05/11
-//ベースプログラムの作成
 //
 //2015/05/13
 //カルマンフィルタの共分散の値を0.0001以下にならないようにした
 //共分散の値を10進数に変換するようにした
+//
+//2015/05/15
+//Robot側からのinかoutかの判定を受け取り、それと緯度と経度をprintするようにしました。
+//Robot側のプログラムcreateReceiveStatusCommand()にてstate関連をいじったので必要に応じて直してください。
+//
+//
 /**********************************************/
 
 #include "mbed.h"
@@ -31,7 +37,7 @@
 #define SIGMA_MIN 0.0001
 
 //************ID Number*****************
-const char MyID = 'd';
+const char MyID = 'e';
 //************ID Number*****************
 
 /////////////////////////////////////////
@@ -164,6 +170,38 @@
 }
 
 
+/////////////////////////////////////////
+//
+//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 Robot data\n");
+        int id = SenderIDc - 'A';
+        agz.set_base_status(id, &packetdata[9]);
+        agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
+        agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
+        
+        //debug
+        for(int i = 0; i < 1; i++){
+        printf("ROBOT:%d\n",i);
+        printf("status:%c, latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n",
+            agz.get_base_status(i), agz.get_basePoint_lati(i),agz.get_basePoint_longi(i),
+            agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)
+            );
+        }
+    }
+    //基地局からデータが来たとき
+    if(SenderIDc >= 'a' && SenderIDc <= 'z'){
+    }
+}
 
 /////////////////////////////////////////
 //
@@ -257,9 +295,9 @@
     const int refresh_Time = 1000; //refresh time in ms
     myGPS.begin(GPS_BAUD_RATE);
     Timer collect_Timer;
-    const int collect_Time = 2000; //when we collect 4 GPS point, we use 
+ /*   const int collect_Time = 2000; //when we collect 4 GPS point, we use 
     int collect_flag = 0;
-    char collect_state = 'a';
+    char collect_state = 'a';*/
     
     char SenderIDc;
     //GPS Send Command
@@ -298,6 +336,10 @@
                         Send_Status(SenderIDc);                     
                         break;          
                     }
+                    case RECEIVE_STATUS:{
+                        Get_Status(SenderIDc,buf1);
+                        break;
+                    }
                     default:{
                         break;
                     }
@@ -313,7 +355,7 @@
                 continue;   
             }       
         }
-        
+  /*      
         if( collect_Timer.read_ms() >= collect_Time){
             collect_Timer.reset();   
                      
@@ -328,7 +370,7 @@
             }    
          } 
         
-        
+*/        
         if (refresh_Timer.read_ms() >= refresh_Time) {
             refresh_Timer.reset();
             Get_GPS(&myGPS);