2017年3月,伊豆大島共同打上実験 CORE_缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
Diff: main.cpp
- Revision:
- 12:211d2c6d2afc
- Parent:
- 11:b61d0fcc2ed3
- Child:
- 13:f73b4beded57
--- a/main.cpp Thu Mar 02 22:59:12 2017 +0000 +++ b/main.cpp Fri Mar 03 01:26:07 2017 +0000 @@ -46,7 +46,7 @@ /*判定パラメータ*/ #define ACC_LAUNCH 4//発射判定加速度[g] #define DROP_TOP 1.5//頂点判定最低降下量[m] -#define ALT_CAN 350//缶サット開放高度[m] +#define ALT_CAN 150//缶サット開放高度[m] #define TIME_TOP 15//タイマー開放時間[s] /*動作レート*/ #define RATE_LOG 10//ログ用 @@ -54,12 +54,18 @@ /*サーボ動作*/ #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 P0 1013.25f//海面気圧[hPa] #define ACC 4096.0f//加速度オフセット値 /*ピン指定*/ @@ -92,10 +98,14 @@ float t_drop,t_top,t_launch; int8_t cnt_judge = 0; int8_t col_open = 0; +/*入力用*/ +int8_t Input_mode = NORMAL; +int8_t input_buff[3] = {}; /*関数*/ void _Open(); void _Servo(int8_t door_num, float motion); void _Log(); +void _Input(); float _GetAlt(float press, float temp); float _Median(float data[], int num); @@ -111,6 +121,7 @@ _Servo(0,LOCK); fp = fopen("/sd/log.txt", "w"); fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n"); + device.attach(&_Input,Serial::RxIrq); loop_log.attach(&_Log,1.0/RATE_LOG); loop_open.attach(&_Open,1.0/RATE_OPEN); while(1); @@ -120,7 +131,7 @@ myled = 1 << (Phase-1); switch (Phase) { case STANDBY://スタンバイモード(発射判定不可) - Phase = LAUNCH; + // Phase = LAUNCH; break; case LAUNCH://点火モード(発射判定可) acc4open = (float)mpu.getAcceleroRawZ()/(ACC*0.981); @@ -203,14 +214,57 @@ Door_2_1.pulsewidth(motion); Door_2_2.pulsewidth(motion); }else{ - device.printf("error\r\n"); + device.printf("servo error:%d\r\n",door_num); } } +/*入力用*/ +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{ + device.printf("\r\nthis command is not found:%c\r\n",c); + device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\n"); + } + 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\n>>"); + Input_mode = NORMAL; + } + 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("Input_mode:NORMAL\r\ns:SERVO\r\n"); + Input_mode = NORMAL; + break; + case MODE: + break; + case BAN://reset only + break; + default: + break; + } +} + +/*その他雑関数*/ float _GetAlt(float press, 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 *sort = (float *)malloc(sizeof(float)*num); @@ -224,10 +278,8 @@ } } } - if(num%2!=0)median = sort[num/2]; else median = (sort[num/2-1]+sort[num/2])/2.0; - free(sort); return median; }