![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
for log
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed
Fork of Aigamozu_Robot_ver4_1 by
main.cpp
- Committer:
- s1200058
- Date:
- 2015-05-16
- Revision:
- 21:76e7f36df4a9
- Parent:
- 20:eee8ac8d1788
- Child:
- 22:af860680417b
File content as of revision 21:76e7f36df4a9:
/**********************************************/ // // // // Program name: Aigamozu ROBOT // Author: Mineta Kizuku // // // /**********************************************/ /**********************************************/ //更新情報 //2015/05/11 //ロボットプログラムの作成 // //2015/05/13 //カルマンフィルタの共分散の値を0.0001以下にならないようにした //共分散の値を10進数に変換するようにした // //2015/05/13 Yokokawa //何回目のGPSでとられたGPSか確認するようにしました。 // //2015/05/15 //プログラムcreateReceiveStatusCommand()にて //Aigamozu_collect_dataにinかoutかを送るためにstate関連をいじったので必要に応じて直してください。 // /**********************************************/ #include "mbed.h" #include "XBee.h" #include "MBed_Adafruit_GPS.h" #include "AigamozuControlPackets.h" #include "agzIDLIST.h" #include "aigamozuSetting.h" #include "Kalman.h" #define SIGMA_MIN 0.0001 //************ID Number***************** //Robot ID: 'A' ~ 'Z' //Base ID: 'a' ~ 'a' //manager ID: '0'(数字のゼロ) // const char MyID = 'D'; //************ID Number***************** ///////////////////////////////////////// // //Pin Setting // ///////////////////////////////////////// VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26); ///////////////////////////////////////// // //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(); //set up GPS module //set up AigamozuControlPackets library AigamozuControlPackets agz(agz_motorShield); ///////////////////////////////////////// // //For Kalman data // ///////////////////////////////////////// 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 Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS); ///////////////////////////////////////// // //Plus Speed // //MNUAL_MODEの時にスピードを変える ///////////////////////////////////////// void Plus_Speed(uint8_t *packetdata){ if(agz.nowMode == MANUAL_MODE){ agz.changeSpeed(packetdata); } } ///////////////////////////////////////// // //Send Status // //リクエストがきたとき、自分の位置情報などを返信する ///////////////////////////////////////// void Send_Status(char SenderIDc){ XBeeAddress64 send_Address; if(SenderIDc == 'Z'){ 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']; } //send normal data //Create GPS Infomation Packet agz.createReceiveStatusCommand(MyID,SenderIDc, (int)agz.gpsAuto(), agz.get_agzPoint_lati(),agz.get_agzPoint_longi(), agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(), agz.get_agzCov_lati(),agz.get_agzCov_longi()); //debug*************************************************** printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n", agz.get_agzPoint_lati(),agz.get_agzPoint_longi(), agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(), agz.get_agzCov_lati(),agz.get_agzCov_longi() ); for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]); printf("\n"); //debug end*************************************************** //Select Destination ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength()); //Send -> Base xbee.send(tx64request); } ///////////////////////////////////////// // //Get GPS function // ///////////////////////////////////////// void Get_GPS(Adafruit_GPS *myGPS){ static bool flag = true; if (myGPS->fix) { agz.nowStatus = GPS_AVAIL; if(flag){//初期値設定 if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=141){ flag = false; x[0][0]=(double)myGPS->latitudeL; x[0][1]=(double)myGPS->longitudeL; } } if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){ return; } //Kalman Filter Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS); agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL); agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL); } else agz.nowStatus = GPS_UNAVAIL; } ///////////////////////////////////////// // //New Mode // ///////////////////////////////////////// void New_Mode(uint8_t *packetdata){ //bool result; agz.changeMode(packetdata); } ///////////////////////////////////////// // //Get Status // ///////////////////////////////////////// void Get_Status(char SenderIDc,uint8_t *packetdata){ //マネージャからデータが来たとき if(SenderIDc == '0'){ printf("get manager Status\n"); } //他のロボットからデータが来たとき if(SenderIDc >= 'A' && SenderIDc <= 'Z'){ printf("get other robots Status\n"); } //基地局からデータが来たとき if(SenderIDc >= 'a' && SenderIDc <= 'z'){ printf("Get Base data\n"); int id = SenderIDc - 'a'; agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]); agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]); //debug for(int i = 0;i < 4;i++){ printf("BASE:%d\n",i); printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\n", agz.get_basePoint_lati(i),agz.get_basePoint_longi(i), agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i) ); } } } void Get_Status_Kalman(char SenderIDc,uint8_t *packetdata){ //マネージャからデータが来たとき if(SenderIDc == '0'){ printf("get manager Status Kalman\n"); } //他のロボットからデータが来たとき if(SenderIDc >= 'A' && SenderIDc <= 'Z'){ printf("get other robots Status Kalman\n"); } //基地局からデータが来たとき if(SenderIDc >= 'a' && SenderIDc <= 'z'){ printf("Get Base data Kalman\n"); int id = SenderIDc - 'a'; agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]); //debug for(int i = 0;i < 4;i++){ printf("BASE:%d\n",i); printf("latitudeK:%f,longitudeK:%f\n", agz.get_basePointKalman_lati(i),agz.get_basePointKalman_longi(i)); } } } ///////////////////////////////////////// // //Send_Request_to_base // ///////////////////////////////////////// void Send_Request_Base(int basenumber){ printf("send\n"); agz.createRequestCommand(MyID, basenumber); //Select Destination ZBTxRequest tx64request(base_Address[basenumber],agz.packetData,agz.getPacketLength()); //Send -> Base xbee.send(tx64request); } ///////////////////////////////////////// // //auto_Move // //InAreaかOutAreaの判定 //Kalmanを通した値を出力(Baseと自分) ///////////////////////////////////////// void auto_Move(){ bool result; int i; result = agz.gpsAuto(); //agz.set_agzAutoGPS(); //agz.set_agzKalmanGPS(); if(result == true){ printf("Out Area\n"); } else if(result == false){ printf("In Area\n"); } for(i = 0; i < 4; i++){ printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i)); } printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi()); } void print_gps(int count){ printf("%d times:\n", count); printf("%f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi()); } ///////////////////////////////////////// // //Kalman Processing // ///////////////////////////////////////// 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,Adafruit_GPS *myGPS){ 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(); //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]; } } if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN; if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN; myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering myGPS->latitudeKL=(long)x[1][0];//latitude after filtering myGPS->longitudeKL=(long)x[1][1];//longitude after filtering agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]); } ///////////////////////////////////////// // //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 = 1000; //refresh time in ms Timer auto_Timer; const int auto_Time = 2000; //refresh time in ms int count = 0; myGPS.begin(GPS_BAUD_RATE); Timer collect_Timer; const int collect_Time = 2000; //when we collect 4 GPS point, we use int collect_flag = 0; char collect_state = 'a'; char SenderIDc; //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 refresh_Timer.start(); auto_Timer.start(); collect_Timer.start(); printf("start\n"); while (true) { //Check Xbee Buffer Available xbee.readPacket(); if (xbee.getResponse().isAvailable()) { xbee.getResponse().getZBRxResponse(zbRx); uint8_t *buf = zbRx.getFrameData(); if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { xbee.getResponse().getZBRxResponse(zbRx); uint8_t *buf = zbRx.getFrameData();//フレームデータを格納する uint8_t *buf1 = &buf[11];//データの部分のみを格納する SenderIDc = buf1[5];//送信元のIDを取得する char Command_type = agz.checkCommnadType(buf);//コマンドタイプを取得する //Check Command Type switch(Command_type){ //Get Request command case MANUAL:{ Plus_Speed(buf); break; } case STATUS_REQUEST:{ Send_Status(SenderIDc); printf("%c\n", SenderIDc); break; } case CHANGE_MODE:{ New_Mode(buf); break; } case RECEIVE_STATUS:{ Get_Status(SenderIDc,buf1); break; } case RECEIVE_KALMAN:{ Get_Status_Kalman(SenderIDc, buf1); break; } default:{ break; } }//endswitch }//endifZB_RX_RESPONSE }//endifisAvailable 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; } else{ count++; } } //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する if (refresh_Timer.read_ms() >= refresh_Time) { refresh_Timer.reset(); //print_gps(count); Get_GPS(&myGPS); } //get base GPS if( collect_Timer.read_ms() >= collect_Time){ collect_Timer.reset(); Send_Request_Base(collect_flag); collect_flag++; collect_state++; if(collect_flag == 4){ collect_state = 'a'; collect_flag = 0; } } if(agz.nowMode == AUTO_GPS_MODE && auto_Timer.read_ms() >= auto_Time){ auto_Timer.reset(); auto_Move(); } } }