Diff: main.cpp

Revision:

7:5f693667d0e9

Parent:

6:15204813ef87

Child:

8:a9e115560ba3

diff -r 15204813ef87 -r 5f693667d0e9 main.cpp
--- a/main.cpp	Wed Feb 22 08:49:59 2017 +0000
+++ b/main.cpp	Thu Feb 23 11:06:28 2017 +0000
@@ -11,6 +11,14 @@
 ・MicroSDスロット
 ・MG995(サーボモータ)×4
 
+使用ライブラリ
+・MPU6050.h
+   https://developer.mbed.org/teams/mbed/code/SDFileSystem/
+・MS5607.h
+   https://developer.mbed.org/users/yamaguch/code/MS5607/
+・SDFileSystem.h
+   https://developer.mbed.org/teams/mbed/code/SDFileSystem/
+
 ピン配置
 LPC1768  MPU6050  MS5607  MicroSD　MG995_1  MG995_2  MG995_3  MG995_4
 VIN--------------------------------------------------------------------4.5~9.0V
@@ -30,12 +38,16 @@
 p25--------------------------------------------------CTRL----------------
 p26-----------------------------------------------------------CTRL-------
 */
-
 #include "mbed.h"
+#include "math.h"
 #include "MS5607I2C.h"
 #include "MPU6050.h"
 #include "SDFileSystem.h"
-
+/*判定パラメータ*/
+#define ACC_LAUNCH -10//発射判定加速度[g]
+#define DROP_TOP 1.5//頂点判定最低降下量[m]
+#define ALT_CAN 1//缶サット開放高度[m]
+#define TIME_TOP 60//タイマー開放時間
 /*動作レート*/
 #define RATE_LOG 10//ログ用
 #define RATE_OPEN 10//開放用
@@ -46,25 +58,24 @@
 #define STANDBY 0
 #define LAUNCH 1
 #define FLIGHT 2
-#define OPEN_1 3
-#define OPEN_2 4
+#define DROP 3
 
+#define P0 1013.25f
+/*ピン指定*/
 MS5607I2C ms5607(p9, p10, false);
 MPU6050 mpu(p9,p10);
 DigitalIn sw(p21);
 BusOut myled(LED1,LED2,LED3,LED4);
+SDFileSystem sd(p11, p12, p13, p14, "sd");
 Serial pc(USBTX, USBRX);
-SDFileSystem sd(p11, p12, p13, p14, "sd");
-
 PwmOut Door_1_1(p23);
 PwmOut Door_1_2(p24);
 PwmOut Door_2_1(p25);
 PwmOut Door_2_2(p26);
-
+/*タイマー類*/
 Timer timer;
 Ticker loop_log;
 Ticker loop_open;
-
 /*ログカウンタ*/
 bool row = 0;
 int8_t col = 0;
@@ -72,17 +83,22 @@
 float pressure[2][RATE_LOG],temperature[2][RATE_LOG];
 float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3];
 float t[2][RATE_LOG];
-float t_top,alt,alt_max,alt_launch;
+FILE *fp;
 /*フェイズ変数*/
 int8_t Phase = STANDBY;
-/*降下判定用*/
+/*判定用*/
+float alt,alt_max,alt_launch;
+float a_buff,p_buff,t_buff;
+float t_drop,t_top,t_launch;
+int8_t cnt_launch = 0;
+int8_t cnt_top = 0;
 int8_t cnt_drop = 0;
-FILE *fp;
-
+/*関数*/
+float getAlt(float press, float temp);
 void _Open();
 void _Servo(int8_t door_num, float motion);
 void _Log();
-
+/*---------------------------------------------------------------------*/
 int main() {
    timer.start();
    Door_1_1.period_ms(20);
@@ -96,46 +112,59 @@
    loop_open.attach(&_Open,1.0/RATE_OPEN);
    while(1);
 }
-
 /*開放用関数　RATE_OPEN[Hz]で判定を行う*/
 void _Open(){
    myled = 1 << (Phase-1);
    switch (Phase) {
       case STANDBY://スタンバイモード（発射判定不可）
-                  Phase = LAUNCH;
-                  break;
-      case LAUNCH://フライトモード（発射判定可）
-                  alt_launch = ms5607.getAltitude();
-                  if(sw==0)Phase = FLIGHT;
+                  if(sw==0)Phase = LAUNCH;
                   break;
-      case FLIGHT://飛翔中（頂点判定可）
-                  alt=ms5607.getAltitude();
-                  if(alt > alt_max) alt_max = alt;
-                  else if(alt_max - alt > 1.5) Phase = OPEN_1;
-                  t_top = timer.read();
-                  break;
-      case OPEN_1://パラシュート開放モード
-                  if(alt_max>1.5) cnt_drop++;
-                  if(cnt_drop==5){
-                     _Servo(1,UNLOCK);
-                     Phase = OPEN_2;
-                     cnt_drop = 0;
+      case LAUNCH://点火モード（発射判定可）
+                  alt_launch = alt;
+                  if((-1)*a_buff>ACC_LAUNCH){
+                     if(cnt_launch++==3) Phase = FLIGHT;
+                     t_launch = timer.read();
+                     alt_max = alt;
+                  }else{
+                     if(timer.read()>t_launch+1.0) cnt_launch = 0;
                   }
                   break;
-      case OPEN_2://缶サット開放モード
-                  if(ms5607.getAltitude()-alt_launch < 350) cnt_drop++;
-                  if (cnt_drop==5) _Servo(2,UNLOCK);
+      case FLIGHT://飛翔中（パラシュート開放判定）
+                  if(alt>alt_max) alt_max = alt;
+                  if(alt<alt_max-DROP_TOP){
+                     if(cnt_top++==3){
+                        _Servo(1,UNLOCK);
+                        Phase = DROP;
+                     }
+                     t_top = timer.read();
+                  }else{
+                     if(timer.read()>t_top+1.0) cnt_top = 0;
+                  }
+                  if(timer.read()-t_launch>TIME_TOP){
+                     _Servo(1,UNLOCK);
+                     Phase = DROP;
+                  }
+                  break;
+      case DROP://降下中（缶サット開放判定)
+                  if(alt < alt_launch+ALT_CAN){
+                     if(cnt_drop++==3)_Servo(2,UNLOCK);
+                     t_drop = timer.read();
+                  }else{
+                     if(timer.read()>t_drop+1.0)cnt_drop = 0;
+                  }
+                  pc.printf("DROP:\r\n");
                   break;
    }
 }
-
 /*記録用関数　RATE_LOG[Hz]で記録を行う*/
 void _Log(){
    t[row][col] = timer.read();
-   pressure[row][col] = ms5607.getPressure();
+   pressure[row][col] = ms5607.getPressure()/100;
    temperature[row][col] = ms5607.getTemperature();
    mpu.getAccelero(&acc[row][col][0]);
    mpu.getGyro(&gyro[row][col][0]);
+   a_buff = acc[row][col][2];
+   alt = getAlt(pressure[row][col],temperature[row][col]);
    fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
                t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0],
                acc[row][col][1],acc[row][col][2],gyro[row][col][0],gyro[row][col][1],gyro[row][col][2]
@@ -147,7 +176,6 @@
       col = 0;
     }
 }
-
 /*サーボ動作用関数 _Servo(int8_t 扉番号,float pwm波長)*/
 void _Servo(int8_t door_num, float motion){
    if(door_num==0){//全扉
@@ -165,3 +193,7 @@
       pc.printf("error\r\n");
    }
 }
+
+float getAlt(float press, float temp){
+    return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
+}