aigamozu / Mbed 2 deprecated aigamozu_auto_ver3_3_2_Nakazawa

20150511, Nakazawa

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

Fork of aigamozu_auto_ver3_3_2 by aigamozu

main.cpp@4:480f4eb9bcbc, 2015-04-26 (annotated)

Committer:
s1200058
Date:
Sun Apr 26 16:44:23 2015 +0000
Revision:
4:480f4eb9bcbc
Parent:
1:d351cf39a6e3
Child:
7:209c07cc2b80
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kityann 0:68d27eee9a6c 1 /**********************************************/
kityann 0:68d27eee9a6c 2 //
kityann 0:68d27eee9a6c 3 //
kityann 0:68d27eee9a6c 4 //
kityann 0:68d27eee9a6c 5 // Program name: Aigamozu Robot Control
kityann 0:68d27eee9a6c 6 // Author: Atsunori Maruyama
kityann 0:68d27eee9a6c 7 // Ver -> 1.3
kityann 0:68d27eee9a6c 8 // Day -> 2014/06/09
kityann 0:68d27eee9a6c 9 //
kityann 0:68d27eee9a6c 10 //
kityann 0:68d27eee9a6c 11 /**********************************************/
kityann 0:68d27eee9a6c 12
kityann 0:68d27eee9a6c 13 #include "mbed.h"
kityann 0:68d27eee9a6c 14 #include "XBee.h"
kityann 0:68d27eee9a6c 15 #include "MBed_Adafruit_GPS.h"
kityann 0:68d27eee9a6c 16 #include "AigamozuControlPackets.h"
kityann 0:68d27eee9a6c 17 #include "agzIDLIST.h"
kityann 0:68d27eee9a6c 18 #include "aigamozuSetting.h"
kityann 0:68d27eee9a6c 19 #include "agz_common.h"
kityann 0:68d27eee9a6c 20 #include "Kalman.h"
kityann 0:68d27eee9a6c 21
kityann 0:68d27eee9a6c 22 /////////////////////////////////////////
kityann 0:68d27eee9a6c 23 //
kityann 0:68d27eee9a6c 24 //Connection Setting
kityann 0:68d27eee9a6c 25 //
kityann 0:68d27eee9a6c 26 /////////////////////////////////////////
kityann 0:68d27eee9a6c 27
kityann 0:68d27eee9a6c 28 //Serial Connect Setting: PC <--> mbed
kityann 0:68d27eee9a6c 29 Serial pc(USBTX, USBRX);
kityann 0:68d27eee9a6c 30
kityann 0:68d27eee9a6c 31 //Serial Connect Setting: GPS <--> mbed
kityann 0:68d27eee9a6c 32 Serial * gps_Serial;
kityann 0:68d27eee9a6c 33
kityann 0:68d27eee9a6c 34 //Serial Connect Setting: XBEE <--> mbed
kityann 0:68d27eee9a6c 35 XBee xbee(p13,p14);
kityann 0:68d27eee9a6c 36 ZBRxResponse zbRx = ZBRxResponse();
kityann 0:68d27eee9a6c 37 XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L);
kityann 0:68d27eee9a6c 38
s1200058 4:480f4eb9bcbc 39 AGZ_ROBOT robot[5];
s1200058 4:480f4eb9bcbc 40
s1200058 4:480f4eb9bcbc 41 char Base_ID = 'A';
kityann 0:68d27eee9a6c 42
kityann 0:68d27eee9a6c 43 /////////////////////////////////////////
kityann 0:68d27eee9a6c 44 //
kityann 0:68d27eee9a6c 45 //Pin Setting
kityann 0:68d27eee9a6c 46 //
kityann 0:68d27eee9a6c 47 /////////////////////////////////////////
kityann 0:68d27eee9a6c 48 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
kityann 0:68d27eee9a6c 49
kityann 0:68d27eee9a6c 50
kityann 0:68d27eee9a6c 51
kityann 0:68d27eee9a6c 52 /////////////////////////////////////////
kityann 0:68d27eee9a6c 53 //
kityann 0:68d27eee9a6c 54 //Kalman Processing
kityann 0:68d27eee9a6c 55 //
kityann 0:68d27eee9a6c 56 /////////////////////////////////////////
kityann 0:68d27eee9a6c 57
kityann 0:68d27eee9a6c 58
kityann 0:68d27eee9a6c 59 double sigmaGPS[2][2] = {{250,0},{0,250}};
kityann 0:68d27eee9a6c 60 double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
kityann 0:68d27eee9a6c 61 double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
kityann 0:68d27eee9a6c 62 double y[2],x[2][2]={0};
kityann 0:68d27eee9a6c 63
kityann 0:68d27eee9a6c 64
kityann 0:68d27eee9a6c 65 void get_K(){
kityann 0:68d27eee9a6c 66 double temp[2][2]={
kityann 0:68d27eee9a6c 67 {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
kityann 0:68d27eee9a6c 68 {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
kityann 0:68d27eee9a6c 69 };
kityann 0:68d27eee9a6c 70 double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
kityann 0:68d27eee9a6c 71 K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
kityann 0:68d27eee9a6c 72 K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
kityann 0:68d27eee9a6c 73 }
kityann 0:68d27eee9a6c 74
kityann 0:68d27eee9a6c 75
kityann 0:68d27eee9a6c 76 void get_x(){
kityann 0:68d27eee9a6c 77 x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
kityann 0:68d27eee9a6c 78 x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
kityann 0:68d27eee9a6c 79 }
kityann 0:68d27eee9a6c 80
kityann 0:68d27eee9a6c 81
kityann 0:68d27eee9a6c 82 void get_sigma(){
kityann 0:68d27eee9a6c 83 double temp[2][2];
kityann 0:68d27eee9a6c 84 for(int i=0;i<2;i++) {
kityann 0:68d27eee9a6c 85 for(int j=0;j<2;j++) {
kityann 0:68d27eee9a6c 86 for(int k=0;k<2;k++) {
kityann 0:68d27eee9a6c 87 temp[i][j]+=K[1][i][k]*sigma[0][k][j];
kityann 0:68d27eee9a6c 88 }
kityann 0:68d27eee9a6c 89 }
kityann 0:68d27eee9a6c 90 }
kityann 0:68d27eee9a6c 91 for(int i = 0;i < 2;i++){
kityann 0:68d27eee9a6c 92 for(int j = 0;j < 2;j++){
kityann 0:68d27eee9a6c 93 sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
kityann 0:68d27eee9a6c 94 }
kityann 0:68d27eee9a6c 95 }
kityann 0:68d27eee9a6c 96 }
kityann 0:68d27eee9a6c 97
kityann 0:68d27eee9a6c 98 void Kalman(double Latitude,double Longitude){
kityann 0:68d27eee9a6c 99 y[0] = Latitude;
kityann 0:68d27eee9a6c 100 y[1] = Longitude;
kityann 0:68d27eee9a6c 101 //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
kityann 0:68d27eee9a6c 102 get_K();
kityann 0:68d27eee9a6c 103 //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
kityann 0:68d27eee9a6c 104 get_x();
kityann 0:68d27eee9a6c 105 //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
kityann 0:68d27eee9a6c 106 get_sigma();
kityann 0:68d27eee9a6c 107 }
kityann 0:68d27eee9a6c 108
kityann 0:68d27eee9a6c 109
kityann 0:68d27eee9a6c 110 /////////////////////////////////////////
kityann 0:68d27eee9a6c 111 //
kityann 0:68d27eee9a6c 112 //Main Processing
kityann 0:68d27eee9a6c 113 //
kityann 0:68d27eee9a6c 114 /////////////////////////////////////////
kityann 0:68d27eee9a6c 115 int main() {
kityann 0:68d27eee9a6c 116 //start up time
kityann 0:68d27eee9a6c 117 wait(3);
kityann 0:68d27eee9a6c 118 //set pc frequency to 57600bps
kityann 0:68d27eee9a6c 119 pc.baud(PC_BAUD_RATE);
kityann 0:68d27eee9a6c 120 //set xbee frequency to 57600bps
kityann 0:68d27eee9a6c 121 xbee.begin(XBEE_BAUD_RATE);
kityann 0:68d27eee9a6c 122
kityann 0:68d27eee9a6c 123 //GPS setting
kityann 0:68d27eee9a6c 124 gps_Serial = new Serial(p28,p27);
kityann 0:68d27eee9a6c 125 Adafruit_GPS myGPS(gps_Serial);
kityann 0:68d27eee9a6c 126 Timer refresh_Timer;
kityann 0:68d27eee9a6c 127 const int refresh_Time = 2000; //refresh time in ms
kityann 0:68d27eee9a6c 128 myGPS.begin(GPS_BAUD_RATE);
kityann 0:68d27eee9a6c 129
kityann 0:68d27eee9a6c 130 Timer collect_Timer;
kityann 0:68d27eee9a6c 131 const int collect_Time = 200; //when we collect 4 GPS point, we use
kityann 0:68d27eee9a6c 132 int collect_flag = 0;
kityann 0:68d27eee9a6c 133 char collect_state = 'a';
s1200058 1:d351cf39a6e3 134 XBeeAddress64 collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L),
s1200058 1:d351cf39a6e3 135 XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L), XBeeAddress64(BASE5_32H,BASE5_32L)};
kityann 0:68d27eee9a6c 136 XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
kityann 0:68d27eee9a6c 137 int id;
kityann 0:68d27eee9a6c 138 bool flag = true;
kityann 0:68d27eee9a6c 139
kityann 0:68d27eee9a6c 140 //GPS Send Command
kityann 0:68d27eee9a6c 141 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
kityann 0:68d27eee9a6c 142 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kityann 0:68d27eee9a6c 143 myGPS.sendCommand(PGCMD_ANTENNA);
kityann 0:68d27eee9a6c 144
kityann 0:68d27eee9a6c 145 wait(2);
kityann 0:68d27eee9a6c 146
kityann 0:68d27eee9a6c 147 //interrupt start
kityann 0:68d27eee9a6c 148 AigamozuControlPackets agz(agz_motorShield);
kityann 0:68d27eee9a6c 149 refresh_Timer.start();
kityann 0:68d27eee9a6c 150
kityann 0:68d27eee9a6c 151 printf("test\n");
kityann 0:68d27eee9a6c 152
kityann 0:68d27eee9a6c 153
kityann 0:68d27eee9a6c 154 while (true) {
kityann 0:68d27eee9a6c 155
kityann 0:68d27eee9a6c 156 //Check Xbee Buffer Available
kityann 0:68d27eee9a6c 157 xbee.readPacket();
kityann 0:68d27eee9a6c 158 if (xbee.getResponse().isAvailable()) {
kityann 0:68d27eee9a6c 159 if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
kityann 0:68d27eee9a6c 160 xbee.getResponse().getZBRxResponse(zbRx);
kityann 0:68d27eee9a6c 161 uint8_t *buf = zbRx.getFrameData();
kityann 0:68d27eee9a6c 162 uint8_t *buf1 = &buf[11];
s1200058 4:480f4eb9bcbc 163 id = buf1[5] - 'A';
kityann 0:68d27eee9a6c 164
kityann 0:68d27eee9a6c 165
kityann 0:68d27eee9a6c 166 //Check Command Type
kityann 0:68d27eee9a6c 167 switch(agz.checkCommnadType(buf)){
kityann 0:68d27eee9a6c 168
kityann 0:68d27eee9a6c 169 //CommandType -> ChanegeMode
kityann 0:68d27eee9a6c 170 case CHANGE_MODE :{
kityann 0:68d27eee9a6c 171 agz.changeMode(buf);
kityann 0:68d27eee9a6c 172 break;
kityann 0:68d27eee9a6c 173 }
kityann 0:68d27eee9a6c 174
kityann 0:68d27eee9a6c 175 //CommandType -> Manual
kityann 0:68d27eee9a6c 176 case MANUAL:{
kityann 0:68d27eee9a6c 177 //Check now Mode
kityann 0:68d27eee9a6c 178 if(agz.nowMode == MANUAL_MODE){
kityann 0:68d27eee9a6c 179 agz.changeSpeed(buf);
kityann 0:68d27eee9a6c 180 }
kityann 0:68d27eee9a6c 181 break;
kityann 0:68d27eee9a6c 182 }
kityann 0:68d27eee9a6c 183
kityann 0:68d27eee9a6c 184 //CommandType -> Send Status
kityann 0:68d27eee9a6c 185 case STATUS_REQUEST:{
kityann 0:68d27eee9a6c 186 //send normal data
kityann 0:68d27eee9a6c 187 //Create GPS Infomation Packet
s1200058 4:480f4eb9bcbc 188 agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:68d27eee9a6c 189 //Select Destination
kityann 0:68d27eee9a6c 190 ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength());
kityann 0:68d27eee9a6c 191 //Send -> Base
kityann 0:68d27eee9a6c 192 xbee.send(tx64request);
kityann 0:68d27eee9a6c 193
kityann 0:68d27eee9a6c 194 //send Kalman data
s1200058 4:480f4eb9bcbc 195 agz.createReceiveStatusCommandwithKalman(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
kityann 0:68d27eee9a6c 196 //Select Destination
kityann 0:68d27eee9a6c 197 ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength());
kityann 0:68d27eee9a6c 198 //Send -> Base
kityann 0:68d27eee9a6c 199 xbee.send(tx64request);
s1200058 4:480f4eb9bcbc 200
s1200058 4:480f4eb9bcbc 201 //send normal data
s1200058 4:480f4eb9bcbc 202 //Create GPS Infomation Packet
s1200058 4:480f4eb9bcbc 203 agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
s1200058 4:480f4eb9bcbc 204 //Select Destination
s1200058 4:480f4eb9bcbc 205 ZBTxRequest tx64request2(collect_Address[id],agz.packetData,agz.getPacketLength());
s1200058 4:480f4eb9bcbc 206 //Send -> Base
s1200058 4:480f4eb9bcbc 207 xbee.send(tx64request);
s1200058 4:480f4eb9bcbc 208
s1200058 4:480f4eb9bcbc 209 //send Kalman data
s1200058 4:480f4eb9bcbc 210 agz.createReceiveStatusCommandwithKalman(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
s1200058 4:480f4eb9bcbc 211 //Select Destination
s1200058 4:480f4eb9bcbc 212 ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength());
s1200058 4:480f4eb9bcbc 213 //Send -> Base
s1200058 4:480f4eb9bcbc 214 xbee.send(tx64request);
kityann 0:68d27eee9a6c 215 break;
kityann 0:68d27eee9a6c 216
kityann 0:68d27eee9a6c 217 }
kityann 0:68d27eee9a6c 218
kityann 0:68d27eee9a6c 219 case RECEIVE_STATUS:{
kityann 0:68d27eee9a6c 220
kityann 0:68d27eee9a6c 221 //printf("Receive Status\n");
kityann 0:68d27eee9a6c 222
kityann 0:68d27eee9a6c 223 id = buf1[5] - 'A';
kityann 0:68d27eee9a6c 224 robot[id].set_state(buf1[9]);
kityann 0:68d27eee9a6c 225 robot[id].set_LatitudeH(&buf1[13]);
kityann 0:68d27eee9a6c 226 robot[id].set_LatitudeL(&buf1[17]);
kityann 0:68d27eee9a6c 227 robot[id].set_LongitudeH(&buf1[21]);
kityann 0:68d27eee9a6c 228 robot[id].set_LongitudeL(&buf1[25]);
kityann 0:68d27eee9a6c 229
kityann 0:68d27eee9a6c 230 agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
kityann 0:68d27eee9a6c 231 /*
kityann 0:68d27eee9a6c 232 printf("%d,", buf1[5]);
kityann 0:68d27eee9a6c 233 printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL());
kityann 0:68d27eee9a6c 234 */
kityann 0:68d27eee9a6c 235 break;
kityann 0:68d27eee9a6c 236 }
kityann 0:68d27eee9a6c 237 case RECEIVE_KALMAN:{
kityann 0:68d27eee9a6c 238 id = buf1[5] - 'A';
kityann 0:68d27eee9a6c 239 robot[id].set_state(buf1[9]);
kityann 0:68d27eee9a6c 240 robot[id].set_LatitudeKH(&buf1[13]);
kityann 0:68d27eee9a6c 241 robot[id].set_LatitudeKL(&buf1[17]);
kityann 0:68d27eee9a6c 242 robot[id].set_LongitudeKH(&buf1[21]);
kityann 0:68d27eee9a6c 243 robot[id].set_LongitudeKL(&buf1[25]);
kityann 0:68d27eee9a6c 244
kityann 0:68d27eee9a6c 245 agz.reNewBasePointKalman(id,robot[id].get_LatitudeKH(),robot[id].get_LatitudeKL(),robot[id].get_LongitudeKH(),robot[id].get_LongitudeKL());
kityann 0:68d27eee9a6c 246
kityann 0:68d27eee9a6c 247 break;
kityann 0:68d27eee9a6c 248 }
kityann 0:68d27eee9a6c 249 default:{
kityann 0:68d27eee9a6c 250 break;
kityann 0:68d27eee9a6c 251 }
kityann 0:68d27eee9a6c 252 }
kityann 0:68d27eee9a6c 253
kityann 0:68d27eee9a6c 254
kityann 0:68d27eee9a6c 255 }
kityann 0:68d27eee9a6c 256 }
kityann 0:68d27eee9a6c 257
kityann 0:68d27eee9a6c 258
kityann 0:68d27eee9a6c 259 myGPS.read();
kityann 0:68d27eee9a6c 260 //recive gps module
kityann 0:68d27eee9a6c 261 //check if we recieved a new message from GPS, if so, attempt to parse it,
kityann 0:68d27eee9a6c 262 if ( myGPS.newNMEAreceived() ) {
kityann 0:68d27eee9a6c 263 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kityann 0:68d27eee9a6c 264 continue;
kityann 0:68d27eee9a6c 265 }
kityann 0:68d27eee9a6c 266 }
kityann 0:68d27eee9a6c 267 if (refresh_Timer.read_ms() >= refresh_Time) {
kityann 0:68d27eee9a6c 268 refresh_Timer.reset();
kityann 0:68d27eee9a6c 269 if (myGPS.fix) {
kityann 0:68d27eee9a6c 270 agz.nowStatus = GPS_AVAIL;
kityann 0:68d27eee9a6c 271
kityann 0:68d27eee9a6c 272 if(flag){
kityann 0:68d27eee9a6c 273 if(myGPS.longitudeH==139 && myGPS.latitudeH==37){
kityann 0:68d27eee9a6c 274 flag = false;
kityann 0:68d27eee9a6c 275 x[0][0]=(double)myGPS.longitudeL;
kityann 0:68d27eee9a6c 276 x[0][1]=(double)myGPS.latitudeL;
kityann 0:68d27eee9a6c 277 }
kityann 0:68d27eee9a6c 278 }
kityann 0:68d27eee9a6c 279 if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue;
kityann 0:68d27eee9a6c 280
kityann 0:68d27eee9a6c 281 //Kalman Filter
kityann 0:68d27eee9a6c 282 Kalman(myGPS.longitudeL,myGPS.latitudeL);
kityann 0:68d27eee9a6c 283 //kousinn
kityann 0:68d27eee9a6c 284 for(int i = 0;i < 2;i++){
kityann 0:68d27eee9a6c 285 for(int j = 0;j < 2;j++){
kityann 0:68d27eee9a6c 286 K[0][i][j]=K[1][i][j];
kityann 0:68d27eee9a6c 287 x[0][i]=x[1][i];
kityann 0:68d27eee9a6c 288 sigma[0][i][j]=sigma[1][i][j];
kityann 0:68d27eee9a6c 289 }
kityann 0:68d27eee9a6c 290 }
kityann 0:68d27eee9a6c 291
kityann 0:68d27eee9a6c 292 myGPS.longitudeKH=myGPS.longitudeH;//longitude after filtering
kityann 0:68d27eee9a6c 293 myGPS.latitudeKH=myGPS.latitudeH;//latitude after filtering
kityann 0:68d27eee9a6c 294 myGPS.longitudeKL=(long)x[1][0];//longitude after filtering
kityann 0:68d27eee9a6c 295 myGPS.latitudeKL=(long)x[1][1];//latitude after filtering
kityann 0:68d27eee9a6c 296
kityann 0:68d27eee9a6c 297 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:68d27eee9a6c 298 agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL);
kityann 0:68d27eee9a6c 299 //printf("state = %d\n",agz.nowMode);
kityann 0:68d27eee9a6c 300 //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
kityann 0:68d27eee9a6c 301 }
kityann 0:68d27eee9a6c 302 else agz.nowStatus = GPS_UNAVAIL;
kityann 0:68d27eee9a6c 303
kityann 0:68d27eee9a6c 304 }
kityann 0:68d27eee9a6c 305
kityann 0:68d27eee9a6c 306
kityann 0:68d27eee9a6c 307 //get base GPS
kityann 0:68d27eee9a6c 308 if( collect_Timer.read_ms() >= collect_Time){
kityann 0:68d27eee9a6c 309 collect_Timer.reset();
kityann 0:68d27eee9a6c 310
kityann 0:68d27eee9a6c 311 //printf("Send Request:%d,%d\n",collect_flag,collect_state);
kityann 0:68d27eee9a6c 312
kityann 0:68d27eee9a6c 313 agz.createRequestCommand('A', collect_state);
kityann 0:68d27eee9a6c 314 //Select Destination
kityann 0:68d27eee9a6c 315 ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength());
kityann 0:68d27eee9a6c 316 //Send -> Base
kityann 0:68d27eee9a6c 317 xbee.send(tx64request);
kityann 0:68d27eee9a6c 318
kityann 0:68d27eee9a6c 319 collect_flag++;
kityann 0:68d27eee9a6c 320 collect_state++;
kityann 0:68d27eee9a6c 321
s1200058 1:d351cf39a6e3 322 if(collect_flag == 5){
kityann 0:68d27eee9a6c 323 collect_state = 'a';
kityann 0:68d27eee9a6c 324 collect_flag = 0;
kityann 0:68d27eee9a6c 325 }
kityann 0:68d27eee9a6c 326 }
kityann 0:68d27eee9a6c 327 }
kityann 0:68d27eee9a6c 328 }