![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
test
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of aigamozu_auto_ver3_4 by
Diff: main.cpp
- Revision:
- 0:567aa3346701
diff -r 000000000000 -r 567aa3346701 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Apr 27 12:08:38 2015 +0000 @@ -0,0 +1,336 @@ +/**********************************************/ +// +// +// +// Program name: Aigamozu Robot Control +// Author: Atsunori Maruyama +// Ver -> 1.3 +// Day -> 2014/06/09 +// +// +/**********************************************/ + +#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 robot[5]; + +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[4] = {XBeeAddress64(BASE1_32H,BASE1_32L), XBeeAddress64(BASE2_32H,BASE2_32L), + XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)}; + XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L); + int id,SenderIDc; + bool flag = true; + + //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{ + + //send normal data + //Create GPS Infomation Packet + agz.createReceiveStatusCommand(MyID,SenderIDc,myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); + //Select Destination + ZBTxRequest tx64request2(collect_Address[id],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"); + + id = buf1[5] - '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()); + /* + 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; + } + } + + } +} \ No newline at end of file