aigamozu / Mbed 2 deprecated Aigamozu_Robot_2016_ver1

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 CanSat2015aizu

Diff: main.cpp

Revision:
40:aa600a5ba02c
Parent:
39:ff6819c7a066
Child:
41:d6350b57f02a
--- 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;