aigamozu / Mbed 2 deprecated Aigamozu_Robot_2016_ver1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ADXL345 AigamozuControlPackets_2016 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST_2016 mbed

Fork of Aigamozu_Robot_ver4 by CanSat2015aizu

Diff: main.cpp

Revision:
34:9c3192c00e41
Parent:
33:3025b16bccd2
Child:
35:c18c96b92121
--- 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