![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2016_05_19 Auto mode: 10sec forward, 2sec stop, 2sec turn right Please change test Auto pwm
Dependencies: ADXL345 AigamozuControlPackets_2016 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST_2016 mbed
Fork of Aigamozu_Robot_ver4 by
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