Diff: main.cpp

Revision:

41:d6350b57f02a

Parent:

40:aa600a5ba02c

Child:

42:5cdd92577dfe

--- a/main.cpp	Wed May 25 11:26:20 2016 +0000
+++ b/main.cpp	Wed May 25 14:34:05 2016 +0000
@@ -60,8 +60,7 @@
 //Base    ID: 'a' ~ 'a'
 //manager ID: '0'(数字のゼロ)
 //
-const char MyID = 'C';
-const int other_robot = 7;
+const char MyID = 'A';
 //************ID Number*****************
 
 /////////////////////////////////////////
@@ -202,12 +201,13 @@
         if(flag < COUNTER_MAX) {
             flag++;
         }
-        if(flag == KALMAN_START_TIME && agz.nowMode == AUTO_GPS_MODE) {
+        if(/*flag == KALMAN_START_TIME */agz.nowMode == AUTO_GPS_MODE && agz.auto_count == 0) {
             x_prev = agz.get_agzPoint_lati();
             y_prev = agz.get_agzPoint_longi();
+            agz.auto_count++;
         }
 
-        if(flag >= KALMAN_START_TIME+1 && agz.nowMode == AUTO_GPS_MODE) {
+        if(/*flag >= KALMAN_START_TIME+1 && agz.nowMode == AUTO_GPS_MODE && */agz.auto_count > 0) {
             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;
@@ -235,14 +235,6 @@
                agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
                agz.get_agzCov_lati(),agz.get_agzCov_longi());
 
-        agz.createReceiveStatusCommand(MyID, other_robot, agz.nowMode,
-                                       agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
-                                       agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
-                                       agz.get_agzCov_lati(),agz.get_agzCov_longi());
-        //Select Destination
-        ZBTxRequest tx64request(robot_Address[other_robot],agz.packetData,agz.getPacketLength());
-        //Send -> Base
-        xbee.send(tx64request);
     } else agz.nowStatus = GPS_UNAVAIL;
 
 }
@@ -285,7 +277,7 @@
         int id = SenderIDc - 'a';
         printf("Get Base data %d\n", id+1);
         agz.reNewBasePoint(id,&packetdata[13],&packetdata[21]);
-        //agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
+        agz.reNewBasePointKalman(id,&packetdata[29],&packetdata[37]);
 
         //debug
         printf("latitude:%f,longitude:%f\n",
@@ -435,10 +427,8 @@
     Timer auto_Timer;
     const int auto_Time = 2000; //refresh time in ms
     int count = 0;
-    Timer startGPS_Timer;
-    const int startGPS_Time = 180000;
 
-    const int straight = 8000, turning = 12000, wait = 10000;
+//    const int straight = 8000, turning = 12000, wait = 10000;
 
     myGPS.begin(GPS_BAUD_RATE);
 
@@ -459,9 +449,9 @@
     auto_Timer.start();
     agz.Move_Timer.start();
     collect_Timer.start();
-    startGPS_Timer.start();
     printf("start\n");
 
+    agz.auto_count = 0;
 
     while (true) {
 
@@ -520,9 +510,7 @@
         if (refresh_Timer.read_ms() >= refresh_Time) {
             refresh_Timer.reset();
             //print_gps(count);
-            if(startGPS_Timer.read_ms() >= startGPS_Time) {
-                Get_GPS(&myGPS);
-            }
+            Get_GPS(&myGPS);
         }
 
         //get base GPS
@@ -542,23 +530,7 @@
             auto_Timer.reset();
             auto_Move();
         }
-
-        if(agz.nowMode == AUTO_GPS_MODE) {
-            if(agz.Move_Timer.read_ms() < straight) {
-                agz.test_Auto(0); //straight
-            }
-            if(agz.Move_Timer.read_ms() > straight && agz.Move_Timer.read_ms() < wait) {
-                agz.test_Auto(1);
-            }
-            if(agz.Move_Timer.read_ms() > wait && agz.Move_Timer.read_ms() < turning) {
-                agz.test_Auto(2); //Turn Right
-            }
-            if(agz.Move_Timer.read_ms() > turning) {
-                agz.test_Auto(3);
-                wait_ms(200);
-                agz.Move_Timer.reset();
-            }
-        }
+        
     }
 
 }
\ No newline at end of file