20150511, Nakazawa
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of aigamozu_auto_ver3_3_2 by
Revision 7:209c07cc2b80, committed 2015-05-11
- Comitter:
- s1210160
- Date:
- Mon May 11 13:23:52 2015 +0000
- Parent:
- 6:11a8ae7ab71e
- Commit message:
- 20150511, Nakazawa
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun Apr 26 16:52:54 2015 +0000 +++ b/main.cpp Mon May 11 13:23:52 2015 +0000 @@ -9,6 +9,16 @@ // // /**********************************************/ +// +// 中澤遥菜 +// 変更点:メイン関数の中の処理を関数化しました。 +// setting(), interrupt_start(VNH5019 motorShield), +// GPS_Setting(int refresh_Time, int collect_Time), +// GPS_SendCommand(), check_CommandType(utf8_t command_type), +// LatLong_afterFiltering(double longitude, double latitude, double x0, double x1), +// send_GPSdata(), get_GPSdata(), getKalman() +// +/**********************************************/ #include "mbed.h" #include "XBee.h" @@ -106,65 +116,72 @@ get_sigma(); } - ///////////////////////////////////////// // -//Main Processing +//setting // ///////////////////////////////////////// -int main() { - //start up time +void setting(){ + //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); + } + +///////////////////////////////////////// +// +//interrupt start +// +///////////////////////////////////////// +void interrupt_start(VNH5019 motorShield, Timer refresh){ + AigamozuControlPackets agz(motorShield); + refresh.start(); - //GPS setting + printf("start\n"); +} + +///////////////////////////////////////// +// +//GPS Setting +// +///////////////////////////////////////// +XBeeAddress64 collect_Address[5]; +XBeeAddress64 robot_Address + +void GPS_Setting(Timer refresh_Timer, Timer collect_Timer){ gps_Serial = new Serial(p28,p27); Adafruit_GPS myGPS(gps_Serial); Timer refresh_Timer; - const int refresh_Time = 2000; //refresh time in ms + const int refresh_Time = refresh_Timer; //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), + const int collect_Time = collect_Timer; //when we collect 4 GPS point, we use + 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; - bool flag = true; - - //GPS Send Command + robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L); + } + +///////////////////////////////////////// +// +//GPS Send Command +// +///////////////////////////////////////// +void GPS_SendCommand(){ 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("test\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'; - - - //Check Command Type - switch(agz.checkCommnadType(buf)){ +///////////////////////////////////////// +// +//Check Command Type +// +///////////////////////////////////////// +void check_CommandType(utf8_t command_type, AigamozuControlPackets ai(motorShield), ){ + switch(command_type){ //CommandType -> ChanegeMode case CHANGE_MODE :{ @@ -183,26 +200,12 @@ //CommandType -> Send Status case STATUS_REQUEST:{ + //send normal data //Create GPS Infomation Packet agz.createReceiveStatusCommand(Base_ID,'a',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(Base_ID,'a',myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL); - //Select Destination - ZBTxRequest tx64request1(robot_Address,agz.packetData,agz.getPacketLength()); - //Send -> Base - xbee.send(tx64request); - - //send normal data - //Create GPS Infomation Packet - agz.createReceiveStatusCommand(Base_ID,'a',myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); - //Select Destination - ZBTxRequest tx64request2(collect_Address[id],agz.packetData,agz.getPacketLength()); + ZBTxRequest tx64request2(collect_Address[id],ai.packetData,ai.getPacketLength()); //Send -> Base xbee.send(tx64request); @@ -215,47 +218,109 @@ 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; } } - - - } } +///////////////////////////////////////// +// +//latitude and longitude after filtering +// +///////////////////////////////////////// +void LatLong_afterFiltering(double longitude, double latitude, double x0, double x1){ + myGPS.longitudeKH=longitude;//longitude after filtering + myGPS.latitudeKH=latitude;//latitude after filtering + myGPS.longitudeKL=(long)x0;//longitude after filtering + myGPS.latitudeKL=(long)x1;//latitude after filtering +} + +///////////////////////////////////////// +// +//send_GPSdata() +// +///////////////////////////////////////// +void send_GPSdata(){ + 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';+ + + //Check Command Type + check_CommandType(agz.checkCommnadType(buf)); + } +} + +///////////////////////////////////////// +// +//get_GPSdata +// +///////////////////////////////////////// +void get_GPSdata(){ + 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; +} + +///////////////////////////////////////// +// +//get_Kalman +// +///////////////////////////////////////// +void get_Kalman(){ + //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]; + } + } + + LatLong_afterFiltering(myGPS.longitudeH, myGPS.latitudeH, x[1][0], x[1][1]); + } +///////////////////////////////////////// +// +//Main Processing +// +///////////////////////////////////////// +int main() { + setting(); + + //GPS setting + GPS_Setting(2000, 200); + int collect_flag = 0; + char collect_state = 'a'; + int id; + bool flag = true; + + //GPS Send Command + GPS_SendCommand(); + + wait(2); + + //interrupt start + interrupt_start(agz_motorShield, refreshTime); + + while (true) { + + //Check Xbee Buffer Available + xbee.readPacket(); + if (xbee.getResponse().isAvailable()) { + send_GPSdata(); + } + myGPS.read(); //recive gps module //check if we recieved a new message from GPS, if so, attempt to parse it, @@ -269,30 +334,9 @@ 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; + get_GPSdata(); - //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 + get_Kalman(); agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL); agz.reNewRobotPointKalman(myGPS.longitudeKH,myGPS.longitudeKL,myGPS.latitudeKH,myGPS.latitudeKL); @@ -303,26 +347,6 @@ } - - //get base GPS - if( collect_Timer.read_ms() >= collect_Time){ - collect_Timer.reset(); - - //printf("Send Request:%d,%d\n",collect_flag,collect_state); - - agz.createRequestCommand('A', 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 == 5){ - collect_state = 'a'; - collect_flag = 0; - } - } + } } \ No newline at end of file