展示会用に簡単にしています
Dependencies: ADXL345 AigamozuControlPackets_展示会 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed
Fork of Aigamozu_Robot_March by
Diff: main.cpp
- Revision:
- 33:3025b16bccd2
- Parent:
- 32:6ba2e5402f00
- Child:
- 34:9c3192c00e41
--- a/main.cpp Sun May 24 06:38:54 2015 +0000 +++ b/main.cpp Sun May 24 08:06:17 2015 +0000 @@ -33,6 +33,9 @@ //2015/05/17 //Get_GPS()の中身longitudeの範囲138〜140に変更 // +//2015/05/19 +//autoモードのとき、三十秒前進・三秒右に転回に変更した。 +// //2015/05/24 //Kalmanフィルターを十進数で計算するようにした。 //Kalmanフィルターの計算式を変更した。 @@ -184,21 +187,19 @@ if(flag < COUNTER_MAX){ flag++; } - if(flag >= 5){ + if(flag == 5){ x_prev = agz.get_agzPoint_lati(); y_prev = agz.get_agzPoint_longi(); } if(flag >= 6){ Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi()); - agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL); } - - 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() - ); + + printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \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()); } else agz.nowStatus = GPS_UNAVAIL;