Diff: main.cpp

Revision:

2:6238109d564d

Parent:

1:b9ea35d93329

Child:

3:217b56515009

--- a/main.cpp	Mon Oct 10 11:21:32 2016 +0000
+++ b/main.cpp	Fri Oct 20 03:49:28 2017 +0000
@@ -1,47 +1,152 @@
-/*
-説明
-Nucleo-F303K8とBMP180を使った気温・気圧・高度計算のサンプルプログラム
-
-ライブラリ
-https://developer.mbed.org/users/spiridion/code/BMP180/
-
-以下ピン配置
-Nucleo  BMP180
-GND-----GND-------0V
-+3V3----VIN
-D4------SDA
-D5------SCL
-
-*/
 #include "mbed.h"
 #include "math.h"
+#include "MPU6050.h"
 #include "BMP180.h"
-#define p0 1013.25f//海面気圧
+
+#define LAUNCH_JUDGE_ACC_g  3.0
+#define PARA_OPEN_TIME_s    5.0
+#define LEAF_UNLOCK_TIME_s  5.0
+#define LEAF_UNLOCK_ALT_m   300.0
+#define p0                  1013.25f
+
+BMP180      bmp(dp5,dp27);
+MPU6050     mpu(dp5,dp27);
+Timer       para_timer;
+Timer       leaf_timer;
+Ticker      readtimer;
+PwmOut      mg995(dp1);
+PwmOut      sg92(dp18);
+DigitalOut  myled(LED1);
+Serial      pc(USBTX,USBRX);
 
-DigitalOut myled(LED1);
-Serial pc(USBTX,USBRX);
-BMP180 bmp(PB_7, PB_6);
-Timer timer;
+/*  自作関数    */
+void    _flight();
+void    _open();
+void    _close();
+void    _leaf_lock();
+void    _leaf_unlock();
+float   _getAlt();
+float   _getAcc();
+float   _median(float data[], int num);
 
-float getAlt(float press, float temp);
+/*  グローバル変数 */
+int   Alt_cnt;
+float a[3],acc;
+float pressure,temperature,altitude;
+float Alt_buff[10],Acc_buff[10];
+float land_alt;
+float t;
+
 
 int main() {
-    float pressure,temperature,altitude;
-    float time;   
+    pc.printf("Hello!");
+    mg995.period_ms(20);
+    sg92.period_ms(20);
+    _close();
+    _leaf_lock();
+    mpu.setAcceleroRange(0);
     bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
-    pc.printf("time, temperature ,pressure, altitude\r\n");
-    timer.start();
+    _flight();
+    while(1);
+}
+
+
+/*フライトモード用関数    */
+void _flight(){
     
-    while(1) {
-        bmp.ReadData(&temperature,&pressure);
-        altitude = getAlt(pressure,temperature);
-        time = timer.read();
-        pc.printf("%f, %f, %f, %f \r\n",time, temperature, pressure, altitude);
-        myled =! myled;
-        wait(1);
+    /*  地上高度取得  */
+    for(int i=0;i<10;i++){
+        Alt_buff[i] = _getAlt();
+    }
+    land_alt = _median(Alt_buff,10);
+    for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
+
+    /*  発射判定    */
+    while(1){
+        for(int i=0;i<10;i++){
+            acc = _getAcc();
+            Acc_buff[i] = acc;
+        }
+        acc = _median(Acc_buff,10);
+        if(acc >= LAUNCH_JUDGE_ACC_g){
+            para_timer.start();
+            for(int i=0;i<10;i++)pc.printf("%f\n\r",Acc_buff[i]);
+            break;
+        }
+    }
+
+    /*  開放判定    */
+    while(1){
+        Alt_cnt = 0;
+        Alt_buff[0] = _getAlt();
+        for(int i=1;i<10;i++){
+            Alt_buff[i] = _getAlt();
+            if(Alt_buff[i] < Alt_buff[i-1]) Alt_cnt++;
+        }
+        if(Alt_cnt > 6 || para_timer.read() >= PARA_OPEN_TIME_s){
+            _open();
+            leaf_timer.start();
+            para_timer.stop();
+            for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
+            break;
+        }
+    }
+
+    /*  リーフィング解除判定  */
+    while(1){
+        for(int i=0;i<10;i++){
+            Alt_buff[i] = _getAlt();
+        }
+        altitude = _median(Alt_buff,10);
+        if(altitude <= LEAF_UNLOCK_ALT_m || leaf_timer.read() >= LEAF_UNLOCK_TIME_s){
+            _leaf_unlock();
+            leaf_timer.stop();
+            for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
+            break;
+        }
     }
 }
 
-float getAlt(float press, float temp){
-    return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
-}
\ No newline at end of file
+
+/*  サーボ用関数  */
+void _close() {mg995.pulsewidth(0.0006);}
+void _open() {mg995.pulsewidth(0.0023);}
+void _leaf_lock() {sg92.pulsewidth(0.0005);}
+void _leaf_unlock() {sg92.pulsewidth(0.0023);}
+
+
+/*  高度取得関数    */
+float _getAlt(){
+    bmp.ReadData(&temperature,&pressure);
+    return (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
+}
+
+
+/*  加速度取得関数 */
+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));
+}
+
+
+/*  中央値計算関数 */
+float _median(float data[], int num){//todo:処理時間計測
+    float *data_cpy, ans;
+    data_cpy = new float[num];
+    memcpy(data_cpy,data,sizeof(float)*num);
+
+    for(int i=0; i<num; i++){
+        for(int j=0; j<num-i-1; j++){
+            if(data_cpy[j]>data_cpy[j+1]){
+                float buff = data_cpy[j+1];
+                data_cpy[j+1] = data_cpy[j];
+                data_cpy[j] = buff;
+            }
+        }
+    }
+    
+    if(num%2!=0) ans = data_cpy[num/2];
+    else         ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
+    delete[] data_cpy;
+    return ans;
+}