Ryota Nakamura / Mbed 2 deprecated Fright_mode

フライトモードのプログラム 発射判定，頂点判定，高度判定の順に行う

Dependencies:   MPU6050 MS5607 SDFileSystem mbed

main.cpp@0:266e722e382f, 2017-02-19 (annotated)

Committer:
RyotaNakamura
Date:
Sun Feb 19 05:39:44 2017 +0000
Revision:
0:266e722e382f
?????????????; TBD??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RyotaNakamura 0:266e722e382f 1 #include "mbed.h"
RyotaNakamura 0:266e722e382f 2 //#include "MS5607SPI.h"
RyotaNakamura 0:266e722e382f 3 #include "MS5607I2C.h"
RyotaNakamura 0:266e722e382f 4 #include "MPU6050.h"
RyotaNakamura 0:266e722e382f 5 #include "SDFileSystem.h"
RyotaNakamura 0:266e722e382f 6 #define ON 1
RyotaNakamura 0:266e722e382f 7 #define OFF 0
RyotaNakamura 0:266e722e382f 8 #define acc 16384
RyotaNakamura 0:266e722e382f 9 #define TBD1 0.95 //TBDは適当に設定
RyotaNakamura 0:266e722e382f 10 #define TBD2 10
RyotaNakamura 0:266e722e382f 11 #define TBD3 0.98
RyotaNakamura 0:266e722e382f 12 #define TBD4 15
RyotaNakamura 0:266e722e382f 13 #define TBD5 5
RyotaNakamura 0:266e722e382f 14 #define TBD6 20
RyotaNakamura 0:266e722e382f 15 #define TBD7 8
RyotaNakamura 0:266e722e382f 16
RyotaNakamura 0:266e722e382f 17 //MS5607SPI ms5607(p11, p12, p13, p3);
RyotaNakamura 0:266e722e382f 18 MS5607I2C ms5607(p9,p10, false);
RyotaNakamura 0:266e722e382f 19 MPU6050 mpu(p9,p10);
RyotaNakamura 0:266e722e382f 20 SDFileSystem sd(p5,p6,p7,p8,"sd");
RyotaNakamura 0:266e722e382f 21 PwmOut servo1(p21); //パラシュート開放用servo
RyotaNakamura 0:266e722e382f 22 PwmOut servo2(p22);
RyotaNakamura 0:266e722e382f 23 PwmOut servo3(p23); //CanSat開放用servo
RyotaNakamura 0:266e722e382f 24 PwmOut servo4(p24);
RyotaNakamura 0:266e722e382f 25 Serial pc(USBTX, USBRX);
RyotaNakamura 0:266e722e382f 26 FILE*fp;
RyotaNakamura 0:266e722e382f 27 Timer timer1,timer2,timer3;
RyotaNakamura 0:266e722e382f 28 Ticker ticker;
RyotaNakamura 0:266e722e382f 29 double decision1,decision2,decision3;
RyotaNakamura 0:266e722e382f 30 double time1,time2,time3;
RyotaNakamura 0:266e722e382f 31
RyotaNakamura 0:266e722e382f 32 float a[3];
RyotaNakamura 0:266e722e382f 33 float g[3];
RyotaNakamura 0:266e722e382f 34 float Pre=ms5607.getPressure();
RyotaNakamura 0:266e722e382f 35 float Tem=ms5607.getTemperature();
RyotaNakamura 0:266e722e382f 36 float Alt = ms5607.getAltitude();
RyotaNakamura 0:266e722e382f 37
RyotaNakamura 0:266e722e382f 38 void dateWriting()
RyotaNakamura 0:266e722e382f 39 {
RyotaNakamura 0:266e722e382f 40 fp=fopen("Fright_date.txt","a");
RyotaNakamura 0:266e722e382f 41
RyotaNakamura 0:266e722e382f 42 mpu.getAccelero(a);
RyotaNakamura 0:266e722e382f 43 mpu.getGyro(g);
RyotaNakamura 0:266e722e382f 44
RyotaNakamura 0:266e722e382f 45 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);
RyotaNakamura 0:266e722e382f 46
RyotaNakamura 0:266e722e382f 47 fclose(fp);
RyotaNakamura 0:266e722e382f 48 }
RyotaNakamura 0:266e722e382f 49
RyotaNakamura 0:266e722e382f 50 int main()
RyotaNakamura 0:266e722e382f 51 {
RyotaNakamura 0:266e722e382f 52 fp=fopen("Fright_date.txt","a");
RyotaNakamura 0:266e722e382f 53
RyotaNakamura 0:266e722e382f 54 if(fp==NULL) {
RyotaNakamura 0:266e722e382f 55 pc.printf("Open Failed!\n");
RyotaNakamura 0:266e722e382f 56 } else {
RyotaNakamura 0:266e722e382f 57 pc.printf("Open Success!\n");
RyotaNakamura 0:266e722e382f 58 }
RyotaNakamura 0:266e722e382f 59
RyotaNakamura 0:266e722e382f 60 pc.printf("Start!\n");
RyotaNakamura 0:266e722e382f 61
RyotaNakamura 0:266e722e382f 62 fprintf(fp,"加速度x,加速度y,加速度z,ジャイロx,ジャイロy,ジャイロz,気圧[Pa],温度[degC],高度[m]\r\n");
RyotaNakamura 0:266e722e382f 63
RyotaNakamura 0:266e722e382f 64 servo1.period_ms(20);
RyotaNakamura 0:266e722e382f 65 servo2.period_ms(20); //パルス周期＝20ms
RyotaNakamura 0:266e722e382f 66 servo3.period_ms(20);
RyotaNakamura 0:266e722e382f 67 servo4.period_ms(20);
RyotaNakamura 0:266e722e382f 68
RyotaNakamura 0:266e722e382f 69 //SG90の動作パルス幅は50µs~2400µs(0度~180度)
RyotaNakamura 0:266e722e382f 70 servo1.pulsewidth(0.0005);
RyotaNakamura 0:266e722e382f 71 servo2.pulsewidth(0.0005);
RyotaNakamura 0:266e722e382f 72 servo3.pulsewidth(0.0005);
RyotaNakamura 0:266e722e382f 73 servo4.pulsewidth(0.0005);
RyotaNakamura 0:266e722e382f 74
RyotaNakamura 0:266e722e382f 75 while(decision1<0.9) { //取得加速度の9割がTBD3より大きければループを抜ける
RyotaNakamura 0:266e722e382f 76 mpu.setAcceleroRange(0); //変数初期化
RyotaNakamura 0:266e722e382f 77 mpu.setGyroRange(0);
RyotaNakamura 0:266e722e382f 78
RyotaNakamura 0:266e722e382f 79 while(a[2]<TBD1) {
RyotaNakamura 0:266e722e382f 80 ticker.attach(&dateWriting, 0.1);
RyotaNakamura 0:266e722e382f 81 }
RyotaNakamura 0:266e722e382f 82
RyotaNakamura 0:266e722e382f 83 /*************************発射判定ループ開始*************************/
RyotaNakamura 0:266e722e382f 84
RyotaNakamura 0:266e722e382f 85 while(time1<TBD2) {
RyotaNakamura 0:266e722e382f 86 int i,counter1=0;
RyotaNakamura 0:266e722e382f 87 float az[i]; //取得加速度を格納する配列要素
RyotaNakamura 0:266e722e382f 88 for(i=0; i<=20; i++) { //条件式はTBD2により決定
RyotaNakamura 0:266e722e382f 89 timer1.start();
RyotaNakamura 0:266e722e382f 90 ticker.attach(&dateWriting, 0.1); //タイマー割込み
RyotaNakamura 0:266e722e382f 91 az[i]=a[2];
RyotaNakamura 0:266e722e382f 92 if(az[i]>TBD3) {
RyotaNakamura 0:266e722e382f 93 counter1++; //取得加速度がTBD3より大きければプラス1
RyotaNakamura 0:266e722e382f 94 }
RyotaNakamura 0:266e722e382f 95 timer1.stop();
RyotaNakamura 0:266e722e382f 96 time1=timer1.read();
RyotaNakamura 0:266e722e382f 97 }
RyotaNakamura 0:266e722e382f 98 decision1=counter1/(i+1); //(TBD3より大きい取得加速度の数)÷(取得加速度の総数)
RyotaNakamura 0:266e722e382f 99 }
RyotaNakamura 0:266e722e382f 100 }
RyotaNakamura 0:266e722e382f 101
RyotaNakamura 0:266e722e382f 102 while(decision2<0.9) { //取得高度の9割が下降を検知すればループを抜ける
RyotaNakamura 0:266e722e382f 103 int j=0;
RyotaNakamura 0:266e722e382f 104 float alt1[j]; //取得高度を各格納する配列要素
RyotaNakamura 0:266e722e382f 105
RyotaNakamura 0:266e722e382f 106 while(j>0&&alt1[j]>alt1[j-1]) {
RyotaNakamura 0:266e722e382f 107 ticker.attach(&dateWriting, 0.1); //タイマー割込み
RyotaNakamura 0:266e722e382f 108 alt1[j]=Alt;
RyotaNakamura 0:266e722e382f 109 j++;
RyotaNakamura 0:266e722e382f 110 }
RyotaNakamura 0:266e722e382f 111
RyotaNakamura 0:266e722e382f 112 /*************************頂点判定ループ開始*************************/
RyotaNakamura 0:266e722e382f 113
RyotaNakamura 0:266e722e382f 114 while(time2<TBD4) {
RyotaNakamura 0:266e722e382f 115 int k,counter2=0;
RyotaNakamura 0:266e722e382f 116 float alt2[k]; //取得高度を格納する配列要素
RyotaNakamura 0:266e722e382f 117 for(k=0; k<=20; k++) { //条件式はTBD4により決定
RyotaNakamura 0:266e722e382f 118 timer2.start();
RyotaNakamura 0:266e722e382f 119 ticker.attach(&dateWriting, 0.1); //タイマー割込み
RyotaNakamura 0:266e722e382f 120 alt2[k]=Alt;
RyotaNakamura 0:266e722e382f 121 if(k>0&&alt2[k]<alt2[k-1]) {
RyotaNakamura 0:266e722e382f 122 counter2++; //下降検知をすればプラス1
RyotaNakamura 0:266e722e382f 123 }
RyotaNakamura 0:266e722e382f 124 timer2.stop();
RyotaNakamura 0:266e722e382f 125 time2=timer2.read();
RyotaNakamura 0:266e722e382f 126 }
RyotaNakamura 0:266e722e382f 127 decision2=counter2/(k+1); //(下降検知した回数)÷(取得高度の総数)
RyotaNakamura 0:266e722e382f 128 }
RyotaNakamura 0:266e722e382f 129 }
RyotaNakamura 0:266e722e382f 130
RyotaNakamura 0:266e722e382f 131 wait(0.1);
RyotaNakamura 0:266e722e382f 132 servo1.pulsewidth(0.0015); //CanSat開放用servo 　　
RyotaNakamura 0:266e722e382f 133 servo2.pulsewidth(0.0015);
RyotaNakamura 0:266e722e382f 134
RyotaNakamura 0:266e722e382f 135 while(decision3<0.6) { //取得高度の6割が初期高度+TBD5以下ならばループを抜ける
RyotaNakamura 0:266e722e382f 136 int m=0;
RyotaNakamura 0:266e722e382f 137 float alt3[m]; //取得高度を格納する配列要素
RyotaNakamura 0:266e722e382f 138 float datum1=alt3[m]+TBD5;
RyotaNakamura 0:266e722e382f 139
RyotaNakamura 0:266e722e382f 140 while(alt3[0]>datum1) {
RyotaNakamura 0:266e722e382f 141 ticker.attach(&dateWriting, 0.1); //タイマー割込み
RyotaNakamura 0:266e722e382f 142 alt3[m]=Alt;
RyotaNakamura 0:266e722e382f 143 m++;
RyotaNakamura 0:266e722e382f 144 }
RyotaNakamura 0:266e722e382f 145
RyotaNakamura 0:266e722e382f 146 /*************************高度判定ループ開始*************************/
RyotaNakamura 0:266e722e382f 147
RyotaNakamura 0:266e722e382f 148 while(time3<TBD6) {
RyotaNakamura 0:266e722e382f 149 int n,counter3=0;
RyotaNakamura 0:266e722e382f 150 float alt4[n],datum2=alt4[n]+TBD7;
RyotaNakamura 0:266e722e382f 151 for(n=0; n<=20; n++) { //条件式はTBD6により決定
RyotaNakamura 0:266e722e382f 152 timer3.start();
RyotaNakamura 0:266e722e382f 153 ticker.attach(&dateWriting, 0.1); //タイマー割込み
RyotaNakamura 0:266e722e382f 154 alt4[n]=Alt;
RyotaNakamura 0:266e722e382f 155 if(alt4[n]<=datum2) {
RyotaNakamura 0:266e722e382f 156 counter3++; //取得高度の6割が初期高度+TBD5以下ならばプラス1
RyotaNakamura 0:266e722e382f 157 }
RyotaNakamura 0:266e722e382f 158 timer3.stop();
RyotaNakamura 0:266e722e382f 159 time3=timer3.read();
RyotaNakamura 0:266e722e382f 160 }
RyotaNakamura 0:266e722e382f 161 decision3=counter3/(n+1); //(取得高度のうち初期高度+TBD5以下の数)÷(取得高度の総数)
RyotaNakamura 0:266e722e382f 162 }
RyotaNakamura 0:266e722e382f 163 }
RyotaNakamura 0:266e722e382f 164
RyotaNakamura 0:266e722e382f 165 wait(0.1);
RyotaNakamura 0:266e722e382f 166 servo3.pulsewidth(0.0015); //CanSat開放(servo角度を90度に設定)
RyotaNakamura 0:266e722e382f 167 servo4.pulsewidth(0.0015);
RyotaNakamura 0:266e722e382f 168
RyotaNakamura 0:266e722e382f 169 pc.printf("Finish!\n");
RyotaNakamura 0:266e722e382f 170 }