2017年3月,伊豆大島共同打上実験 CORE_缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
Diff: main.cpp
- Revision:
- 14:cbee1bfdfca7
- Parent:
- 13:f73b4beded57
- Child:
- 15:01d3969b89d0
diff -r f73b4beded57 -r cbee1bfdfca7 main.cpp --- a/main.cpp Fri Mar 03 05:14:02 2017 +0000 +++ b/main.cpp Wed Mar 08 15:50:06 2017 +0000 @@ -43,48 +43,45 @@ #include "MS5607I2C.h" #include "MPU6050.h" #include "SDFileSystem.h" -/*判定パラメータ*/ -#define ACC_LAUNCH 4//発射判定加速度[g] -#define DROP_TOP 1.5//頂点判定最低降下量[m] -#define ALT_CAN 150//缶サット開放高度[m] -#define TIME_TOP 15//タイマー開放時間[s] -/*動作レート*/ -#define RATE_LOG 10//ログ用 -#define RATE_OPEN 10//開放用 + +#define ACC_LAUNCH 4 +#define TOP_DROP_AMOUNT 1.5 +#define ALT_UAV_DROP 150 +#define TIME_REACH_TOP 15 + +#define RATE_LOG 10 +#define RATE_OPEN 10 /*サーボ動作*/ -#define LOCK 0.0005 -#define UNLOCK 0.0015 -/*フェーズ定義*/ -#define STANDBY 0 -#define LAUNCH 1 -#define FLIGHT 2 -#define DROP 3 -/*入力モード定義*/ -#define BAN 0 -#define NORMAL 1 -#define SERVO_1 2 -#define SERVO_2 3 -#define MODE 4 -#define DEBUG 5 +#define LOCK 0.0005 +#define UNLOCK 0.0015 +/*モード定義*/ +#define STANDBY 0 +#define TEST 1 +#define FLIGHT 2 +/*開放フェーズ定義*/ +#define SETUP 0 +#define LAUNCH 1 +#define RISE 2 +#define DROP 3 /**/ -#define P0 1013.25f//海面気圧[hPa] -#define ACC 4096.0f//加速度オフセット値 +#define P0 1013.25f//海面気圧[hPa] +#define ACC 4096.0f//加速度オフセット値 /*ピン指定*/ -MS5607I2C ms5607(p9, p10, false); -MPU6050 mpu(p9,p10); -BusOut myled(LED1,LED2,LED3,LED4); -SDFileSystem sd(p11, p12, p13, p14, "sd"); -Serial device(USBTX, USBRX); -PwmOut Door_1_1(p23); -PwmOut Door_1_2(p24); -PwmOut Door_2_1(p25); -PwmOut Door_2_2(p26); +MS5607I2C ms5607(p9, p10, false); +MPU6050 mpu(p9,p10); +BusOut myled(LED1,LED2,LED3,LED4); +SDFileSystem sd(p11, p12, p13, p14, "sd"); +Serial device(USBTX, USBRX); +PwmOut Door_1_1(p23); +PwmOut Door_1_2(p24); +PwmOut Door_2_1(p25); +PwmOut Door_2_2(p26); /*タイマー類*/ -Timer timer; +Timer timer; Ticker loop_log; Ticker loop_open; /*ログカウンタ*/ -bool row = 0; +bool row = 0; int8_t col = 0; /*ログ格納用*/ float altitude[2][RATE_LOG],pressure[2][RATE_LOG],temperature[2][RATE_LOG]; @@ -92,22 +89,23 @@ float t[2][RATE_LOG]; FILE *fp; /*フェーズ変数*/ -int8_t Phase = STANDBY; +int8_t Phase = SETUP; +int8_t Mode = STANDBY; /*判定用*/ -float alt4open[RATE_OPEN],acc4open; -float alt_max,alt_launch; -float t_drop,t_top,t_launch; +float alt_buff[RATE_OPEN]; +float alt_max,alt_launch; +float t_drop,t_top,t_launch; int8_t cnt_judge = 0; -int8_t col_open = 0; +int8_t col_open = 0; /*入力用*/ -int8_t Input_mode = NORMAL; int8_t input_buff[3] = {}; +int8_t input_cnt = 0; /*関数*/ -void _Open(); -void _Servo(int8_t door_num, float motion); -void _Log(); -void _Input(); -float _GetAlt(float press, float temp); +void _Open(); +void _Servo(int8_t door_num, float motion); +void _Log(); +void _Input(); +float _Measure_Alt(float press, float temp); float _Median(float data[], int num); /*---------------------------------------------------------------------*/ @@ -129,16 +127,15 @@ void _Open(){ myled = 1 << (Phase-1); switch (Phase) { - case STANDBY://スタンバイモード(発射判定不可) - // Phase = LAUNCH; + case SETUP://セットアップモード(発射判定不可) break; case LAUNCH://点火モード(発射判定可) - acc4open = (float)mpu.getAcceleroRawZ()/(ACC*0.981); - alt4open[col_open] = _GetAlt(ms5607.getPressure()/100,ms5607.getTemperature()); - if(acc4open>ACC_LAUNCH){ + float acc_buff = (float)mpu.getAcceleroRawZ()/(ACC*0.981); + alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature()); + if(acc_buff>ACC_LAUNCH){ if(cnt_judge++==3){ - Phase = FLIGHT; - alt_launch = _Median(alt4open, RATE_OPEN); + Phase = RISE; + alt_launch = _Median(alt_buff, RATE_OPEN); cnt_judge = 0; } t_launch = timer.read(); @@ -147,10 +144,10 @@ if(timer.read()>t_launch+1.0) cnt_judge = 0; } break; - case FLIGHT://飛翔中(パラシュート開放判定) - alt4open[col_open] = _GetAlt(ms5607.getPressure()/100,ms5607.getTemperature()); - if(alt4open[col_open]>alt_max) alt_max = alt4open[col_open]; - if(alt4open[col_open]<alt_max-DROP_TOP){ + case RISE://上昇中(パラシュート開放判定) + alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature()); + if(alt_buff[col_open]>alt_max) alt_max = alt_buff[col_open]; + if(alt_buff[col_open]<alt_max-TOP_DROP_AMOUNT){ if(cnt_judge++==3){ _Servo(1,UNLOCK); Phase = DROP; @@ -160,14 +157,14 @@ }else{ if(timer.read()>t_top+1.0) cnt_judge = 0; } - if(timer.read()-t_launch>TIME_TOP){ + if(timer.read()-t_launch>TIME_REACH_TOP){ _Servo(1,UNLOCK); Phase = DROP; cnt_judge = 0; } break; case DROP://降下中(缶サット開放判定) - if(alt4open[col_open] < alt_launch+ALT_CAN){ + if(alt_buff[col_open] < alt_launch+ALT_UAV_DROP){ if(cnt_judge++==3){ _Servo(2,UNLOCK); cnt_judge = 0; @@ -182,8 +179,8 @@ } /*記録用関数 RATE_LOG[Hz]で記録を行う*/ void _Log(){ - t[row][col] = timer.read(); - pressure[row][col] = ms5607.getPressure()/100; + t[row][col] = timer.read(); + pressure[row][col] = ms5607.getPressure()/100; temperature[row][col] = ms5607.getTemperature(); mpu.getAccelero(&acc[row][col][0]); mpu.getGyro(&gyro[row][col][0]); @@ -193,9 +190,9 @@ ); if(col++==RATE_LOG){ fclose(fp); - fp = fopen("/sd/log.txt", "a"); + fp = fopen("/sd/log.txt", "a"); row =! row; - col = 0; + col = 0; } } /*サーボ動作用関数 _Servo(int8_t 扉番号,float pwm波長)*/ @@ -217,72 +214,103 @@ } /*入力用*/ +//TODO :テストモード実装 +//FIXME:android端末だと入力崩れが起こる void _Input(){ - char c = device.getc(); - switch (Input_mode) { - case NORMAL: - if(c=='s'){ - Input_mode = SERVO_1; - device.printf("\r\nInput_mode:SERVO\r\n"); - device.printf("\r\n0:All\r\n1:Parachute\r\n2:Cansat\r\n>>"); - }else if(c=='c'){ - Input_mode = MODE; - device.printf("\r\nn:NORMAL\r\nf:FLIGHT\r\nt:TEST\r\n>>"); - }else{ - device.printf("\r\nthis command is not found:%c\r\n",c); - device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>"); + input_buff[input_cnt] = device.getc(); + device.printf("\r\n"); + device.printf("input_cnt:%d\r\n",input_cnt); + device.printf("input_buff:%s\r\n",input_buff); + switch (Mode) { + case STANDBY: + if(input_cnt==0){ + if(input_buff[0]=='S'){ + device.printf("P >> Parachute\r\n"); + device.printf("C >> CanSat\r\n"); + }else if(input_buff[0]=='M'){ + device.printf("S >> STANDBY\r\n"); + device.printf("T >> TEST\r\n"); + device.printf("F >> FLIGHT\r\n"); + }else{ + device.printf("This command is not found >> %c\r\n",input_buff[0]); + device.printf(">>MAINMENU<<\r\n"); + device.printf("S >> Servo Operation\r\n"); + device.printf("M >> Mode Change\r\n"); + device.printf("-->>"); + return; + } + }else if(input_cnt==1){ + if(input_buff[0]=='S'){ + if(input_buff[1]=='P'||input_buff[1]=='C'){ + device.printf("U >> UNLOCK\r\n"); + device.printf("L >> LOCK\r\n"); + }else{ + device.printf("This command is not found >> %c\r\n",input_buff[1]); + device.printf("P >> Parachute\r\n"); + device.printf("C >> CanSat\r\n"); + device.printf("-->>"); + return; + } + }else if(input_buff[0]=='M'){ + if(input_buff[1]=='S'){ + Mode = STANDBY; + device.printf(">>MAINMENU<<\r\n"); + device.printf("S >> Servo Operation\r\n"); + device.printf("M >> Mode Change\r\n"); + device.printf("-->>"); + }else if(input_buff[1]=='T'){ + Mode = TEST; + }else if(input_buff[1]=='F'){ + Mode = FLIGHT; + device.printf("FLIGHT-Mode ON!\r\n"); + device.printf("***alert***\r\n"); + device.printf("You will be able to reset only!\r\n"); + }else{ + device.printf("This command is not found >> %c\r\n",input_buff[1]); + device.printf("S >> STANDBY\r\n"); + device.printf("T >> TEST\r\n"); + device.printf("F >> FLIGHT\r\n"); + device.printf("-->>"); + return; + } + input_cnt = 0; + } + }else if(input_cnt==2){ + if(input_buff[2]=='U'){ + if(input_buff[1]=='P') _Servo(1,UNLOCK); + if(input_buff[1]=='C') _Servo(2,UNLOCK); + }else if(input_buff[2]=='L'){ + if(input_buff[1]=='P') _Servo(1,LOCK); + if(input_buff[1]=='C') _Servo(2,LOCK); + }else{ + device.printf("This command is not found >> %c\r\n",input_buff[2]); + device.printf("U >> UNLOCK\r\n"); + device.printf("L >> LOCK\r\n"); + device.printf("-->>"); + return; + } + input_cnt = 0; + device.printf(">>MAINMENU<<\r\n"); + device.printf("S >> Servo Operation\r\n"); + device.printf("M >> Mode Change\r\n"); + device.printf("-->>"); + return; } - break; - case SERVO_1: - input_buff[0] = (int)(c-'0'); - if(c=='0'||c=='1'||c=='2'){ - Input_mode = SERVO_2; - device.printf("\r\n0:OPEN\r\n1:CLOSE\r\n>>"); - }else{ - device.printf("\r\nthis servo_num is not found:%d\r\n",input_buff[0]); - device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>"); - Input_mode = NORMAL; - } + device.printf("-->>"); + input_cnt++; break; - case SERVO_2: - if(c=='0')_Servo(input_buff[0],UNLOCK); - else if(c=='1')_Servo(input_buff[0],LOCK); - else device.printf("\r\nthis motion_num is not found:%c\r\n",c); - device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>"); - Input_mode = NORMAL; + case TEST: break; - case MODE: - if(c=='n'){ - Input_mode = NORMAL; - device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>"); - }else if(c=='f'){ - Input_mode = BAN; - device.printf("\r\nyou will be able to reset only after this action!"); - fp = fopen("/sd/log.txt", "w"); - fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n"); - Phase = LAUNCH; - loop_log.attach(&_Log,1.0/RATE_LOG); - loop_open.attach(&_Open,1.0/RATE_OPEN); - device.printf("\r\nFLIGHT-Mode ON!\r\n"); - }else if(c=='d'){ - Input_mode = DEBUG; - } - break; - case DEBUG: - - break; - case BAN://reset only - break; - default: + case FLIGHT://reset only break; } } /*その他雑関数*/ -float _GetAlt(float press, float temp){//高度計算 - return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; +float _Measure_Alt(float press/*[hPa]*/, float temp/*[℃]*/){ + return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; } -float _Median(float data[], int num){//中央値計算 +float _Median(float data[], int num){ float median; float *sort = (float *)malloc(sizeof(float)*num); for(int i=0; i<num; i++) sort[i] = data[i];