フライトモードのプログラム 発射判定,頂点判定,高度判定の順に行う
Dependencies: MPU6050 MS5607 SDFileSystem mbed
Diff: main.cpp
- Revision:
- 0:266e722e382f
diff -r 000000000000 -r 266e722e382f main.cpp --- /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