2017年3月,伊豆大島共同打上実験 CORE_缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
main.cpp
- Committer:
- mikawataru
- Date:
- 2017-02-20
- Revision:
- 5:bcf55d8fe7a7
- Parent:
- 4:cc266df87f3e
- Child:
- 6:15204813ef87
File content as of revision 5:bcf55d8fe7a7:
/* 2017年3月 伊豆大島共同打上実験 団体名:CORE チーム名:ヌペリオル 該当電装:ロケット搭載用 使用部品 ・LPC1768(マイコン) ・MPU6050(加速度・ジャイロセンサ) ・MS5607(気圧・気温センサ) ・MicroSDスロット ・MG995(サーボモータ)×4 ピン配置 LPC1768 MPU6050 MS5607 MicroSD MG995_1 MG995_2 MG995_3 MG995_4 VIN--------------------------------------------------------------------4.5~9.0V -----------------------------------VIN------VIN------VIN------VIN------5V Vout-----VCC------VIN-----VDD------------------------------------------3.3V -ADO -PS GND------GND------GND-----VSS------GND------GND------GND------GND------0V -CS p9-------SDA------SDA---------------------------------------------------- p10------SCL------SCL---------------------------------------------------- p11-----------------------CLK-------------------------------------------- p12-----------------------DAT0------------------------------------------- p13-----------------------CMD-------------------------------------------- p14-----------------------CD/DAT0---------------------------------------- p23--------------------------------CTRL---------------------------------- p24-----------------------------------------CTRL------------------------- p25--------------------------------------------------CTRL---------------- p26-----------------------------------------------------------CTRL------- */ #include "mbed.h" #include "MS5607I2C.h" #include "MPU6050.h" #include "SDFileSystem.h" /*動作レート*/ #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); BusOut myled(LED1,LED2,LED3,LED4); 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; /*ログ格納用*/ 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 Phase = STANDBY; /*降下判定用*/ int8_t cnt_drop = 0; FILE *fp; void _Open(); void _Servo(int8_t door_num, bool door_motion); void _Log(); int main() { timer.start(); Door_1_1.period_ms(20); Door_1_2.period_ms(20); Door_2_1.period_ms(20); Door_2_2.period_ms(20); fp = fopen("/sd/log.txt", "w"); fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n"); 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; } break; 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){ }