main.cpp

00001 #include "mbed.h"
00002 //#include "MS5607SPI.h"
00003 #include "MS5607I2C.h"
00004 #include "MPU6050.h"
00005 #include "SDFileSystem.h"
00006 #define ON  1
00007 #define OFF 0
00008 #define acc 16384
00009 #define TBD1 0.95             //TBDは適当に設定
00010 #define TBD2 10
00011 #define TBD3 0.98
00012 #define TBD4 15
00013 #define TBD5 5
00014 #define TBD6 20
00015 #define TBD7 8
00016 
00017 //MS5607SPI ms5607(p11, p12, p13, p3);
00018 MS5607I2C ms5607(p9,p10, false);
00019 MPU6050 mpu(p9,p10);
00020 SDFileSystem sd(p5,p6,p7,p8,"sd");
00021 PwmOut servo1(p21);           //パラシュート開放用servo
00022 PwmOut servo2(p22);
00023 PwmOut servo3(p23);           //CanSat開放用servo
00024 PwmOut servo4(p24);        
00025 Serial pc(USBTX, USBRX);
00026 FILE*fp;
00027 Timer timer1,timer2,timer3;
00028 Ticker ticker;
00029 double decision1,decision2,decision3;
00030 double time1,time2,time3;
00031 
00032 float a[3];
00033 float g[3];
00034 float Pre=ms5607.getPressure();
00035 float Tem=ms5607.getTemperature();
00036 float Alt = ms5607.getAltitude();
00037 
00038 void dateWriting()
00039 {
00040     fp=fopen("Fright_date.txt","a");
00041 
00042     mpu.getAccelero(a);
00043     mpu.getGyro(g);
00044 
00045     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);
00046 
00047     fclose(fp);
00048 }
00049 
00050 int main()
00051 {
00052     fp=fopen("Fright_date.txt","a");
00053 
00054     if(fp==NULL) {
00055         pc.printf("Open Failed!\n");
00056     } else {
00057         pc.printf("Open Success!\n");
00058     }
00059 
00060     pc.printf("Start!\n");
00061 
00062     fprintf(fp,"加速度x,加速度y,加速度z,ジャイロx,ジャイロy,ジャイロz,気圧[Pa],温度[degC],高度[m]\r\n");
00063 
00064     servo1.period_ms(20);
00065     servo2.period_ms(20);                   //パルス周期＝20ms
00066     servo3.period_ms(20);
00067     servo4.period_ms(20);
00068 
00069     //SG90の動作パルス幅は50µs~2400µs(0度~180度)
00070     servo1.pulsewidth(0.0005);
00071     servo2.pulsewidth(0.0005);
00072     servo3.pulsewidth(0.0005);
00073     servo4.pulsewidth(0.0005);
00074 
00075     while(decision1<0.9) {                  //取得加速度の9割がTBD3より大きければループを抜ける
00076         mpu.setAcceleroRange(0);            //変数初期化
00077         mpu.setGyroRange(0);
00078 
00079         while(a[2]<TBD1) {
00080             ticker.attach(&dateWriting, 0.1);
00081         }
00082 
00083         /*************************発射判定ループ開始*************************/
00084 
00085         while(time1<TBD2) {
00086             int i,counter1=0;
00087             float az[i];                                  //取得加速度を格納する配列要素
00088             for(i=0; i<=20; i++) {                        //条件式はTBD2により決定
00089                 timer1.start();
00090                 ticker.attach(&dateWriting, 0.1);         //タイマー割込み
00091                 az[i]=a[2];                     
00092                 if(az[i]>TBD3) {
00093                     counter1++;                           //取得加速度がTBD3より大きければプラス1
00094                 }
00095                 timer1.stop();
00096                 time1=timer1.read();
00097             }
00098             decision1=counter1/(i+1);                     //(TBD3より大きい取得加速度の数)÷(取得加速度の総数)
00099         }
00100     }
00101 
00102     while(decision2<0.9) {                                //取得高度の9割が下降を検知すればループを抜ける
00103         int j=0;
00104         float alt1[j];                                    //取得高度を各格納する配列要素
00105         
00106         while(j>0&&alt1[j]>alt1[j-1]) {
00107             ticker.attach(&dateWriting, 0.1);             //タイマー割込み
00108             alt1[j]=Alt;      
00109             j++; 
00110         }
00111 
00112         /*************************頂点判定ループ開始*************************/
00113 
00114         while(time2<TBD4) {
00115             int k,counter2=0;
00116             float alt2[k];                                //取得高度を格納する配列要素
00117             for(k=0; k<=20; k++) {                        //条件式はTBD4により決定
00118                 timer2.start();
00119                 ticker.attach(&dateWriting, 0.1);         //タイマー割込み
00120                 alt2[k]=Alt;
00121                 if(k>0&&alt2[k]<alt2[k-1]) {
00122                     counter2++;                           //下降検知をすればプラス1
00123                 }
00124                 timer2.stop();
00125                 time2=timer2.read();
00126             }
00127             decision2=counter2/(k+1);                     //(下降検知した回数)÷(取得高度の総数)
00128         }
00129     }
00130 
00131     wait(0.1);
00132     servo1.pulsewidth(0.0015);                            //CanSat開放用servo                  　　
00133     servo2.pulsewidth(0.0015);
00134     
00135     while(decision3<0.6) {                                //取得高度の6割が初期高度+TBD5以下ならばループを抜ける
00136         int m=0;
00137         float alt3[m];                                    //取得高度を格納する配列要素
00138         float datum1=alt3[m]+TBD5;
00139         
00140         while(alt3[0]>datum1) {
00141             ticker.attach(&dateWriting, 0.1);             //タイマー割込み
00142             alt3[m]=Alt;
00143             m++;
00144         }
00145 
00146         /*************************高度判定ループ開始*************************/
00147 
00148         while(time3<TBD6) {
00149             int n,counter3=0;
00150             float alt4[n],datum2=alt4[n]+TBD7;
00151             for(n=0; n<=20; n++) {                        //条件式はTBD6により決定
00152                 timer3.start();
00153                 ticker.attach(&dateWriting, 0.1);         //タイマー割込み
00154                 alt4[n]=Alt;
00155                 if(alt4[n]<=datum2) {
00156                     counter3++;                           //取得高度の6割が初期高度+TBD5以下ならばプラス1
00157                 }
00158                 timer3.stop();
00159                 time3=timer3.read();
00160             }
00161             decision3=counter3/(n+1);                     //(取得高度のうち初期高度+TBD5以下の数)÷(取得高度の総数)
00162         }
00163     }
00164 
00165     wait(0.1);
00166     servo3.pulsewidth(0.0015);                            //CanSat開放(servo角度を90度に設定)
00167     servo4.pulsewidth(0.0015);
00168 
00169     pc.printf("Finish!\n");
00170 }