CORE / Mbed 2 deprecated 2017_03_Kaiho

2017年3月，伊豆大島共同打上実験 CORE＿缶ロケチーム電装

Dependencies:   MPU6050 MS5607 mbed SDFileSystem

main.cpp@4:cc266df87f3e, 2017-02-19 (annotated)

Committer:
mikawataru
Date:
Sun Feb 19 10:42:33 2017 +0000
Revision:
4:cc266df87f3e
Parent:
3:9cd74af355cc
Child:
5:bcf55d8fe7a7
2/19????????

Who changed what in which revision?

UserRevisionLine numberNew 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 4:cc266df87f3e 76 break;
mikawataru 3:9cd74af355cc 77 case OPEN_2://缶サット開放モード
mikawataru 3:9cd74af355cc 78 if(ms5607.getAltitude()-alt_launch < 350) cnt_drop++;
mikawataru 3:9cd74af355cc 79 if (cnt_drop==5) _Servo(2,UNLOCK);
mikawataru 3:9cd74af355cc 80 break;
mikawataru 0:10a9c9c5ce83 81 }
mikawataru 0:10a9c9c5ce83 82 }
mikawataru 3:9cd74af355cc 83
mikawataru 3:9cd74af355cc 84 void _Log(){
mikawataru 3:9cd74af355cc 85 t[row][col] = timer.read();
mikawataru 3:9cd74af355cc 86 pressure[row][col] = ms5607.getPressure();
mikawataru 3:9cd74af355cc 87 temperature[row][col] = ms5607.getTemperature();
mikawataru 3:9cd74af355cc 88 mpu.getAccelero(&acc[row][col][0]);
mikawataru 3:9cd74af355cc 89 mpu.getGyro(&gyro[row][col][0]);
mikawataru 3:9cd74af355cc 90 fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
mikawataru 3:9cd74af355cc 91 t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0],
mikawataru 3:9cd74af355cc 92 acc[row][col][1],acc[row][col][2],gyro[row][col][0],gyro[row][col][1],gyro[row][col][2]
mikawataru 3:9cd74af355cc 93 );
mikawataru 3:9cd74af355cc 94 if(col++==RATE_LOG){
mikawataru 3:9cd74af355cc 95 fclose(fp);
mikawataru 3:9cd74af355cc 96 fp = fopen("/sd/log.txt", "a");
mikawataru 3:9cd74af355cc 97 row =! row;
mikawataru 3:9cd74af355cc 98 col = 0;
mikawataru 3:9cd74af355cc 99 }
mikawataru 3:9cd74af355cc 100 }
mikawataru 3:9cd74af355cc 101
mikawataru 3:9cd74af355cc 102 void _Servo(int8_t door_num, bool door_motion){
mikawataru 3:9cd74af355cc 103
mikawataru 3:9cd74af355cc 104 }