2017年3月,伊豆大島共同打上実験 CORE_缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
main.cpp@3:9cd74af355cc, 2017-02-18 (annotated)
- Committer:
- mikawataru
- Date:
- Sat Feb 18 19:08:20 2017 +0000
- Revision:
- 3:9cd74af355cc
- Parent:
- 2:b6eb08d059cc
- Child:
- 4:cc266df87f3e
?????????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mikawataru | 0:10a9c9c5ce83 | 1 | #include "mbed.h" |
mikawataru | 0:10a9c9c5ce83 | 2 | #include "MS5607I2C.h" |
mikawataru | 0:10a9c9c5ce83 | 3 | #include "MPU6050.h" |
mikawataru | 1:ec75f428c6b3 | 4 | #include "SDFileSystem.h" |
mikawataru | 0:10a9c9c5ce83 | 5 | |
mikawataru | 3:9cd74af355cc | 6 | /*動作レート*/ |
mikawataru | 3:9cd74af355cc | 7 | #define RATE_LOG 10//ログ用 |
mikawataru | 3:9cd74af355cc | 8 | #define RATE_OPEN 10//開放用 |
mikawataru | 3:9cd74af355cc | 9 | /*サーボ動作*/ |
mikawataru | 3:9cd74af355cc | 10 | #define LOCK true |
mikawataru | 3:9cd74af355cc | 11 | #define UNLOCK false |
mikawataru | 3:9cd74af355cc | 12 | /*フェイズ定義*/ |
mikawataru | 3:9cd74af355cc | 13 | #define STANDBY 0 |
mikawataru | 3:9cd74af355cc | 14 | #define LAUNCH 1 |
mikawataru | 3:9cd74af355cc | 15 | #define FLIGHT 2 |
mikawataru | 3:9cd74af355cc | 16 | #define OPEN_1 3 |
mikawataru | 3:9cd74af355cc | 17 | #define OPEN_2 4 |
mikawataru | 0:10a9c9c5ce83 | 18 | |
mikawataru | 0:10a9c9c5ce83 | 19 | MS5607I2C ms5607(p9, p10, false); |
mikawataru | 0:10a9c9c5ce83 | 20 | MPU6050 mpu(p9,p10); |
mikawataru | 0:10a9c9c5ce83 | 21 | DigitalIn sw(p21); |
mikawataru | 3:9cd74af355cc | 22 | BusOut myled(LED1,LED2,LED3,LED4); |
mikawataru | 0:10a9c9c5ce83 | 23 | Serial pc(USBTX, USBRX); |
mikawataru | 1:ec75f428c6b3 | 24 | SDFileSystem sd(p11, p12, p13, p14, "sd"); |
mikawataru | 0:10a9c9c5ce83 | 25 | |
mikawataru | 1:ec75f428c6b3 | 26 | Timer timer; |
mikawataru | 3:9cd74af355cc | 27 | Ticker loop_log; |
mikawataru | 3:9cd74af355cc | 28 | Ticker loop_open; |
mikawataru | 0:10a9c9c5ce83 | 29 | |
mikawataru | 3:9cd74af355cc | 30 | int8_t col = 0; |
mikawataru | 3:9cd74af355cc | 31 | int8_t Phase = STANDBY; |
mikawataru | 3:9cd74af355cc | 32 | bool row = 0; |
mikawataru | 3:9cd74af355cc | 33 | float pressure[2][RATE_LOG],temperature[2][RATE_LOG]; |
mikawataru | 3:9cd74af355cc | 34 | float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3]; |
mikawataru | 3:9cd74af355cc | 35 | float t[2][RATE_LOG]; |
mikawataru | 3:9cd74af355cc | 36 | float t_top,alt,alt_max,alt_launch; |
mikawataru | 3:9cd74af355cc | 37 | int8_t cnt_drop = 0; |
mikawataru | 1:ec75f428c6b3 | 38 | FILE *fp; |
mikawataru | 1:ec75f428c6b3 | 39 | |
mikawataru | 3:9cd74af355cc | 40 | void _Open(); |
mikawataru | 3:9cd74af355cc | 41 | void _Servo(int8_t door_num, bool door_motion); |
mikawataru | 3:9cd74af355cc | 42 | void _Log(); |
mikawataru | 0:10a9c9c5ce83 | 43 | |
mikawataru | 0:10a9c9c5ce83 | 44 | int main() { |
mikawataru | 1:ec75f428c6b3 | 45 | timer.start(); |
mikawataru | 2:b6eb08d059cc | 46 | fp = fopen("/sd/log.txt", "w"); |
mikawataru | 1:ec75f428c6b3 | 47 | fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n"); |
mikawataru | 3:9cd74af355cc | 48 | loop_log.attach(&_Log,1.0/RATE_LOG); |
mikawataru | 3:9cd74af355cc | 49 | loop_open.attach(&_Open,1.0/RATE_OPEN); |
mikawataru | 3:9cd74af355cc | 50 | while(1); |
mikawataru | 3:9cd74af355cc | 51 | } |
mikawataru | 3:9cd74af355cc | 52 | |
mikawataru | 3:9cd74af355cc | 53 | void _Open(){ |
mikawataru | 3:9cd74af355cc | 54 | myled = 1 << (Phase-1); |
mikawataru | 3:9cd74af355cc | 55 | switch (Phase) { |
mikawataru | 3:9cd74af355cc | 56 | case STANDBY://スタンバイモード(発射判定不可) |
mikawataru | 3:9cd74af355cc | 57 | Phase = LAUNCH; |
mikawataru | 3:9cd74af355cc | 58 | break; |
mikawataru | 3:9cd74af355cc | 59 | case LAUNCH://フライトモード(発射判定可) |
mikawataru | 3:9cd74af355cc | 60 | alt_launch = ms5607.getAltitude(); |
mikawataru | 3:9cd74af355cc | 61 | if(sw==0)Phase = FLIGHT; |
mikawataru | 3:9cd74af355cc | 62 | break; |
mikawataru | 3:9cd74af355cc | 63 | case FLIGHT://飛翔中(頂点判定可) |
mikawataru | 3:9cd74af355cc | 64 | alt=ms5607.getAltitude(); |
mikawataru | 3:9cd74af355cc | 65 | if(alt > alt_max) alt_max = alt; |
mikawataru | 3:9cd74af355cc | 66 | else if(alt_max - alt > 1.5) Phase = OPEN_1; |
mikawataru | 3:9cd74af355cc | 67 | t_top = timer.read(); |
mikawataru | 3:9cd74af355cc | 68 | break; |
mikawataru | 3:9cd74af355cc | 69 | case OPEN_1://パラシュート開放モード |
mikawataru | 3:9cd74af355cc | 70 | if(alt_max>1.5) cnt_drop++; |
mikawataru | 3:9cd74af355cc | 71 | if(cnt_drop==5){ |
mikawataru | 3:9cd74af355cc | 72 | _Servo(1,UNLOCK); |
mikawataru | 3:9cd74af355cc | 73 | Phase = OPEN_2; |
mikawataru | 3:9cd74af355cc | 74 | cnt_drop = 0; |
mikawataru | 3:9cd74af355cc | 75 | } |
mikawataru | 3:9cd74af355cc | 76 | case OPEN_2://缶サット開放モード |
mikawataru | 3:9cd74af355cc | 77 | if(ms5607.getAltitude()-alt_launch < 350) cnt_drop++; |
mikawataru | 3:9cd74af355cc | 78 | if (cnt_drop==5) _Servo(2,UNLOCK); |
mikawataru | 3:9cd74af355cc | 79 | break; |
mikawataru | 0:10a9c9c5ce83 | 80 | } |
mikawataru | 0:10a9c9c5ce83 | 81 | } |
mikawataru | 3:9cd74af355cc | 82 | |
mikawataru | 3:9cd74af355cc | 83 | void _Log(){ |
mikawataru | 3:9cd74af355cc | 84 | t[row][col] = timer.read(); |
mikawataru | 3:9cd74af355cc | 85 | pressure[row][col] = ms5607.getPressure(); |
mikawataru | 3:9cd74af355cc | 86 | temperature[row][col] = ms5607.getTemperature(); |
mikawataru | 3:9cd74af355cc | 87 | mpu.getAccelero(&acc[row][col][0]); |
mikawataru | 3:9cd74af355cc | 88 | mpu.getGyro(&gyro[row][col][0]); |
mikawataru | 3:9cd74af355cc | 89 | fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", |
mikawataru | 3:9cd74af355cc | 90 | t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0], |
mikawataru | 3:9cd74af355cc | 91 | acc[row][col][1],acc[row][col][2],gyro[row][col][0],gyro[row][col][1],gyro[row][col][2] |
mikawataru | 3:9cd74af355cc | 92 | ); |
mikawataru | 3:9cd74af355cc | 93 | if(col++==RATE_LOG){ |
mikawataru | 3:9cd74af355cc | 94 | fclose(fp); |
mikawataru | 3:9cd74af355cc | 95 | fp = fopen("/sd/log.txt", "a"); |
mikawataru | 3:9cd74af355cc | 96 | row =! row; |
mikawataru | 3:9cd74af355cc | 97 | col = 0; |
mikawataru | 3:9cd74af355cc | 98 | } |
mikawataru | 3:9cd74af355cc | 99 | } |
mikawataru | 3:9cd74af355cc | 100 | |
mikawataru | 3:9cd74af355cc | 101 | void _Servo(int8_t door_num, bool door_motion){ |
mikawataru | 3:9cd74af355cc | 102 | |
mikawataru | 3:9cd74af355cc | 103 | } |