aigamozu / Mbed 2 deprecated aigamozu_auto_for_robot_ver1

test

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

Fork of aigamozu_auto_ver3_4 by aigamozu

main.cpp@0:567aa3346701, 2015-04-27 (annotated)

Committer:
kityann
Date:
Mon Apr 27 12:08:38 2015 +0000
Revision:
0:567aa3346701
test

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