![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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
main.cpp
- Committer:
- s1200058
- Date:
- 2015-05-11
- Revision:
- 3:a66f4fa1f8a8
- Parent:
- 2:2afe6fd6e565
File content as of revision 3:a66f4fa1f8a8:
/**********************************************/ // // // // Program name: Aigamozu Robot Control // Author: Atsunori Maruyama // Ver -> 1.3 // Day -> 2014/06/09 // // /**********************************************/ /*********************/ // 2015/05/11 yokokawa mami // AGZ_ROBOTでbase[5], robot[2], managerを宣言しました。 // XbeeAddressでbase_Address, robot_Address, manager_Addressを宣言しました。 // case STATUS_REQUEST:での条件分岐 // send_AddressのところにそのときのAddressが入るようにしました。 // case RECEIVE_STATUS:での条件分岐 // base, robot, managerに対応するように変更しました。 /*********************/ #include "mbed.h" #include "XBee.h" #include "MBed_Adafruit_GPS.h" #include "AigamozuControlPackets.h" #include "agzIDLIST.h" #include "aigamozuSetting.h" #include "agz_common.h" #include "Kalman.h" ///////////////////////////////////////// // //Connection Setting // ///////////////////////////////////////// //Serial Connect Setting: PC <--> mbed Serial pc(USBTX, USBRX); //Serial Connect Setting: GPS <--> mbed Serial * gps_Serial; //Serial Connect Setting: XBEE <--> mbed XBee xbee(p13,p14); ZBRxResponse zbRx = ZBRxResponse(); XBeeAddress64 remoteAddress = XBeeAddress64(BASE1_32H,BASE1_32L); AGZ_ROBOT base[5], robot[2], manager; char MyID = 'Z'; ///////////////////////////////////////// // //Pin Setting // ///////////////////////////////////////// VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); ///////////////////////////////////////// // //Kalman Processing // ///////////////////////////////////////// double sigmaGPS[2][2] = {{250,0},{0,250}}; double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}}; double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}}; double y[2],x[2][2]={0}; void get_K(){ double temp[2][2]={ {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]}, {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]} }; double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1]; K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]); K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]); } void get_x(){ x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]); x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]); } void get_sigma(){ double temp[2][2]; for(int i=0;i<2;i++) { for(int j=0;j<2;j++) { for(int k=0;k<2;k++) { temp[i][j]+=K[1][i][k]*sigma[0][k][j]; } } } for(int i = 0;i < 2;i++){ for(int j = 0;j < 2;j++){ sigma[1][i][j] = sigma[0][i][j]-temp[i][j]; } } } void Kalman(double Latitude,double Longitude){ y[0] = Latitude; y[1] = Longitude; //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS]; get_K(); //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]); get_x(); //sigma[t+1] = sigma[t]-K[t+1]*sigma[t]; get_sigma(); } ///////////////////////////////////////// // //Main Processing // ///////////////////////////////////////// int main() { //start up time wait(3); //set pc frequency to 57600bps pc.baud(PC_BAUD_RATE); //set xbee frequency to 57600bps xbee.begin(XBEE_BAUD_RATE); //GPS setting gps_Serial = new Serial(p28,p27); Adafruit_GPS myGPS(gps_Serial); Timer refresh_Timer; const int refresh_Time = 2000; //refresh time in ms myGPS.begin(GPS_BAUD_RATE); Timer collect_Timer; const int collect_Time = 200; //when we collect 4 GPS point, we use int collect_flag = 0; char collect_state = 'a'; XBeeAddress64 collect_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L), XBeeAddress64(BASE5_32H,BASE5_32L)}; //XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L); int id, SenderIDc; bool flag = true; XBeeAddress64 base_Address[5] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L), XBeeAddress64(BASE5_32H,BASE5_32L)}; XBeeAddress64 robot_Address[2] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L)}; XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L); XBeeAddress64 send_Address; //GPS Send Command myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); myGPS.sendCommand(PGCMD_ANTENNA); wait(2); //interrupt start AigamozuControlPackets agz(agz_motorShield); refresh_Timer.start(); printf("start\n"); while (true) { //Check Xbee Buffer Available xbee.readPacket(); if (xbee.getResponse().isAvailable()) { if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { xbee.getResponse().getZBRxResponse(zbRx); uint8_t *buf = zbRx.getFrameData(); uint8_t *buf1 = &buf[11]; //id = buf1[5] - 'A'; SenderIDc = buf1[5]; //Check Command Type switch(agz.checkCommnadType(buf)){ //CommandType -> ChanegeMode case CHANGE_MODE :{ agz.changeMode(buf); break; } //CommandType -> Manual case MANUAL:{ //Check now Mode if(agz.nowMode == MANUAL_MODE){ agz.changeSpeed(buf); } break; } //CommandType -> Send Status case STATUS_REQUEST:{ /* if(SenderIDc == 'Z'){ //send normal data //Create GPS Infomation Packet agz.createReceiveStatusCommand(MyID,SenderIDc,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); //Select Destination ZBTxRequest tx64request(robot_Address,agz.packetData,agz.getPacketLength()); //Send -> Base xbee.send(tx64request); //send Kalman data agz.createReceiveStatusCommandwithKalman(MyID,SenderIDc,myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL); //Select Destination ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength()); //Send -> Base xbee.send(tx64request1); }else{*/ printf("%d: catch request\n", id); //send normal data //Create GPS Infomation Packet if(SenderIDc == '0'){ send_Address = manager_Address; } if(SenderIDc >= 'A' && SenderIDc <= 'Z'){ send_Address = robot_Address[SenderIDc - 'A']; } if(SenderIDc >= 'a' && SenderIDc <= 'z'){ send_Address = base_Address[SenderIDc - 'a']; } agz.createReceiveStatusCommand(MyID,SenderIDc,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); //Select Destination ZBTxRequest tx64request2(send_Address,agz.packetData,agz.getPacketLength()); //Send -> Base xbee.send(tx64request2); //send Kalman data agz.createReceiveStatusCommandwithKalman(MyID,SenderIDc,myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL); //Select Destination ZBTxRequest tx64request3(collect_Address[id],agz.packetData,agz.getPacketLength()); //Send -> Base xbee.send(tx64request3); // } break; } case RECEIVE_STATUS:{ //printf("Receive Status\n"); if(SenderIDc == '0'){ id = 0; manager.set_state(buf1[9]); manager.set_LatitudeH(&buf1[13]); manager.set_LatitudeL(&buf1[17]); manager.set_LongitudeH(&buf1[21]); manager.set_LongitudeL(&buf1[25]); agz.reNewBasePoint(id,manager.get_LatitudeH(),manager.get_LatitudeL(),manager.get_LongitudeH(),manager.get_LongitudeL()); } if(SenderIDc >= 'A' && SenderIDc <= 'Z'){ id = SenderIDc - 'A'; robot[id].set_state(buf1[9]); robot[id].set_LatitudeH(&buf1[13]); robot[id].set_LatitudeL(&buf1[17]); robot[id].set_LongitudeH(&buf1[21]); robot[id].set_LongitudeL(&buf1[25]); agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL()); } if(SenderIDc >= 'a' && SenderIDc <= 'z'){ id = SenderIDc - 'a'; base[id].set_state(buf1[9]); base[id].set_LatitudeH(&buf1[13]); base[id].set_LatitudeL(&buf1[17]); base[id].set_LongitudeH(&buf1[21]); base[id].set_LongitudeL(&buf1[25]); agz.reNewBasePoint(id,base[id].get_LatitudeH(),base[id].get_LatitudeL(),base[id].get_LongitudeH(),base[id].get_LongitudeL()); } /* printf("%d,", buf1[5]); printf(" %ld, %ld, %ld, %ld\n", robot[id].get_LatitudeH(), robot[id].get_LatitudeL(), robot[id].get_LongitudeH(), robot[id].get_LongitudeL()); */ break; } case RECEIVE_KALMAN:{ id = buf1[5] - 'A'; robot[id].set_state(buf1[9]); robot[id].set_LatitudeKH(&buf1[13]); robot[id].set_LatitudeKL(&buf1[17]); robot[id].set_LongitudeKH(&buf1[21]); robot[id].set_LongitudeKL(&buf1[25]); agz.reNewBasePointKalman(id,robot[id].get_LatitudeKH(),robot[id].get_LatitudeKL(),robot[id].get_LongitudeKH(),robot[id].get_LongitudeKL()); break; } default:{ break; } } } } myGPS.read(); //recive gps module //check if we recieved a new message from GPS, if so, attempt to parse it, if ( myGPS.newNMEAreceived() ) { if ( !myGPS.parse(myGPS.lastNMEA()) ) { continue; } } if (refresh_Timer.read_ms() >= refresh_Time) { refresh_Timer.reset(); if (myGPS.fix) { agz.nowStatus = GPS_AVAIL; if(flag){ if(myGPS.longitudeH==139 && myGPS.latitudeH==37){ flag = false; x[0][0]=(double)myGPS.longitudeL; x[0][1]=(double)myGPS.latitudeL; } } if(myGPS.longitudeH!=139 || myGPS.latitudeH!=37) continue; //Kalman Filter Kalman(myGPS.longitudeL,myGPS.latitudeL); //kousinn for(int i = 0;i < 2;i++){ for(int j = 0;j < 2;j++){ K[0][i][j]=K[1][i][j]; x[0][i]=x[1][i]; sigma[0][i][j]=sigma[1][i][j]; } } myGPS.longitudeKH=myGPS.longitudeH;//longitude after filtering myGPS.latitudeKH=myGPS.latitudeH;//latitude after filtering myGPS.longitudeKL=(long)x[1][0];//longitude after filtering myGPS.latitudeKL=(long)x[1][1];//latitude after filtering agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL); //printf("state = %d\n",agz.nowMode); //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); } else agz.nowStatus = GPS_UNAVAIL; } //get base GPS //if this program is for base,this program don't execute if( collect_Timer.read_ms() >= collect_Time){ collect_Timer.reset(); //printf("Send Request:%d,%d\n",collect_flag,collect_state); agz.createRequestCommand(MyID, collect_state); //Select Destination ZBTxRequest tx64request(collect_Address[collect_flag],agz.packetData,agz.getPacketLength()); //Send -> Base xbee.send(tx64request); collect_flag++; collect_state++; if(collect_flag == 4){ collect_state = 'a'; collect_flag = 0; } } } }