Diff: main.cpp

Revision:

6:e0c25c3976ae

Parent:

5:a5cf6914e7c4

Child:

7:4efaca1e9acd

--- a/main.cpp	Sun Oct 22 15:07:19 2017 +0000
+++ b/main.cpp	Sun Oct 29 13:38:47 2017 +0000
@@ -5,6 +5,7 @@
 
 #define LAUNCH_JUDGE_ACC_g  3.0
 #define PARA_OPEN_TIME_s    10.0
+#define PARA_OPEN_CNT       7
 #define PARA_OPEN_FILTER_s  6.0
 #define LEAF_UNLOCK_TIME_s  10.0
 #define LEAF_UNLOCK_ALT_m   300.0
@@ -15,8 +16,8 @@
 Timer           para_timer;
 Timer           leaf_timer;
 Ticker          readtimer;
-PwmOut          mg996(dp18);
-PwmOut          sg92(dp1);
+PwmOut          mg996(dp1);
+PwmOut          sg92(dp18);
 DigitalIn       oshirase1(dp11);
 DigitalInOut    oshirase2(dp10);
 DigitalOut      myled(LED1);
@@ -36,7 +37,7 @@
 int     Alt_cnt;
 float   a[3],Acc;
 float   Pressure,Temperature,Altitude;
-float   Alt_buff[10],Acc_buff[10];
+float   Alt_buff[10],Alt_median_buff[10],Acc_buff[10];
 float   Land_Alt;
 bool    Mg996Open = true;
 bool    Sg92Open  = true;
@@ -46,8 +47,6 @@
 //    pc.printf("Hello!");
     mg996.period_ms(20);
     sg92.period_ms(20);
-    _close();
-    _leaf_lock();
     mpu.setAcceleroRange(0);
     bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
     oshirase2.input();
@@ -88,6 +87,8 @@
 void _flight(){
     oshirase2.output();
     oshirase2 = 0;
+    _close();
+    _leaf_lock();
 
     /*  地上高度取得  */
     for(int i=0;i<10;i++){
@@ -115,13 +116,19 @@
     /*  開放判定    */
     while(1){
         Alt_cnt = 0;
-        Alt_buff[0] = _getAlt();
+        for(int i=0;i<10;i++){
+            Alt_buff[i] = _getAlt();
+        }
+        Alt_median_buff[0] = _median(Alt_buff,10);
         for(int i=1;i<10;i++){
-            Alt_buff[i] = _getAlt();
-            if(Alt_buff[i] < Alt_buff[i-1]) Alt_cnt++;
+            for(int j=0;j<10;j++){
+                Alt_buff[j] = _getAlt();
+            }
+            Alt_median_buff[i] = _median(Alt_buff,10);
+            if(Alt_median_buff[i] < Alt_median_buff[i-1]) Alt_cnt++;
         }
         if(para_timer.read() >= PARA_OPEN_FILTER_s){
-            if(Alt_cnt > 6 || para_timer.read() >= PARA_OPEN_TIME_s){
+            if(Alt_cnt >= PARA_OPEN_CNT/* || para_timer.read() >= PARA_OPEN_TIME_s*/){
                 _open();
                 leaf_timer.start();
                 oshirase2 = 0;
@@ -138,8 +145,9 @@
             Alt_buff[i] = _getAlt();
         }
         Altitude = _median(Alt_buff,10);
-        if(Altitude <= LEAF_UNLOCK_ALT_m || leaf_timer.read() >= LEAF_UNLOCK_TIME_s){//TODO:現在の高度から地上高度を引く
+        if(/*Altitude - Land_Alt <= LEAF_UNLOCK_ALT_m ||*/ leaf_timer.read() >= LEAF_UNLOCK_TIME_s){
             _leaf_unlock();
+            oshirase2 = 1;
             leaf_timer.stop();
 //            for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
             break;
@@ -149,10 +157,10 @@
 
 
 /*  サーボ用関数  */
-void _close() {mg996.pulsewidth(0.0006);}
-void _open() {mg996.pulsewidth(0.0023);}
-void _leaf_lock() {sg92.pulsewidth(0.0005);}
-void _leaf_unlock() {sg92.pulsewidth(0.0023);}
+void _close() {mg996.pulsewidth(0.0010);}
+void _open() {mg996.pulsewidth(0.0020);}
+void _leaf_lock() {sg92.pulsewidth(0.0010);}
+void _leaf_unlock() {sg92.pulsewidth(0.0020);}
 
 
 /*  高度取得関数    */
@@ -162,7 +170,7 @@
 }
 
 
-/*  加速度取得関数 */
+/*  合成加速度取得関数 */
 float _getAcc(){
     mpu.getAccelero(a);
     return sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2));