![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2017年3月,伊豆大島共同打上実験 CORE_缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
Diff: main.cpp
- Revision:
- 7:5f693667d0e9
- Parent:
- 6:15204813ef87
- Child:
- 8:a9e115560ba3
--- 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; +}