cansat program1
Dependencies: ADXL345 BME280 HMC5883L ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST cansat mbed
Fork of Cansat_program4_1 by
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; }