20150512 Nakazawa
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of Aigamozu_Robot_ver1_0 by
Diff: main.cpp
- Revision:
- 1:d10d72b1d29b
- Parent:
- 0:eee5e3d906ce
diff -r eee5e3d906ce -r d10d72b1d29b main.cpp --- a/main.cpp Tue May 12 01:03:31 2015 +0000 +++ b/main.cpp Tue May 12 10:21:05 2015 +0000 @@ -13,6 +13,8 @@ //2015/05/11 //ベースプログラムの作成 // +//void Get_GPS()内でLongitudeL, LatitudeL それぞれに+10する処理。 +//main()内でAutoModeのときでかつ2000ms立った時にGPSを取得する。 // // /**********************************************/ @@ -83,6 +85,7 @@ XBeeAddress64 manager_Address = XBeeAddress64(BASE1_32H,BASE1_32L); + ///////////////////////////////////////// // //Send_Status @@ -102,13 +105,13 @@ } //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()); +// 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()); //Select Destination - ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength()); +// ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength()); //Send -> Base - xbee.send(tx64request); +// xbee.send(tx64request); } ///////////////////////////////////////// @@ -134,6 +137,7 @@ if(myGPS->longitudeH<138 || myGPS->longitudeH>141 || myGPS->latitudeH<36 || myGPS->latitudeH>38){ return; } + //Kalman Filter Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS); @@ -212,6 +216,27 @@ agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]); } +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()); + +} ///////////////////////////////////////// // @@ -235,6 +260,10 @@ myGPS.begin(GPS_BAUD_RATE); char SenderIDc; + + Timer auto_Timer; + const int auto_Time = 2000; + //GPS Send Command myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); @@ -244,6 +273,7 @@ //interrupt start refresh_Timer.start(); + auto_Timer.start(); printf("start\n"); @@ -276,6 +306,8 @@ }//endswitch }//endifZB_RX_RESPONSE }//endifisAvailable + + myGPS.read(); //recive gps module @@ -298,5 +330,11 @@ ); } + + if(agz.nowMode == AUTO_MODE && auto_Timer.read_ms() >= auto_Time){ + auto_Timer.reset(); + auto_Move(); + } + } } \ No newline at end of file