展示会用です 内容をすっきりさせました
Dependencies: ADXL345 AigamozuControlPackets_展示会 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed
Fork of Aigamozu_Robot_ver4 by
Diff: main.cpp
- Revision:
- 34:9c3192c00e41
- Parent:
- 33:3025b16bccd2
- Child:
- 35:c18c96b92121
diff -r 3025b16bccd2 -r 9c3192c00e41 main.cpp --- a/main.cpp Sun May 24 08:06:17 2015 +0000 +++ b/main.cpp Mon May 25 11:06:34 2015 +0000 @@ -50,6 +50,7 @@ #include "agzIDLIST.h" #include "aigamozuSetting.h" #include "Kalman.h" +#include "math.h" #define SIGMA_MIN 0.0001 @@ -104,6 +105,10 @@ double Kx=0,Ky=0;//カルマンゲイン double zx,zy;//観測値 void Kalman(double Latitude,double Longitude); +int change = 0; + + +LocalFileSystem local("local"); // マウントポイントを定義(ディレクトリパスになる) ///////////////////////////////////////// // @@ -193,7 +198,20 @@ } if(flag >= 6){ - Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi()); + if(abs(x_prev - agz.get_agzPoint_lati()) < 0.001 && abs(y_prev - agz.get_agzPoint_longi()) < 0.001){ + Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi()); + change = 1; + } + else{ + change = 0; + } + /* fp = fopen(filename, "w"); + fprintf(fp, "%d %.14lf %.14lf %.14lf %.14lf %.14le %.14le \n", + change, 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", @@ -510,4 +528,5 @@ } } } + } \ No newline at end of file