Diff: main.cpp

Revision:

3:0bd9ad37f319

Parent:

1:f1f7413ae6bd

Child:

4:0fc7221e2f79

--- a/main.cpp	Sat Jul 11 06:50:23 2015 +0000
+++ b/main.cpp	Thu Jul 23 08:55:47 2015 +0000
@@ -11,6 +11,7 @@
 #include "aigamozuSetting.h"
 #include "HMC5883L.h"
 #include "VNH5019.h"
+#include "cansat.h"
 #include "math.h"
 
 #define SIGMA_MIN 0.0001
@@ -43,6 +44,7 @@
 
 //set up AigamozuControlPackets library
 //AigamozuControlPackets agz(agz_motorShield);
+CanSat cansat(agz_motorShield);
 
 
 /////////////////////////////////////////
@@ -97,11 +99,9 @@
     s2x_prev = s2x_cur;
     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);
-    
+    //robotK\x,robotK_yに格納する
+    cansat.set_robotKalman_x(x_cur);
+    cansat.set_robotKalman_y(y_cur);
 }
 
 /////////////////////////////////////////
@@ -115,20 +115,22 @@
     
     if (myGPS->fix) {
         
-        //agz.nowStatus = GPS_AVAIL;
-        //agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
+        cansat.nowStatus = GPS_AVAIL;
+        cansat.set_robot_x((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 10000.0));
+        cansat.set_robot_y((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 10000.0));
+    
         
         if(flag < COUNTER_MAX){
             flag++; 
         }
         if(flag == 5){
-            //x_prev = agz.get_agzPoint_lati();
-            //y_prev = agz.get_agzPoint_longi();
+            x_prev = cansat.get_robot_x();
+            y_prev = cansat.get_robot_y();
         }
             
         if(flag >= 6){
-            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());
+            if(abs(x_prev - cansat.get_robot_x()) < ERROR_RANGE && abs(y_prev - cansat.get_robot_y()) < ERROR_RANGE){
+                Kalman(cansat.get_robot_x(), cansat.get_robot_y());
                 change = 1;
             }
             else{
@@ -140,7 +142,7 @@
                 //agz.get_agzCov_lati(),agz.get_agzCov_longi());
         }
     }
-    else ;//agz.nowStatus = GPS_UNAVAIL;
+    else cansat.nowStatus = GPS_UNAVAIL;
     
 }