Revision 3:0bd9ad37f319, committed 2015-07-23

Comitter:

kityann

Date:

Thu Jul 23 08:55:47 2015 +0000

Parent:

2:de5ce1b56c0d

Commit message:

2015/07/23

Changed in this revision

diff -r de5ce1b56c0d -r 0bd9ad37f319 CanSat.lib
--- a/CanSat.lib	Sat Jul 11 06:50:23 2015 +0000
+++ b/CanSat.lib	Thu Jul 23 08:55:47 2015 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/CanSat2015aizu/code/CanSat/#3f50511c1c1f
+https://developer.mbed.org/teams/CanSat2015aizu/code/CanSat/#0f76226be922

diff -r de5ce1b56c0d -r 0bd9ad37f319 VNH5019.lib
--- a/VNH5019.lib	Sat Jul 11 06:50:23 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/teams/aigamozu/code/VNH5019/#1bcdb655df71

diff -r de5ce1b56c0d -r 0bd9ad37f319 main.cpp
--- 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;
     
 }