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:
- 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);