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@5:b2e1495da29b, 2015-05-18 (annotated)

Committer:
s1200058
Date:
Mon May 18 05:29:32 2015 +0000
Revision:
5:b2e1495da29b
Parent:
3:b7b30e73cae0 
see for robot gps

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