2017年3月,伊豆大島共同打上実験 CORE_缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
Diff: main.cpp
- Revision:
- 3:9cd74af355cc
- Parent:
- 2:b6eb08d059cc
- Child:
- 4:cc266df87f3e
diff -r b6eb08d059cc -r 9cd74af355cc main.cpp --- a/main.cpp Sat Feb 18 17:32:44 2017 +0000 +++ b/main.cpp Sat Feb 18 19:08:20 2017 +0000 @@ -3,57 +3,101 @@ #include "MPU6050.h" #include "SDFileSystem.h" -#define RATE 10 +/*動作レート*/ +#define RATE_LOG 10//ログ用 +#define RATE_OPEN 10//開放用 +/*サーボ動作*/ +#define LOCK true +#define UNLOCK false +/*フェイズ定義*/ +#define STANDBY 0 +#define LAUNCH 1 +#define FLIGHT 2 +#define OPEN_1 3 +#define OPEN_2 4 MS5607I2C ms5607(p9, p10, false); MPU6050 mpu(p9,p10); DigitalIn sw(p21); -DigitalOut myled(LED1); +BusOut myled(LED1,LED2,LED3,LED4); Serial pc(USBTX, USBRX); SDFileSystem sd(p11, p12, p13, p14, "sd"); Timer timer; -Ticker loop_measure; +Ticker loop_log; +Ticker loop_open; -int8_t cnt = 0; -bool mode = 0; -float pressure[2][RATE],temperature[2][RATE]; -float acc[2][RATE][3],gyro[2][RATE][3]; -float t[2][RATE],alt,alt_max,alt_launch; +int8_t col = 0; +int8_t Phase = STANDBY; +bool row = 0; +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; +int8_t cnt_drop = 0; FILE *fp; -void _Launch(){ - alt_launch = alt; - myled = 1; - -} -void _measure(){ - t[mode][cnt] = timer.read(); - pressure[mode][cnt] = ms5607.getPressure(); - temperature[mode][cnt] = ms5607.getTemperature(); - mpu.getAccelero(&acc[mode][cnt][0]); - mpu.getGyro(&gyro[mode][cnt][0]); - alt = ms5607.getAltitude(); - fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", - t[mode][cnt],pressure[mode][cnt],temperature[mode][cnt],acc[mode][cnt][0], - acc[mode][cnt][1],acc[mode][cnt][2],gyro[mode][cnt][0],gyro[mode][cnt][1],gyro[mode][cnt][2] - ); - cnt++; - if(cnt==RATE){ - fclose(fp); - fp = fopen("/sd/log.txt", "a"); - mode =! mode; - cnt = 0; - } -} +void _Open(); +void _Servo(int8_t door_num, bool door_motion); +void _Log(); int main() { timer.start(); fp = fopen("/sd/log.txt", "w"); fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n"); - myled = 0; - loop_measure.attach(&_measure,1.0/RATE); - while(1){ - if(sw==0)_Launch(); + loop_log.attach(&_Log,1.0/RATE_LOG); + loop_open.attach(&_Open,1.0/RATE_OPEN); + while(1); +} + +void _Open(){ + myled = 1 << (Phase-1); + switch (Phase) { + case STANDBY://スタンバイモード(発射判定不可) + Phase = LAUNCH; + break; + case LAUNCH://フライトモード(発射判定可) + alt_launch = ms5607.getAltitude(); + if(sw==0)Phase = FLIGHT; + 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 OPEN_2://缶サット開放モード + if(ms5607.getAltitude()-alt_launch < 350) cnt_drop++; + if (cnt_drop==5) _Servo(2,UNLOCK); + break; } } + +void _Log(){ + t[row][col] = timer.read(); + pressure[row][col] = ms5607.getPressure(); + temperature[row][col] = ms5607.getTemperature(); + mpu.getAccelero(&acc[row][col][0]); + mpu.getGyro(&gyro[row][col][0]); + 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] + ); + if(col++==RATE_LOG){ + fclose(fp); + fp = fopen("/sd/log.txt", "a"); + row =! row; + col = 0; + } +} + +void _Servo(int8_t door_num, bool door_motion){ + +}