![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
change main() longitudeL: from 138 to 140
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed
Fork of Aigamozu_Robot_ver3_4 by
Diff: main.cpp
- Revision:
- 0:daab5accfd83
- Child:
- 1:b2b950b916ce
diff -r 000000000000 -r daab5accfd83 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue May 12 11:49:24 2015 +0000 @@ -0,0 +1,312 @@ +/**********************************************/ +// +// +// +// Program name: Aigamozu ROBOT +// Author: Mineta Kizuku +// +// +/**********************************************/ + +/**********************************************/ +//更新情報 +//2015/05/11 +//ロボットプログラムの作成 +// +// +// +/**********************************************/ + +#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" + +//************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); + +///////////////////////////////////////// +// +//Address List +// +///////////////////////////////////////// +XBeeAddress64 base_Address[BaseNumber] = {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[RobotNumber] = {XBeeAddress64(ROBOT1_32H,ROBOT1_32L), XBeeAddress64(ROBOT1_32H,ROBOT1_32L)}; +XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L); + + +///////////////////////////////////////// +// +//Send_Status +// +//リクエストがきたとき、自分の位置情報などを返信する +///////////////////////////////////////// +void Send_Status(char SenderIDc){ + XBeeAddress64 send_Address; + 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']; + } + //send normal data + //Create GPS Infomation Packet + agz.createReceiveStatusCommand(MyID,SenderIDc, + 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; + +} + + +///////////////////////////////////////// +// +//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]; + } + } + + 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 = 2000; //refresh time in ms + myGPS.begin(GPS_BAUD_RATE); + + 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(); + + 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 STATUS_REQUEST:{ + Send_Status(SenderIDc); + 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; + } + } + + + if (refresh_Timer.read_ms() >= refresh_Time) { + refresh_Timer.reset(); + Get_GPS(&myGPS); + + } + } +} \ No newline at end of file