Diff: main.cpp

Revision:

1:f1f7413ae6bd

Parent:

0:1fcc61be1dcf

Child:

3:0bd9ad37f319

--- a/main.cpp	Thu Jul 09 07:46:44 2015 +0000
+++ b/main.cpp	Sat Jul 11 06:35:56 2015 +0000
@@ -6,9 +6,11 @@
 #include "mbed.h"
 #include "XBee.h"
 #include "MBed_Adafruit_GPS.h"
-#include "AigamozuControlPackets.h"
+//#include "AigamozuControlPackets.h"
 #include "agzIDLIST.h"
 #include "aigamozuSetting.h"
+#include "HMC5883L.h"
+#include "VNH5019.h"
 #include "math.h"
 
 #define SIGMA_MIN 0.0001
@@ -40,7 +42,7 @@
 //set up GPS module
 
 //set up AigamozuControlPackets library
-AigamozuControlPackets agz(agz_motorShield);
+//AigamozuControlPackets agz(agz_motorShield);
 
 
 /////////////////////////////////////////
@@ -96,9 +98,9 @@
     s2y_prev = s2y_cur;
     
     //agzPontKalmanとagzCovに格納する
-    agz.set_agzPointKalman_lati(x_cur);
-    agz.set_agzPointKalman_longi(y_cur);
-    agz.set_agzCov(s2x_cur,s2y_cur);
+    //agz.set_agzPointKalman_lati(x_cur);
+    //agz.set_agzPointKalman_longi(y_cur);
+    //agz.set_agzCov(s2x_cur,s2y_cur);
     
 }
 
@@ -113,31 +115,32 @@
     
     if (myGPS->fix) {
         
-        agz.nowStatus = GPS_AVAIL;
-        agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
+        //agz.nowStatus = GPS_AVAIL;
+        //agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
         
         if(flag < COUNTER_MAX){
             flag++; 
         }
         if(flag == 5){
-            x_prev = agz.get_agzPoint_lati();
-            y_prev = agz.get_agzPoint_longi();
+            //x_prev = agz.get_agzPoint_lati();
+            //y_prev = agz.get_agzPoint_longi();
         }
             
         if(flag >= 6){
-            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());
+            if(1){//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;
             }
             else{
                 change = 0;
             }            
-                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());
+                //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;
+    else ;//agz.nowStatus = GPS_UNAVAIL;
     
 } 
 
@@ -189,8 +192,6 @@
     Adafruit_GPS myGPS(gps_Serial); 
     Timer refresh_Timer;
     const int refresh_Time = 1000; //refresh time in ms
-    Timer auto_Timer;
-    const int auto_Time = 100; //refresh time in ms
     int count = 0;
 
     myGPS.begin(GPS_BAUD_RATE);