aigamozu / Mbed 2 deprecated Aigamozu_Robot_ver4_1

change kalman

Dependencies:   ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed

Fork of Aigamozu_Robot_ver4 by aigamozu

Files at this revision

API Documentation at this revision

Revision 35:3094c84a024b, committed 2015-05-27

Comitter:
s1200058
Date:
Wed May 27 12:10:20 2015 +0000
Parent:
34:9c3192c00e41
Commit message:
define the range of error

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 9c3192c00e41 -r 3094c84a024b main.cpp
--- a/main.cpp	Mon May 25 11:06:34 2015 +0000
+++ b/main.cpp	Wed May 27 12:10:20 2015 +0000
@@ -99,6 +99,8 @@
 /////////////////////////////////////////
 #define FIRST_S2 0.000001
 #define COUNTER_MAX 10000
+#define ERROR_RANGE 0.001
+
 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
 double s2x_cur=FIRST_S2,s2x_prev=FIRST_S2,s2y_cur=FIRST_S2,s2y_prev=FIRST_S2;//緯度経度のの時刻tと時刻t-1での共分散
 double s2_R=FIRST_S2;//GPSセンサの分散
@@ -198,7 +200,7 @@
         }
             
         if(flag >= 6){
-            if(abs(x_prev - agz.get_agzPoint_lati()) < 0.001 && abs(y_prev - agz.get_agzPoint_longi()) < 0.001){
+            if(abs(x_prev - agz.get_agzPoint_lati()) < ERROR_RANGE && abs(y_prev - agz.get_agzPoint_longi()) < ERROR_RANGE){
                 Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
                 change = 1;
             }