![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2016_05_19 Auto mode: 10sec forward, 2sec stop, 2sec turn right Please change test Auto pwm
Dependencies: ADXL345 AigamozuControlPackets_2016 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST_2016 mbed
Fork of Aigamozu_Robot_ver4 by
Diff: main.cpp
- Revision:
- 40:aa600a5ba02c
- Parent:
- 39:ff6819c7a066
- Child:
- 41:d6350b57f02a
diff -r ff6819c7a066 -r aa600a5ba02c main.cpp --- a/main.cpp Wed May 25 10:49:29 2016 +0000 +++ b/main.cpp Wed May 25 11:26:20 2016 +0000 @@ -53,6 +53,7 @@ #include "math.h" #define SIGMA_MIN 0.0001 +#define KALMAN_START_TIME 5 //************ID Number***************** //Robot ID: 'A' ~ 'Z' @@ -201,12 +202,12 @@ if(flag < COUNTER_MAX) { flag++; } - if(flag == 15) { + if(flag == KALMAN_START_TIME && agz.nowMode == AUTO_GPS_MODE) { x_prev = agz.get_agzPoint_lati(); y_prev = agz.get_agzPoint_longi(); } - if(flag >= 16) { + if(flag >= KALMAN_START_TIME+1 && agz.nowMode == AUTO_GPS_MODE) { if(abs(x_prev - agz.get_agzPoint_lati()) < 0.001 && abs(y_prev - agz.get_agzPoint_longi()) < 0.001) { Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi()); change = 1;