Diff: main.cpp

Revision:

36:a11060f5199e

Parent:

35:3094c84a024b

Child:

37:26374d6066cb

--- a/main.cpp	Wed May 27 12:10:20 2015 +0000
+++ b/main.cpp	Fri May 29 13:07:46 2015 +0000
@@ -41,6 +41,8 @@
 //Kalmanフィルターの計算式を変更した。
 //set_kalmanを追加した。
 //
+//2015/05/29
+//auto_Move関数の実装とAigamozuControlPackets内にcontrol_Mortor関数の実装
 /**********************************************/
 
 #include "mbed.h"
@@ -59,7 +61,7 @@
 //Base    ID: 'a' ~ 'a'
 //manager ID: '0'(数字のゼロ)
 //
-const char MyID = 'D';
+const char MyID = 'A';
 //************ID Number*****************
 
 /////////////////////////////////////////
@@ -316,28 +318,64 @@
 //
 //InAreaかOutAreaの判定
 //Kalmanを通した値を出力(Baseと自分)
+//2015/05/29
+//外側判定と処理の実装
+//内側判定：シーケンス動作
+//外側判定：10秒間バック、3秒間旋回を行い、その後シーケンス動作へ
 /////////////////////////////////////////
 
 void auto_Move(){
  
- bool result;
- int i;
+ bool result; // 毎回の内外判定の結果を格納
+ bool out_flag = false; // 外側処理の実行フラグ
+ const int sequenceTime[4] = {30000, 31000, 34000, 34200};
+ const int outSequenceTime[4] = {10000, 11000, 14000, 14200};
  
  result = agz.gpsAuto();
  //agz.set_agzAutoGPS();
  //agz.set_agzKalmanGPS();
  
- if(result == true){
-        printf("Out Area\n");
- }
-else if(result == false){
-        printf("In Area\n");
-}  
-for(i = 0; i < 4; i++){
-    printf("%d: %f, %f\n", i, agz.get_basePointKalman_lati(i), agz.get_basePointKalman_longi(i));
- }
- printf("robot: %f, %f\n", agz.get_agzPointKalman_lati(), agz.get_agzPointKalman_longi());
+ if(out_flag == false && result == true){
+    out_flag = true;
+    agz.Out_Timer.reset();
+    }
  
+ if(out_flag == false){
+    if(agz.Move_Timer.read_ms() < sequenceTime[0]){
+          agz.control_Motor(0); //straight
+        }
+    if(agz.Move_Timer.read_ms() > sequenceTime[0] && agz.Move_Timer.read_ms() < sequenceTime[1]){
+          agz.control_Motor(1);
+        }
+    if(agz.Move_Timer.read_ms() > sequenceTime[1] && agz.Move_Timer.read_ms() < sequenceTime[2]){
+          agz.control_Motor(2); //Turn Right
+        }   
+    if(agz.Move_Timer.read_ms() > sequenceTime[2] && agz.Move_Timer.read_ms() < sequenceTime[3]){
+          agz.control_Motor(1);
+        }
+    if(agz.Move_Timer.read_ms() > sequenceTime[3]){
+          agz.Move_Timer.reset();
+        }
+    } 
+ if(out_flag == true){
+    if(agz.Out_Timer.read_ms() < outSequenceTime[0]){
+           agz.control_Motor(3); //back
+        }
+    if(agz.Out_Timer.read_ms() > outSequenceTime[0] && agz.Out_Timer.read_ms() < outSequenceTime[1]){
+           agz.control_Motor(1);
+        }
+    if(agz.Out_Timer.read_ms() > outSequenceTime[1] && agz.Out_Timer.read_ms() < outSequenceTime[2]){
+           agz.control_Motor(2); //Turn Right
+        }   
+    if(agz.Out_Timer.read_ms() > outSequenceTime[2] && agz.Out_Timer.read_ms() < outSequenceTime[3]){
+           agz.control_Motor(1);
+        }
+    if(agz.Out_Timer.read_ms() > outSequenceTime[3]){
+           agz.Out_Timer.reset();
+           agz.Move_Timer.reset();
+           out_flag = false;
+        }
+  }
 }
 
 void print_gps(int count){
@@ -406,10 +444,10 @@
     Timer refresh_Timer;
     const int refresh_Time = 1000; //refresh time in ms
     Timer auto_Timer;
-    const int auto_Time = 2000; //refresh time in ms
+    const int auto_Time = 100; //refresh time in ms
     int count = 0;
     
-    const int straight = 30000, turning = 34000, wait = 31000;
+    
     
     myGPS.begin(GPS_BAUD_RATE); 
     
@@ -429,6 +467,7 @@
     refresh_Timer.start();
     auto_Timer.start();
     agz.Move_Timer.start();
+    agz.Out_Timer.start();
     collect_Timer.start();
     printf("start\n");
     
@@ -513,22 +552,6 @@
            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