aigamozu / Mbed 2 deprecated Aigamozu_collect_data_1

2015/05/16

Dependencies:   ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed

Fork of Aigamozu_collect_data by aigamozu

main.cpp@0:0d2f5546f51a, 2015-05-15 (annotated)

Committer:
s1200058
Date:
Fri May 15 07:05:06 2015 +0000
Revision:
0:0d2f5546f51a
Child:
1:b684ee7d282a
2015/05/15

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1200058 0:0d2f5546f51a 1 /**********************************************/
s1200058 0:0d2f5546f51a 2 //
s1200058 0:0d2f5546f51a 3 //
s1200058 0:0d2f5546f51a 4 //
s1200058 0:0d2f5546f51a 5 // Program name: Aigamozu BASE
s1200058 0:0d2f5546f51a 6 // Author: Mineta Kizuku
s1200058 0:0d2f5546f51a 7 //
s1200058 0:0d2f5546f51a 8 //
s1200058 0:0d2f5546f51a 9 /**********************************************/
s1200058 0:0d2f5546f51a 10
s1200058 0:0d2f5546f51a 11 /**********************************************/
s1200058 0:0d2f5546f51a 12 //更新情報
s1200058 0:0d2f5546f51a 13 //2015/05/11
s1200058 0:0d2f5546f51a 14 //ベースプログラムの作成
s1200058 0:0d2f5546f51a 15 //
s1200058 0:0d2f5546f51a 16 //2015/05/13
s1200058 0:0d2f5546f51a 17 //カルマンフィルタの共分散の値を0.0001以下にならないようにした
s1200058 0:0d2f5546f51a 18 //共分散の値を10進数に変換するようにした
s1200058 0:0d2f5546f51a 19 /**********************************************/
s1200058 0:0d2f5546f51a 20
s1200058 0:0d2f5546f51a 21 #include "mbed.h"
s1200058 0:0d2f5546f51a 22 #include "XBee.h"
s1200058 0:0d2f5546f51a 23 #include "MBed_Adafruit_GPS.h"
s1200058 0:0d2f5546f51a 24 #include "AigamozuControlPackets.h"
s1200058 0:0d2f5546f51a 25 #include "agzIDLIST.h"
s1200058 0:0d2f5546f51a 26 #include "aigamozuSetting.h"
s1200058 0:0d2f5546f51a 27 #include "agz_common.h"
s1200058 0:0d2f5546f51a 28 #include "Kalman.h"
s1200058 0:0d2f5546f51a 29
s1200058 0:0d2f5546f51a 30
s1200058 0:0d2f5546f51a 31 #define SIGMA_MIN 0.0001
s1200058 0:0d2f5546f51a 32
s1200058 0:0d2f5546f51a 33 //************ID Number*****************
s1200058 0:0d2f5546f51a 34 const char MyID = 'd';
s1200058 0:0d2f5546f51a 35 //************ID Number*****************
s1200058 0:0d2f5546f51a 36
s1200058 0:0d2f5546f51a 37 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 38 //
s1200058 0:0d2f5546f51a 39 //Pin Setting
s1200058 0:0d2f5546f51a 40 //
s1200058 0:0d2f5546f51a 41 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 42 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
s1200058 0:0d2f5546f51a 43
s1200058 0:0d2f5546f51a 44
s1200058 0:0d2f5546f51a 45 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 46 //
s1200058 0:0d2f5546f51a 47 //Connection Setting
s1200058 0:0d2f5546f51a 48 //
s1200058 0:0d2f5546f51a 49 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 50
s1200058 0:0d2f5546f51a 51 //Serial Connect Setting: PC <--> mbed
s1200058 0:0d2f5546f51a 52 Serial pc(USBTX, USBRX);
s1200058 0:0d2f5546f51a 53
s1200058 0:0d2f5546f51a 54 //Serial Connect Setting: GPS <--> mbed
s1200058 0:0d2f5546f51a 55 Serial * gps_Serial;
s1200058 0:0d2f5546f51a 56
s1200058 0:0d2f5546f51a 57 //Serial Connect Setting: XBEE <--> mbed
s1200058 0:0d2f5546f51a 58 XBee xbee(p13,p14);
s1200058 0:0d2f5546f51a 59 ZBRxResponse zbRx = ZBRxResponse();
s1200058 0:0d2f5546f51a 60
s1200058 0:0d2f5546f51a 61 //set up GPS module
s1200058 0:0d2f5546f51a 62
s1200058 0:0d2f5546f51a 63 //set up AigamozuControlPackets library
s1200058 0:0d2f5546f51a 64 AigamozuControlPackets agz(agz_motorShield);
s1200058 0:0d2f5546f51a 65
s1200058 0:0d2f5546f51a 66
s1200058 0:0d2f5546f51a 67 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 68 //
s1200058 0:0d2f5546f51a 69 //For Kalman data
s1200058 0:0d2f5546f51a 70 //
s1200058 0:0d2f5546f51a 71 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 72 double sigmaGPS[2][2] = {{250,0},{0,250}};
s1200058 0:0d2f5546f51a 73 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
s1200058 0:0d2f5546f51a 74 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
s1200058 0:0d2f5546f51a 75 double y[2],x[2][2]={0};
s1200058 0:0d2f5546f51a 76 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);
s1200058 0:0d2f5546f51a 77
s1200058 0:0d2f5546f51a 78
s1200058 0:0d2f5546f51a 79 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 80 //
s1200058 0:0d2f5546f51a 81 //Send_Status
s1200058 0:0d2f5546f51a 82 //
s1200058 0:0d2f5546f51a 83 //リクエストがきたとき、自分の位置情報などを返信する
s1200058 0:0d2f5546f51a 84 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 85 void Send_Status(char SenderIDc){
s1200058 0:0d2f5546f51a 86 XBeeAddress64 send_Address;
s1200058 0:0d2f5546f51a 87 if(SenderIDc == '0'){
s1200058 0:0d2f5546f51a 88 send_Address = manager_Address;
s1200058 0:0d2f5546f51a 89 }
s1200058 0:0d2f5546f51a 90 if(SenderIDc >= 'A' && SenderIDc <= 'Z'){
s1200058 0:0d2f5546f51a 91 send_Address = robot_Address[SenderIDc - 'A'];
s1200058 0:0d2f5546f51a 92 }
s1200058 0:0d2f5546f51a 93 if(SenderIDc >= 'a' && SenderIDc <= 'z'){
s1200058 0:0d2f5546f51a 94 send_Address = base_Address[SenderIDc - 'a'];
s1200058 0:0d2f5546f51a 95 }
s1200058 0:0d2f5546f51a 96 //send normal data
s1200058 0:0d2f5546f51a 97 //Create GPS Infomation Packet
s1200058 0:0d2f5546f51a 98 agz.createReceiveStatusCommand(MyID,SenderIDc,
s1200058 0:0d2f5546f51a 99 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1200058 0:0d2f5546f51a 100 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1200058 0:0d2f5546f51a 101 agz.get_agzCov_lati(),agz.get_agzCov_longi());
s1200058 0:0d2f5546f51a 102
s1200058 0:0d2f5546f51a 103 //debug***************************************************
s1200058 0:0d2f5546f51a 104 printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
s1200058 0:0d2f5546f51a 105 agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
s1200058 0:0d2f5546f51a 106 agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
s1200058 0:0d2f5546f51a 107 agz.get_agzCov_lati(),agz.get_agzCov_longi()
s1200058 0:0d2f5546f51a 108 );
s1200058 0:0d2f5546f51a 109 for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
s1200058 0:0d2f5546f51a 110 printf("\n");
s1200058 0:0d2f5546f51a 111 //debug end***************************************************
s1200058 0:0d2f5546f51a 112
s1200058 0:0d2f5546f51a 113 //Select Destination
s1200058 0:0d2f5546f51a 114 ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
s1200058 0:0d2f5546f51a 115 //Send -> Base
s1200058 0:0d2f5546f51a 116 xbee.send(tx64request);
s1200058 0:0d2f5546f51a 117 }
s1200058 0:0d2f5546f51a 118
s1200058 0:0d2f5546f51a 119 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 120 //
s1200058 0:0d2f5546f51a 121 //Get GPS function
s1200058 0:0d2f5546f51a 122 //
s1200058 0:0d2f5546f51a 123 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 124
s1200058 0:0d2f5546f51a 125 void Get_GPS(Adafruit_GPS *myGPS){
s1200058 0:0d2f5546f51a 126 static bool flag = true;
s1200058 0:0d2f5546f51a 127
s1200058 0:0d2f5546f51a 128 if (myGPS->fix) {
s1200058 0:0d2f5546f51a 129 agz.nowStatus = GPS_AVAIL;
s1200058 0:0d2f5546f51a 130
s1200058 0:0d2f5546f51a 131 if(flag){//初期値設定
s1200058 0:0d2f5546f51a 132 if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){
s1200058 0:0d2f5546f51a 133 flag = false;
s1200058 0:0d2f5546f51a 134 x[0][0]=(double)myGPS->latitudeL;
s1200058 0:0d2f5546f51a 135 x[0][1]=(double)myGPS->longitudeL;
s1200058 0:0d2f5546f51a 136 }
s1200058 0:0d2f5546f51a 137 }
s1200058 0:0d2f5546f51a 138
s1200058 0:0d2f5546f51a 139 if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
s1200058 0:0d2f5546f51a 140 return;
s1200058 0:0d2f5546f51a 141 }
s1200058 0:0d2f5546f51a 142 //Kalman Filter
s1200058 0:0d2f5546f51a 143 Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
s1200058 0:0d2f5546f51a 144
s1200058 0:0d2f5546f51a 145 agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
s1200058 0:0d2f5546f51a 146 agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
s1200058 0:0d2f5546f51a 147 }
s1200058 0:0d2f5546f51a 148 else agz.nowStatus = GPS_UNAVAIL;
s1200058 0:0d2f5546f51a 149
s1200058 0:0d2f5546f51a 150 }
s1200058 0:0d2f5546f51a 151
s1200058 0:0d2f5546f51a 152 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 153 //
s1200058 0:0d2f5546f51a 154 //Send_Request_to_robot
s1200058 0:0d2f5546f51a 155 //
s1200058 0:0d2f5546f51a 156 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 157 void Send_Request_Robot(int robotnumber){
s1200058 0:0d2f5546f51a 158 printf("send\n");
s1200058 0:0d2f5546f51a 159 agz.createRequestCommand(MyID, robotnumber);
s1200058 0:0d2f5546f51a 160 //Select Destination
s1200058 0:0d2f5546f51a 161 ZBTxRequest tx64request(robot_Address[robotnumber],agz.packetData,agz.getPacketLength());
s1200058 0:0d2f5546f51a 162 //Send -> Base
s1200058 0:0d2f5546f51a 163 xbee.send(tx64request);
s1200058 0:0d2f5546f51a 164 }
s1200058 0:0d2f5546f51a 165
s1200058 0:0d2f5546f51a 166
s1200058 0:0d2f5546f51a 167
s1200058 0:0d2f5546f51a 168 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 169 //
s1200058 0:0d2f5546f51a 170 //Kalman Processing
s1200058 0:0d2f5546f51a 171 //
s1200058 0:0d2f5546f51a 172 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 173
s1200058 0:0d2f5546f51a 174 void get_K(){
s1200058 0:0d2f5546f51a 175 double temp[2][2]={
s1200058 0:0d2f5546f51a 176 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
s1200058 0:0d2f5546f51a 177 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
s1200058 0:0d2f5546f51a 178 };
s1200058 0:0d2f5546f51a 179 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
s1200058 0:0d2f5546f51a 180 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
s1200058 0:0d2f5546f51a 181 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
s1200058 0:0d2f5546f51a 182 }
s1200058 0:0d2f5546f51a 183
s1200058 0:0d2f5546f51a 184
s1200058 0:0d2f5546f51a 185 void get_x(){
s1200058 0:0d2f5546f51a 186 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
s1200058 0:0d2f5546f51a 187 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
s1200058 0:0d2f5546f51a 188 }
s1200058 0:0d2f5546f51a 189
s1200058 0:0d2f5546f51a 190
s1200058 0:0d2f5546f51a 191 void get_sigma(){
s1200058 0:0d2f5546f51a 192 double temp[2][2];
s1200058 0:0d2f5546f51a 193 for(int i=0;i<2;i++) {
s1200058 0:0d2f5546f51a 194 for(int j=0;j<2;j++) {
s1200058 0:0d2f5546f51a 195 for(int k=0;k<2;k++) {
s1200058 0:0d2f5546f51a 196 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
s1200058 0:0d2f5546f51a 197 }
s1200058 0:0d2f5546f51a 198 }
s1200058 0:0d2f5546f51a 199 }
s1200058 0:0d2f5546f51a 200 for(int i = 0;i < 2;i++){
s1200058 0:0d2f5546f51a 201 for(int j = 0;j < 2;j++){
s1200058 0:0d2f5546f51a 202 sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
s1200058 0:0d2f5546f51a 203 }
s1200058 0:0d2f5546f51a 204 }
s1200058 0:0d2f5546f51a 205 }
s1200058 0:0d2f5546f51a 206
s1200058 0:0d2f5546f51a 207 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
s1200058 0:0d2f5546f51a 208 y[0] = Latitude;
s1200058 0:0d2f5546f51a 209 y[1] = Longitude;
s1200058 0:0d2f5546f51a 210 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
s1200058 0:0d2f5546f51a 211 get_K();
s1200058 0:0d2f5546f51a 212 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
s1200058 0:0d2f5546f51a 213 get_x();
s1200058 0:0d2f5546f51a 214 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
s1200058 0:0d2f5546f51a 215 get_sigma();
s1200058 0:0d2f5546f51a 216
s1200058 0:0d2f5546f51a 217
s1200058 0:0d2f5546f51a 218 //kousinn
s1200058 0:0d2f5546f51a 219 for(int i = 0;i < 2;i++){
s1200058 0:0d2f5546f51a 220 for(int j = 0;j < 2;j++){
s1200058 0:0d2f5546f51a 221 K[0][i][j]=K[1][i][j];
s1200058 0:0d2f5546f51a 222 x[0][i]=x[1][i];
s1200058 0:0d2f5546f51a 223 sigma[0][i][j]=sigma[1][i][j];
s1200058 0:0d2f5546f51a 224 }
s1200058 0:0d2f5546f51a 225 }
s1200058 0:0d2f5546f51a 226
s1200058 0:0d2f5546f51a 227 if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN;
s1200058 0:0d2f5546f51a 228 if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN;
s1200058 0:0d2f5546f51a 229
s1200058 0:0d2f5546f51a 230 myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
s1200058 0:0d2f5546f51a 231 myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
s1200058 0:0d2f5546f51a 232 myGPS->latitudeKL=(long)x[1][0];//latitude after filtering
s1200058 0:0d2f5546f51a 233 myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
s1200058 0:0d2f5546f51a 234
s1200058 0:0d2f5546f51a 235 agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
s1200058 0:0d2f5546f51a 236 }
s1200058 0:0d2f5546f51a 237
s1200058 0:0d2f5546f51a 238
s1200058 0:0d2f5546f51a 239 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 240 //
s1200058 0:0d2f5546f51a 241 //Main Processing
s1200058 0:0d2f5546f51a 242 //
s1200058 0:0d2f5546f51a 243 /////////////////////////////////////////
s1200058 0:0d2f5546f51a 244 int main() {
s1200058 0:0d2f5546f51a 245 //start up time
s1200058 0:0d2f5546f51a 246 wait(3);
s1200058 0:0d2f5546f51a 247 //set pc frequency to 57600bps
s1200058 0:0d2f5546f51a 248 pc.baud(PC_BAUD_RATE);
s1200058 0:0d2f5546f51a 249 //set xbee frequency to 57600bps
s1200058 0:0d2f5546f51a 250 xbee.begin(XBEE_BAUD_RATE);
s1200058 0:0d2f5546f51a 251
s1200058 0:0d2f5546f51a 252
s1200058 0:0d2f5546f51a 253 //GPS setting
s1200058 0:0d2f5546f51a 254 gps_Serial = new Serial(p28,p27);
s1200058 0:0d2f5546f51a 255 Adafruit_GPS myGPS(gps_Serial);
s1200058 0:0d2f5546f51a 256 Timer refresh_Timer;
s1200058 0:0d2f5546f51a 257 const int refresh_Time = 1000; //refresh time in ms
s1200058 0:0d2f5546f51a 258 myGPS.begin(GPS_BAUD_RATE);
s1200058 0:0d2f5546f51a 259 Timer collect_Timer;
s1200058 0:0d2f5546f51a 260 const int collect_Time = 2000; //when we collect 4 GPS point, we use
s1200058 0:0d2f5546f51a 261 int collect_flag = 0;
s1200058 0:0d2f5546f51a 262 char collect_state = 'a';
s1200058 0:0d2f5546f51a 263
s1200058 0:0d2f5546f51a 264 char SenderIDc;
s1200058 0:0d2f5546f51a 265 //GPS Send Command
s1200058 0:0d2f5546f51a 266 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
s1200058 0:0d2f5546f51a 267 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
s1200058 0:0d2f5546f51a 268 myGPS.sendCommand(PGCMD_ANTENNA);
s1200058 0:0d2f5546f51a 269
s1200058 0:0d2f5546f51a 270 wait(2);
s1200058 0:0d2f5546f51a 271
s1200058 0:0d2f5546f51a 272 //interrupt start
s1200058 0:0d2f5546f51a 273 refresh_Timer.start();
s1200058 0:0d2f5546f51a 274 collect_Timer.start();
s1200058 0:0d2f5546f51a 275
s1200058 0:0d2f5546f51a 276 printf("start\n");
s1200058 0:0d2f5546f51a 277
s1200058 0:0d2f5546f51a 278 while (true) {
s1200058 0:0d2f5546f51a 279
s1200058 0:0d2f5546f51a 280 //Check Xbee Buffer Available
s1200058 0:0d2f5546f51a 281 xbee.readPacket();
s1200058 0:0d2f5546f51a 282
s1200058 0:0d2f5546f51a 283 if (xbee.getResponse().isAvailable()) {
s1200058 0:0d2f5546f51a 284 xbee.getResponse().getZBRxResponse(zbRx);
s1200058 0:0d2f5546f51a 285 uint8_t *buf = zbRx.getFrameData();
s1200058 0:0d2f5546f51a 286
s1200058 0:0d2f5546f51a 287 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
s1200058 0:0d2f5546f51a 288 xbee.getResponse().getZBRxResponse(zbRx);
s1200058 0:0d2f5546f51a 289 uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する
s1200058 0:0d2f5546f51a 290 uint8_t *buf1 = &buf[11];//データの部分のみを格納する
s1200058 0:0d2f5546f51a 291 SenderIDc = buf1[5];//送信元のIDを取得する
s1200058 0:0d2f5546f51a 292 char Command_type =agz.checkCommnadType(buf);//コマンドタイプを取得する
s1200058 0:0d2f5546f51a 293
s1200058 0:0d2f5546f51a 294 //Check Command Type
s1200058 0:0d2f5546f51a 295 switch(Command_type){
s1200058 0:0d2f5546f51a 296 //Get Request command
s1200058 0:0d2f5546f51a 297 case STATUS_REQUEST:{
s1200058 0:0d2f5546f51a 298 Send_Status(SenderIDc);
s1200058 0:0d2f5546f51a 299 break;
s1200058 0:0d2f5546f51a 300 }
s1200058 0:0d2f5546f51a 301 default:{
s1200058 0:0d2f5546f51a 302 break;
s1200058 0:0d2f5546f51a 303 }
s1200058 0:0d2f5546f51a 304 }//endswitch
s1200058 0:0d2f5546f51a 305 }//endifZB_RX_RESPONSE
s1200058 0:0d2f5546f51a 306 }//endifisAvailable
s1200058 0:0d2f5546f51a 307
s1200058 0:0d2f5546f51a 308 myGPS.read();
s1200058 0:0d2f5546f51a 309 //recive gps module
s1200058 0:0d2f5546f51a 310 //check if we recieved a new message from GPS, if so, attempt to parse it,
s1200058 0:0d2f5546f51a 311 if ( myGPS.newNMEAreceived() ) {
s1200058 0:0d2f5546f51a 312 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
s1200058 0:0d2f5546f51a 313 continue;
s1200058 0:0d2f5546f51a 314 }
s1200058 0:0d2f5546f51a 315 }
s1200058 0:0d2f5546f51a 316
s1200058 0:0d2f5546f51a 317 if( collect_Timer.read_ms() >= collect_Time){
s1200058 0:0d2f5546f51a 318 collect_Timer.reset();
s1200058 0:0d2f5546f51a 319
s1200058 0:0d2f5546f51a 320 Send_Request_Robot(collect_flag);
s1200058 0:0d2f5546f51a 321
s1200058 0:0d2f5546f51a 322 collect_flag++;
s1200058 0:0d2f5546f51a 323 collect_state++;
s1200058 0:0d2f5546f51a 324
s1200058 0:0d2f5546f51a 325 if(collect_flag == 4){
s1200058 0:0d2f5546f51a 326 collect_state = 'a';
s1200058 0:0d2f5546f51a 327 collect_flag = 0;
s1200058 0:0d2f5546f51a 328 }
s1200058 0:0d2f5546f51a 329 }
s1200058 0:0d2f5546f51a 330
s1200058 0:0d2f5546f51a 331
s1200058 0:0d2f5546f51a 332 if (refresh_Timer.read_ms() >= refresh_Time) {
s1200058 0:0d2f5546f51a 333 refresh_Timer.reset();
s1200058 0:0d2f5546f51a 334 Get_GPS(&myGPS);
s1200058 0:0d2f5546f51a 335
s1200058 0:0d2f5546f51a 336 }
s1200058 0:0d2f5546f51a 337 }
s1200058 0:0d2f5546f51a 338 }