CORE / Mbed 2 deprecated 2017_03_Kaiho

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

Dependencies:   MPU6050 MS5607 mbed SDFileSystem

main.cpp@5:bcf55d8fe7a7, 2017-02-20 (annotated)

Committer:
mikawataru
Date:
Mon Feb 20 12:25:06 2017 +0000
Revision:
5:bcf55d8fe7a7
Parent:
4:cc266df87f3e
Child:
6:15204813ef87
??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikawataru 5:bcf55d8fe7a7 1 /*
mikawataru 5:bcf55d8fe7a7 2 2017年3月　伊豆大島共同打上実験
mikawataru 5:bcf55d8fe7a7 3 団体名：CORE
mikawataru 5:bcf55d8fe7a7 4 チーム名：ヌペリオル
mikawataru 5:bcf55d8fe7a7 5 該当電装：ロケット搭載用
mikawataru 5:bcf55d8fe7a7 6
mikawataru 5:bcf55d8fe7a7 7 使用部品
mikawataru 5:bcf55d8fe7a7 8 ・LPC1768(マイコン)
mikawataru 5:bcf55d8fe7a7 9 ・MPU6050(加速度・ジャイロセンサ)
mikawataru 5:bcf55d8fe7a7 10 ・MS5607(気圧・気温センサ)
mikawataru 5:bcf55d8fe7a7 11 ・MicroSDスロット
mikawataru 5:bcf55d8fe7a7 12 ・MG995(サーボモータ)×4
mikawataru 5:bcf55d8fe7a7 13
mikawataru 5:bcf55d8fe7a7 14 ピン配置
mikawataru 5:bcf55d8fe7a7 15 LPC1768 MPU6050 MS5607 MicroSD　MG995_1 MG995_2 MG995_3 MG995_4
mikawataru 5:bcf55d8fe7a7 16 VIN--------------------------------------------------------------------4.5~9.0V
mikawataru 5:bcf55d8fe7a7 17 -----------------------------------VIN------VIN------VIN------VIN------5V
mikawataru 5:bcf55d8fe7a7 18 Vout-----VCC------VIN-----VDD------------------------------------------3.3V
mikawataru 5:bcf55d8fe7a7 19 -ADO -PS
mikawataru 5:bcf55d8fe7a7 20 GND------GND------GND-----VSS------GND------GND------GND------GND------0V
mikawataru 5:bcf55d8fe7a7 21 -CS
mikawataru 5:bcf55d8fe7a7 22 p9-------SDA------SDA----------------------------------------------------
mikawataru 5:bcf55d8fe7a7 23 p10------SCL------SCL----------------------------------------------------
mikawataru 5:bcf55d8fe7a7 24 p11-----------------------CLK--------------------------------------------
mikawataru 5:bcf55d8fe7a7 25 p12-----------------------DAT0-------------------------------------------
mikawataru 5:bcf55d8fe7a7 26 p13-----------------------CMD--------------------------------------------
mikawataru 5:bcf55d8fe7a7 27 p14-----------------------CD/DAT0----------------------------------------
mikawataru 5:bcf55d8fe7a7 28 p23--------------------------------CTRL----------------------------------
mikawataru 5:bcf55d8fe7a7 29 p24-----------------------------------------CTRL-------------------------
mikawataru 5:bcf55d8fe7a7 30 p25--------------------------------------------------CTRL----------------
mikawataru 5:bcf55d8fe7a7 31 p26-----------------------------------------------------------CTRL-------
mikawataru 5:bcf55d8fe7a7 32 */
mikawataru 5:bcf55d8fe7a7 33
mikawataru 0:10a9c9c5ce83 34 #include "mbed.h"
mikawataru 0:10a9c9c5ce83 35 #include "MS5607I2C.h"
mikawataru 0:10a9c9c5ce83 36 #include "MPU6050.h"
mikawataru 1:ec75f428c6b3 37 #include "SDFileSystem.h"
mikawataru 0:10a9c9c5ce83 38
mikawataru 3:9cd74af355cc 39 /*動作レート*/
mikawataru 3:9cd74af355cc 40 #define RATE_LOG 10//ログ用
mikawataru 3:9cd74af355cc 41 #define RATE_OPEN 10//開放用
mikawataru 3:9cd74af355cc 42 /*サーボ動作*/
mikawataru 3:9cd74af355cc 43 #define LOCK true
mikawataru 3:9cd74af355cc 44 #define UNLOCK false
mikawataru 3:9cd74af355cc 45 /*フェイズ定義*/
mikawataru 3:9cd74af355cc 46 #define STANDBY 0
mikawataru 3:9cd74af355cc 47 #define LAUNCH 1
mikawataru 3:9cd74af355cc 48 #define FLIGHT 2
mikawataru 3:9cd74af355cc 49 #define OPEN_1 3
mikawataru 3:9cd74af355cc 50 #define OPEN_2 4
mikawataru 0:10a9c9c5ce83 51
mikawataru 0:10a9c9c5ce83 52 MS5607I2C ms5607(p9, p10, false);
mikawataru 0:10a9c9c5ce83 53 MPU6050 mpu(p9,p10);
mikawataru 0:10a9c9c5ce83 54 DigitalIn sw(p21);
mikawataru 3:9cd74af355cc 55 BusOut myled(LED1,LED2,LED3,LED4);
mikawataru 0:10a9c9c5ce83 56 Serial pc(USBTX, USBRX);
mikawataru 1:ec75f428c6b3 57 SDFileSystem sd(p11, p12, p13, p14, "sd");
mikawataru 0:10a9c9c5ce83 58
mikawataru 5:bcf55d8fe7a7 59 PwmOut Door_1_1(p23);
mikawataru 5:bcf55d8fe7a7 60 PwmOut Door_1_2(p24);
mikawataru 5:bcf55d8fe7a7 61 PwmOut Door_2_1(p25);
mikawataru 5:bcf55d8fe7a7 62 PwmOut Door_2_2(p26);
mikawataru 5:bcf55d8fe7a7 63
mikawataru 1:ec75f428c6b3 64 Timer timer;
mikawataru 3:9cd74af355cc 65 Ticker loop_log;
mikawataru 3:9cd74af355cc 66 Ticker loop_open;
mikawataru 0:10a9c9c5ce83 67
mikawataru 5:bcf55d8fe7a7 68 /*ログカウンタ*/
mikawataru 5:bcf55d8fe7a7 69 bool row = 0;
mikawataru 3:9cd74af355cc 70 int8_t col = 0;
mikawataru 5:bcf55d8fe7a7 71 /*ログ格納用*/
mikawataru 3:9cd74af355cc 72 float pressure[2][RATE_LOG],temperature[2][RATE_LOG];
mikawataru 3:9cd74af355cc 73 float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3];
mikawataru 3:9cd74af355cc 74 float t[2][RATE_LOG];
mikawataru 3:9cd74af355cc 75 float t_top,alt,alt_max,alt_launch;
mikawataru 5:bcf55d8fe7a7 76 /*フェイズ変数*/
mikawataru 5:bcf55d8fe7a7 77 int8_t Phase = STANDBY;
mikawataru 5:bcf55d8fe7a7 78 /*降下判定用*/
mikawataru 3:9cd74af355cc 79 int8_t cnt_drop = 0;
mikawataru 1:ec75f428c6b3 80 FILE *fp;
mikawataru 1:ec75f428c6b3 81
mikawataru 3:9cd74af355cc 82 void _Open();
mikawataru 3:9cd74af355cc 83 void _Servo(int8_t door_num, bool door_motion);
mikawataru 3:9cd74af355cc 84 void _Log();
mikawataru 0:10a9c9c5ce83 85
mikawataru 0:10a9c9c5ce83 86 int main() {
mikawataru 1:ec75f428c6b3 87 timer.start();
mikawataru 5:bcf55d8fe7a7 88 Door_1_1.period_ms(20);
mikawataru 5:bcf55d8fe7a7 89 Door_1_2.period_ms(20);
mikawataru 5:bcf55d8fe7a7 90 Door_2_1.period_ms(20);
mikawataru 5:bcf55d8fe7a7 91 Door_2_2.period_ms(20);
mikawataru 2:b6eb08d059cc 92 fp = fopen("/sd/log.txt", "w");
mikawataru 1:ec75f428c6b3 93 fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
mikawataru 3:9cd74af355cc 94 loop_log.attach(&_Log,1.0/RATE_LOG);
mikawataru 3:9cd74af355cc 95 loop_open.attach(&_Open,1.0/RATE_OPEN);
mikawataru 3:9cd74af355cc 96 while(1);
mikawataru 3:9cd74af355cc 97 }
mikawataru 3:9cd74af355cc 98
mikawataru 3:9cd74af355cc 99 void _Open(){
mikawataru 3:9cd74af355cc 100 myled = 1 << (Phase-1);
mikawataru 3:9cd74af355cc 101 switch (Phase) {
mikawataru 3:9cd74af355cc 102 case STANDBY://スタンバイモード（発射判定不可）
mikawataru 3:9cd74af355cc 103 Phase = LAUNCH;
mikawataru 3:9cd74af355cc 104 break;
mikawataru 3:9cd74af355cc 105 case LAUNCH://フライトモード（発射判定可）
mikawataru 5:bcf55d8fe7a7 106 alt_launch = ms5607.getAltitude();
mikawataru 3:9cd74af355cc 107 if(sw==0)Phase = FLIGHT;
mikawataru 3:9cd74af355cc 108 break;
mikawataru 3:9cd74af355cc 109 case FLIGHT://飛翔中（頂点判定可）
mikawataru 3:9cd74af355cc 110 alt=ms5607.getAltitude();
mikawataru 3:9cd74af355cc 111 if(alt > alt_max) alt_max = alt;
mikawataru 3:9cd74af355cc 112 else if(alt_max - alt > 1.5) Phase = OPEN_1;
mikawataru 3:9cd74af355cc 113 t_top = timer.read();
mikawataru 3:9cd74af355cc 114 break;
mikawataru 3:9cd74af355cc 115 case OPEN_1://パラシュート開放モード
mikawataru 3:9cd74af355cc 116 if(alt_max>1.5) cnt_drop++;
mikawataru 3:9cd74af355cc 117 if(cnt_drop==5){
mikawataru 3:9cd74af355cc 118 _Servo(1,UNLOCK);
mikawataru 3:9cd74af355cc 119 Phase = OPEN_2;
mikawataru 3:9cd74af355cc 120 cnt_drop = 0;
mikawataru 3:9cd74af355cc 121 }
mikawataru 4:cc266df87f3e 122 break;
mikawataru 3:9cd74af355cc 123 case OPEN_2://缶サット開放モード
mikawataru 3:9cd74af355cc 124 if(ms5607.getAltitude()-alt_launch < 350) cnt_drop++;
mikawataru 3:9cd74af355cc 125 if (cnt_drop==5) _Servo(2,UNLOCK);
mikawataru 3:9cd74af355cc 126 break;
mikawataru 0:10a9c9c5ce83 127 }
mikawataru 0:10a9c9c5ce83 128 }
mikawataru 3:9cd74af355cc 129
mikawataru 3:9cd74af355cc 130 void _Log(){
mikawataru 3:9cd74af355cc 131 t[row][col] = timer.read();
mikawataru 3:9cd74af355cc 132 pressure[row][col] = ms5607.getPressure();
mikawataru 3:9cd74af355cc 133 temperature[row][col] = ms5607.getTemperature();
mikawataru 3:9cd74af355cc 134 mpu.getAccelero(&acc[row][col][0]);
mikawataru 3:9cd74af355cc 135 mpu.getGyro(&gyro[row][col][0]);
mikawataru 3:9cd74af355cc 136 fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
mikawataru 3:9cd74af355cc 137 t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0],
mikawataru 3:9cd74af355cc 138 acc[row][col][1],acc[row][col][2],gyro[row][col][0],gyro[row][col][1],gyro[row][col][2]
mikawataru 3:9cd74af355cc 139 );
mikawataru 3:9cd74af355cc 140 if(col++==RATE_LOG){
mikawataru 3:9cd74af355cc 141 fclose(fp);
mikawataru 3:9cd74af355cc 142 fp = fopen("/sd/log.txt", "a");
mikawataru 3:9cd74af355cc 143 row =! row;
mikawataru 3:9cd74af355cc 144 col = 0;
mikawataru 3:9cd74af355cc 145 }
mikawataru 3:9cd74af355cc 146 }
mikawataru 3:9cd74af355cc 147
mikawataru 3:9cd74af355cc 148 void _Servo(int8_t door_num, bool door_motion){
mikawataru 3:9cd74af355cc 149
mikawataru 3:9cd74af355cc 150 }