change kalman
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed
Fork of Aigamozu_Robot_ver4 by
Diff: main.cpp
- Revision:
- 3:1ac506a96fd6
- Parent:
- 2:886fac7f4399
- Child:
- 4:f36986ceb73d
--- a/main.cpp Wed May 13 06:04:47 2015 +0000 +++ b/main.cpp Wed May 13 06:29:21 2015 +0000 @@ -256,6 +256,36 @@ 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()); + +} + ///////////////////////////////////////// // @@ -344,12 +374,11 @@ Adafruit_GPS myGPS(gps_Serial); Timer refresh_Timer; const int refresh_Time = 2000; //refresh time in ms - Timer collect_Timer; - const int collect_Time = 2000; //refresh time in ms + Timer auto_Timer; + const int auto_Time = 2000; //refresh time in ms myGPS.begin(GPS_BAUD_RATE); char SenderIDc; - int basenumber=0; //GPS Send Command myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); @@ -359,7 +388,7 @@ //interrupt start refresh_Timer.start(); - collect_Timer.start(); + auto_Timer.start(); printf("start\n"); @@ -423,13 +452,10 @@ Get_GPS(&myGPS); } - - //一定時間ごとに基地局のGPSデータを取得し、AigamozuControlPacketsないのagzBasePointとagzBasePointKalmanに格納する - if (collect_Timer.read_ms() >= collect_Time) { - collect_Timer.reset(); - Send_Request_Base(basenumber); - basenumber++; - if(basenumber > 4)basenumber=0; + + if(agz.nowMode == AUTO_MODE && auto_Timer.read_ms() >= auto_Time){ + auto_Timer.reset(); + auto_Move(); } } } \ No newline at end of file