Diff: main.cpp

Revision:

0:266e722e382f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Feb 19 05:39:44 2017 +0000
@@ -0,0 +1,170 @@
+#include "mbed.h"
+//#include "MS5607SPI.h"
+#include "MS5607I2C.h"
+#include "MPU6050.h"
+#include "SDFileSystem.h"
+#define ON  1
+#define OFF 0
+#define acc 16384
+#define TBD1 0.95             //TBDは適当に設定
+#define TBD2 10
+#define TBD3 0.98
+#define TBD4 15
+#define TBD5 5
+#define TBD6 20
+#define TBD7 8
+
+//MS5607SPI ms5607(p11, p12, p13, p3);
+MS5607I2C ms5607(p9,p10, false);
+MPU6050 mpu(p9,p10);
+SDFileSystem sd(p5,p6,p7,p8,"sd");
+PwmOut servo1(p21);           //パラシュート開放用servo
+PwmOut servo2(p22);
+PwmOut servo3(p23);           //CanSat開放用servo
+PwmOut servo4(p24);        
+Serial pc(USBTX, USBRX);
+FILE*fp;
+Timer timer1,timer2,timer3;
+Ticker ticker;
+double decision1,decision2,decision3;
+double time1,time2,time3;
+
+float a[3];
+float g[3];
+float Pre=ms5607.getPressure();
+float Tem=ms5607.getTemperature();
+float Alt = ms5607.getAltitude();
+
+void dateWriting()
+{
+    fp=fopen("Fright_date.txt","a");
+
+    mpu.getAccelero(a);
+    mpu.getGyro(g);
+
+    fprintf(fp,"%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",a[0],a[1],a[2],g[0],g[1],g[2],Pre,Tem,Alt);
+
+    fclose(fp);
+}
+
+int main()
+{
+    fp=fopen("Fright_date.txt","a");
+
+    if(fp==NULL) {
+        pc.printf("Open Failed!\n");
+    } else {
+        pc.printf("Open Success!\n");
+    }
+
+    pc.printf("Start!\n");
+
+    fprintf(fp,"加速度x,加速度y,加速度z,ジャイロx,ジャイロy,ジャイロz,気圧[Pa],温度[degC],高度[m]\r\n");
+
+    servo1.period_ms(20);
+    servo2.period_ms(20);                   //パルス周期＝20ms
+    servo3.period_ms(20);
+    servo4.period_ms(20);
+
+    //SG90の動作パルス幅は50µs~2400µs(0度~180度)
+    servo1.pulsewidth(0.0005);
+    servo2.pulsewidth(0.0005);
+    servo3.pulsewidth(0.0005);
+    servo4.pulsewidth(0.0005);
+
+    while(decision1<0.9) {                  //取得加速度の9割がTBD3より大きければループを抜ける
+        mpu.setAcceleroRange(0);            //変数初期化
+        mpu.setGyroRange(0);
+
+        while(a[2]<TBD1) {
+            ticker.attach(&dateWriting, 0.1);
+        }
+
+        /*************************発射判定ループ開始*************************/
+
+        while(time1<TBD2) {
+            int i,counter1=0;
+            float az[i];                                  //取得加速度を格納する配列要素
+            for(i=0; i<=20; i++) {                        //条件式はTBD2により決定
+                timer1.start();
+                ticker.attach(&dateWriting, 0.1);         //タイマー割込み
+                az[i]=a[2];                     
+                if(az[i]>TBD3) {
+                    counter1++;                           //取得加速度がTBD3より大きければプラス1
+                }
+                timer1.stop();
+                time1=timer1.read();
+            }
+            decision1=counter1/(i+1);                     //(TBD3より大きい取得加速度の数)÷(取得加速度の総数)
+        }
+    }
+
+    while(decision2<0.9) {                                //取得高度の9割が下降を検知すればループを抜ける
+        int j=0;
+        float alt1[j];                                    //取得高度を各格納する配列要素
+        
+        while(j>0&&alt1[j]>alt1[j-1]) {
+            ticker.attach(&dateWriting, 0.1);             //タイマー割込み
+            alt1[j]=Alt;      
+            j++; 
+        }
+
+        /*************************頂点判定ループ開始*************************/
+
+        while(time2<TBD4) {
+            int k,counter2=0;
+            float alt2[k];                                //取得高度を格納する配列要素
+            for(k=0; k<=20; k++) {                        //条件式はTBD4により決定
+                timer2.start();
+                ticker.attach(&dateWriting, 0.1);         //タイマー割込み
+                alt2[k]=Alt;
+                if(k>0&&alt2[k]<alt2[k-1]) {
+                    counter2++;                           //下降検知をすればプラス1
+                }
+                timer2.stop();
+                time2=timer2.read();
+            }
+            decision2=counter2/(k+1);                     //(下降検知した回数)÷(取得高度の総数)
+        }
+    }
+
+    wait(0.1);
+    servo1.pulsewidth(0.0015);                            //CanSat開放用servo                  　　
+    servo2.pulsewidth(0.0015);
+    
+    while(decision3<0.6) {                                //取得高度の6割が初期高度+TBD5以下ならばループを抜ける
+        int m=0;
+        float alt3[m];                                    //取得高度を格納する配列要素
+        float datum1=alt3[m]+TBD5;
+        
+        while(alt3[0]>datum1) {
+            ticker.attach(&dateWriting, 0.1);             //タイマー割込み
+            alt3[m]=Alt;
+            m++;
+        }
+
+        /*************************高度判定ループ開始*************************/
+
+        while(time3<TBD6) {
+            int n,counter3=0;
+            float alt4[n],datum2=alt4[n]+TBD7;
+            for(n=0; n<=20; n++) {                        //条件式はTBD6により決定
+                timer3.start();
+                ticker.attach(&dateWriting, 0.1);         //タイマー割込み
+                alt4[n]=Alt;
+                if(alt4[n]<=datum2) {
+                    counter3++;                           //取得高度の6割が初期高度+TBD5以下ならばプラス1
+                }
+                timer3.stop();
+                time3=timer3.read();
+            }
+            decision3=counter3/(n+1);                     //(取得高度のうち初期高度+TBD5以下の数)÷(取得高度の総数)
+        }
+    }
+
+    wait(0.1);
+    servo3.pulsewidth(0.0015);                            //CanSat開放(servo角度を90度に設定)
+    servo4.pulsewidth(0.0015);
+
+    pc.printf("Finish!\n");
+}
\ No newline at end of file