aigamozu / Mbed 2 deprecated aigamozu_auto_for_robot_ver1_yokokawa

2015/05/11

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

Fork of aigamozu_auto_for_robot_ver1 by aigamozu

main.cpp@3:a66f4fa1f8a8, 2015-05-11 (annotated)

Committer:
s1200058
Date:
Mon May 11 13:15:05 2015 +0000
Revision:
3:a66f4fa1f8a8
Parent:
2:2afe6fd6e565 
2015/05/11 yokokawa

Who changed what in which revision?

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