Diff: main.cpp

Revision:

14:1b5be519bd49

Parent:

13:38c5ffe5873a

Child:

15:4779723a4f75

--- a/main.cpp	Wed Oct 27 06:47:49 2021 +0000
+++ b/main.cpp	Wed Oct 27 08:11:10 2021 +0000
@@ -1,19 +1,10 @@
 // ライブラリ
 #include "mbed.h"
-#include "TB6612.h"
-#include "ATP3011.h" 
-#include "HMC6352.h"
+
 // 自作関数
-#include "AngleGet.h"
-#include "Avoid.h"  // 廃止予定 
- #include "getGPS.h"
-#include "catchGPS.h"
-#include "FrontGet.h"
-#include "MotorDriver.h"
+#include "Function.h"
 
-Serial pc(USBTX, USBRX);
 // フライトピン・ニクロム線関係
-Serial pc(SERIAL_TX, SERIAL_RX);
 DigitalIn flight_pin(A0);
 DigitalOut nichrome(D13);
 // 
@@ -30,14 +21,15 @@
     
     // 落下検知
     // パラシュート分離
-        wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
-        while(flight_pin){}
+    wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
+    while(flight_pin){
         pc.printf("flight_pin nuketa");
         wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
         nichrome=1;
         pc.printf("nichrome in");
         wait(30);
         nichrome=0;
+    }
     // 落下終了
     
     
@@ -48,29 +40,36 @@
         int i;
         for (i = CP_num; last_num; i++) {
             // 移動
-            catchGPS(&GPS_x, &GPS_y);
-            AngleGet();
-            MotorDriver(4, 0.5);    //回転
-            wait();
-            MotorDriver(1, 0);      //回転停止
+        catchGPS();
+            
+            while (1) {
+                direction = AngleGet();
+                pc.printf("direction=%f", direction);
+                if(direction < 5 || direction > 355) {  //角度判定
+                    Move('2', 1);
+                }
+                else {
+                    Move('1', 0);
+                    Move('4', 0.5);
+                }
                 
-            MotorDriver(2, 0.5);  // 前進開始
-        while (True) {
-            if (FrontGet()) {
-                MotorDriver(1, 0);      //停止
-                MotorDriver(4, 0.5);    //回転
-                wait();
-                MotorDriver(1, 0);      //回転停止
-                continue;
-            } else {
-                MotorDriver(2, 0.5);
-            }
-            catchGPS(&GPS_x, &GPS_y);
-            if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x) + (next_CP_y = GPS_y)*(next_CP_y = GPS_y) < 5) { // CP到着判定 //試験で調整
-                break;
+                if (FrontGet()) {
+                    Move('1', 0);      //停止
+                    Move('4', 0.5);    //回転
+                    wait(1);
+                    Move('1', 0);      //回転停止
+                    continue;
+                } 
+                else {
+                    Move('2', 0.5);
+                }
+                catchGPS();
+                if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x) + (next_CP_y = GPS_y)*(next_CP_y = GPS_y) < 5) { // CP到着判定 //試験で調整
+                    break;
+                }
             }
         }
-    }
     // 行動フロー終了
     return 0;
+    }
 }