2015/05/15
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
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);