Diff: main.cpp

Revision:

0:e386cbba36d5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Dec 19 09:31:36 2021 +0000
@@ -0,0 +1,105 @@
+/*ライブラリ*/
+#include "mbed.h"
+
+// 自作関数
+#include "Function.h"
+
+// フライトピン・ニクロム線関係
+DigitalIn flight_pin(A0);
+DigitalOut nichrome(D13);
+// 
+#define cp_max 4    //CPの数を入力する
+
+int main() {
+    // 変数宣言
+    double GPS_x, GPS_y;  // 現在地の座標
+    double direction;     // 次CPへの向き
+    double CPs_x[cp_max]={34.54497251645555,34.545054367534235,34.54502498510097,34.545342036897935};  //CPリスト(x座標)
+    double CPs_y[cp_max]={135.50877273927074,135.50837525002106,135.50798030878718,135.50778889521388};    // CPリスト(y座標)
+    double next_CP_x, next_CP_y;
+    
+    // 落下検知
+    // パラシュート分離
+    
+    wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
+    while(flight_pin){}
+        xbee.printf("flight_pin nuketa\n\r"); 
+        wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
+        nichrome=1;//  ここ変える
+        xbee.printf("nichrome in\n\r");
+        wait(10);
+        nichrome=0;
+    // 落下終了
+    
+    
+    // 行動フロー開始
+    Calibration();
+    xbee.printf("XBee Connected\r\n");
+    xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
+    for (int i = 0; i<=cp_max-1 ; i++) {//最後のcp=goalまで移動
+        next_CP_x = CPs_x[i];
+        next_CP_y = CPs_y[i];
+        
+        xbee.printf("next_i=%d\r\n", i);
+            
+        while (1) {
+                while(FrontGet()) {
+                    xbee.printf("frontget\n\r");
+                    Move('2', 0.1);
+                    wait(0.5);
+                    Move('1', 0);      //停止
+                    Move('4', 0.2);    //時計回り回転
+                    wait(0.5);
+                    /*Move('2', 0.17);  
+                    wait(0.2);*/
+                    Move('1', 0);      //回転停止
+                    xbee.printf("front_avoid_rotate\n\r");
+                }
+                
+                //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも
+                Move('2', 0.2);  
+                wait(0.8);
+                Move('1', 0);  
+                //ちょっと走るのおわり
+                //走りながらanglegetできたら、止まらない
+                
+                catchGPS();
+                direction = AngleGet(next_CP_x,next_CP_y);
+                xbee.printf("\n\n\r----direction start-----\r\n");
+                xbee.printf("next_cp_x=%lf, next_cp_y=%lf\r\n",next_CP_x,next_CP_y);
+                //角度調節
+                while(1) {
+                    if(direction < 5 || direction > 350) {
+                        xbee.printf("\n-----direction finish-----\r\n");
+                        Move('1', 0);   //停止
+                        Move('2', 0.39);
+                        xbee.printf("now_direction=%f\r\n", direction);
+                        break;
+                        }
+                    else {
+                        Move('4', 0.19);//時計回りに回転
+                        xbee.printf("now_direction=%lf\r\n", direction);
+                        direction = AngleGet(next_CP_x,next_CP_y);
+                        }
+                }
+                catchGPS();
+                xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
+                
+                double lati = 111132.8715;    //1度あたりの緯度の距離(m)
+                double longi = 91535.79099;    //1度あたりの経度の距離(m)
+                GPS_x = gps.latitude;
+                GPS_y = gps.longitude;
+                if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x)*lati*lati + (next_CP_y - GPS_y)*(next_CP_y - GPS_y)*longi*longi < 12.25) { // CP到着判定 //試験で調整
+                    xbee.printf("!!!!!!!!now leach cp[%d]=x_%f,y_%f!!!!!!!!!!!\r\n",i,next_CP_x ,next_CP_y);
+                    break;
+                }
+                speak();            
+            }//while(1){}
+        }//for(){}
+    // 行動フロー終了
+    xbee.printf("End\r\n");
+    Move('2', 0.17);  
+    wait(0.2);
+    Move('1', 0);      //停止
+    return 0;
+}