Ryota Nakamura
フライトモードのプログラム 発射判定，頂点判定，高度判定の順に行う
Dependencies: MPU6050 MS5607 SDFileSystem mbed
Revision 0:266e722e382f, committed 2017-02-19
- Comitter:
- RyotaNakamura
- Date:
- Sun Feb 19 05:39:44 2017 +0000
- Commit message:
- ?????????????; TBD??????
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Sun Feb 19 05:39:44 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MPU6050/#5c63e20c50f3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MS5607.lib Sun Feb 19 05:39:44 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/yamaguch/code/MS5607/#5760862143d1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Sun Feb 19 05:39:44 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/neilt6/code/SDFileSystem/#e4d2567200db
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Feb 19 05:39:44 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/176b8275d35d \ No newline at end of file