2017年3月,伊豆大島共同打上実験 CORE_缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
main.cpp@10:c1d1ff35a6a1, 2017-02-23 (annotated)
- Committer:
- mikawataru
- Date:
- Thu Feb 23 15:05:50 2017 +0000
- Revision:
- 10:c1d1ff35a6a1
- Parent:
- 8:a9e115560ba3
- Child:
- 11:b61d0fcc2ed3
add serial device
Who changed what in which revision?
User | Revision | Line number | New 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 | 7:5f693667d0e9 | 14 | 使用ライブラリ |
mikawataru | 7:5f693667d0e9 | 15 | ・MPU6050.h |
mikawataru | 7:5f693667d0e9 | 16 | https://developer.mbed.org/teams/mbed/code/SDFileSystem/ |
mikawataru | 7:5f693667d0e9 | 17 | ・MS5607.h |
mikawataru | 7:5f693667d0e9 | 18 | https://developer.mbed.org/users/yamaguch/code/MS5607/ |
mikawataru | 7:5f693667d0e9 | 19 | ・SDFileSystem.h |
mikawataru | 7:5f693667d0e9 | 20 | https://developer.mbed.org/teams/mbed/code/SDFileSystem/ |
mikawataru | 7:5f693667d0e9 | 21 | |
mikawataru | 5:bcf55d8fe7a7 | 22 | ピン配置 |
mikawataru | 5:bcf55d8fe7a7 | 23 | LPC1768 MPU6050 MS5607 MicroSD MG995_1 MG995_2 MG995_3 MG995_4 |
mikawataru | 5:bcf55d8fe7a7 | 24 | VIN--------------------------------------------------------------------4.5~9.0V |
mikawataru | 5:bcf55d8fe7a7 | 25 | -----------------------------------VIN------VIN------VIN------VIN------5V |
mikawataru | 5:bcf55d8fe7a7 | 26 | Vout-----VCC------VIN-----VDD------------------------------------------3.3V |
mikawataru | 6:15204813ef87 | 27 | \-ADO \-PS |
mikawataru | 5:bcf55d8fe7a7 | 28 | GND------GND------GND-----VSS------GND------GND------GND------GND------0V |
mikawataru | 6:15204813ef87 | 29 | \-CS |
mikawataru | 5:bcf55d8fe7a7 | 30 | p9-------SDA------SDA---------------------------------------------------- |
mikawataru | 5:bcf55d8fe7a7 | 31 | p10------SCL------SCL---------------------------------------------------- |
mikawataru | 5:bcf55d8fe7a7 | 32 | p11-----------------------CLK-------------------------------------------- |
mikawataru | 5:bcf55d8fe7a7 | 33 | p12-----------------------DAT0------------------------------------------- |
mikawataru | 5:bcf55d8fe7a7 | 34 | p13-----------------------CMD-------------------------------------------- |
mikawataru | 5:bcf55d8fe7a7 | 35 | p14-----------------------CD/DAT0---------------------------------------- |
mikawataru | 5:bcf55d8fe7a7 | 36 | p23--------------------------------CTRL---------------------------------- |
mikawataru | 5:bcf55d8fe7a7 | 37 | p24-----------------------------------------CTRL------------------------- |
mikawataru | 5:bcf55d8fe7a7 | 38 | p25--------------------------------------------------CTRL---------------- |
mikawataru | 5:bcf55d8fe7a7 | 39 | p26-----------------------------------------------------------CTRL------- |
mikawataru | 5:bcf55d8fe7a7 | 40 | */ |
mikawataru | 0:10a9c9c5ce83 | 41 | #include "mbed.h" |
mikawataru | 7:5f693667d0e9 | 42 | #include "math.h" |
mikawataru | 0:10a9c9c5ce83 | 43 | #include "MS5607I2C.h" |
mikawataru | 0:10a9c9c5ce83 | 44 | #include "MPU6050.h" |
mikawataru | 1:ec75f428c6b3 | 45 | #include "SDFileSystem.h" |
mikawataru | 7:5f693667d0e9 | 46 | /*判定パラメータ*/ |
mikawataru | 8:a9e115560ba3 | 47 | #define ACC_LAUNCH 4//発射判定加速度[g] |
mikawataru | 7:5f693667d0e9 | 48 | #define DROP_TOP 1.5//頂点判定最低降下量[m] |
mikawataru | 8:a9e115560ba3 | 49 | #define ALT_CAN 350//缶サット開放高度[m] |
mikawataru | 8:a9e115560ba3 | 50 | #define TIME_TOP 15//タイマー開放時間[s] |
mikawataru | 3:9cd74af355cc | 51 | /*動作レート*/ |
mikawataru | 3:9cd74af355cc | 52 | #define RATE_LOG 10//ログ用 |
mikawataru | 3:9cd74af355cc | 53 | #define RATE_OPEN 10//開放用 |
mikawataru | 3:9cd74af355cc | 54 | /*サーボ動作*/ |
mikawataru | 6:15204813ef87 | 55 | #define LOCK 0.0005 |
mikawataru | 6:15204813ef87 | 56 | #define UNLOCK 0.0015 |
mikawataru | 3:9cd74af355cc | 57 | /*フェイズ定義*/ |
mikawataru | 3:9cd74af355cc | 58 | #define STANDBY 0 |
mikawataru | 3:9cd74af355cc | 59 | #define LAUNCH 1 |
mikawataru | 3:9cd74af355cc | 60 | #define FLIGHT 2 |
mikawataru | 7:5f693667d0e9 | 61 | #define DROP 3 |
mikawataru | 0:10a9c9c5ce83 | 62 | |
mikawataru | 8:a9e115560ba3 | 63 | #define P0 1013.25f//海面気圧[hPa] |
mikawataru | 7:5f693667d0e9 | 64 | /*ピン指定*/ |
mikawataru | 0:10a9c9c5ce83 | 65 | MS5607I2C ms5607(p9, p10, false); |
mikawataru | 0:10a9c9c5ce83 | 66 | MPU6050 mpu(p9,p10); |
mikawataru | 0:10a9c9c5ce83 | 67 | DigitalIn sw(p21); |
mikawataru | 3:9cd74af355cc | 68 | BusOut myled(LED1,LED2,LED3,LED4); |
mikawataru | 7:5f693667d0e9 | 69 | SDFileSystem sd(p11, p12, p13, p14, "sd"); |
mikawataru | 10:c1d1ff35a6a1 | 70 | Serial device(USBTX, USBRX); |
mikawataru | 5:bcf55d8fe7a7 | 71 | PwmOut Door_1_1(p23); |
mikawataru | 5:bcf55d8fe7a7 | 72 | PwmOut Door_1_2(p24); |
mikawataru | 5:bcf55d8fe7a7 | 73 | PwmOut Door_2_1(p25); |
mikawataru | 5:bcf55d8fe7a7 | 74 | PwmOut Door_2_2(p26); |
mikawataru | 7:5f693667d0e9 | 75 | /*タイマー類*/ |
mikawataru | 1:ec75f428c6b3 | 76 | Timer timer; |
mikawataru | 3:9cd74af355cc | 77 | Ticker loop_log; |
mikawataru | 3:9cd74af355cc | 78 | Ticker loop_open; |
mikawataru | 5:bcf55d8fe7a7 | 79 | /*ログカウンタ*/ |
mikawataru | 5:bcf55d8fe7a7 | 80 | bool row = 0; |
mikawataru | 3:9cd74af355cc | 81 | int8_t col = 0; |
mikawataru | 5:bcf55d8fe7a7 | 82 | /*ログ格納用*/ |
mikawataru | 3:9cd74af355cc | 83 | float pressure[2][RATE_LOG],temperature[2][RATE_LOG]; |
mikawataru | 3:9cd74af355cc | 84 | float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3]; |
mikawataru | 3:9cd74af355cc | 85 | float t[2][RATE_LOG]; |
mikawataru | 7:5f693667d0e9 | 86 | FILE *fp; |
mikawataru | 5:bcf55d8fe7a7 | 87 | /*フェイズ変数*/ |
mikawataru | 5:bcf55d8fe7a7 | 88 | int8_t Phase = STANDBY; |
mikawataru | 7:5f693667d0e9 | 89 | /*判定用*/ |
mikawataru | 8:a9e115560ba3 | 90 | float a_buff; |
mikawataru | 7:5f693667d0e9 | 91 | float alt,alt_max,alt_launch; |
mikawataru | 7:5f693667d0e9 | 92 | float t_drop,t_top,t_launch; |
mikawataru | 7:5f693667d0e9 | 93 | int8_t cnt_launch = 0; |
mikawataru | 7:5f693667d0e9 | 94 | int8_t cnt_top = 0; |
mikawataru | 3:9cd74af355cc | 95 | int8_t cnt_drop = 0; |
mikawataru | 7:5f693667d0e9 | 96 | /*関数*/ |
mikawataru | 7:5f693667d0e9 | 97 | float getAlt(float press, float temp); |
mikawataru | 3:9cd74af355cc | 98 | void _Open(); |
mikawataru | 6:15204813ef87 | 99 | void _Servo(int8_t door_num, float motion); |
mikawataru | 3:9cd74af355cc | 100 | void _Log(); |
mikawataru | 7:5f693667d0e9 | 101 | /*---------------------------------------------------------------------*/ |
mikawataru | 0:10a9c9c5ce83 | 102 | int main() { |
mikawataru | 1:ec75f428c6b3 | 103 | timer.start(); |
mikawataru | 5:bcf55d8fe7a7 | 104 | Door_1_1.period_ms(20); |
mikawataru | 5:bcf55d8fe7a7 | 105 | Door_1_2.period_ms(20); |
mikawataru | 5:bcf55d8fe7a7 | 106 | Door_2_1.period_ms(20); |
mikawataru | 5:bcf55d8fe7a7 | 107 | Door_2_2.period_ms(20); |
mikawataru | 6:15204813ef87 | 108 | _Servo(0,LOCK); |
mikawataru | 2:b6eb08d059cc | 109 | fp = fopen("/sd/log.txt", "w"); |
mikawataru | 1:ec75f428c6b3 | 110 | fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n"); |
mikawataru | 3:9cd74af355cc | 111 | loop_log.attach(&_Log,1.0/RATE_LOG); |
mikawataru | 3:9cd74af355cc | 112 | loop_open.attach(&_Open,1.0/RATE_OPEN); |
mikawataru | 3:9cd74af355cc | 113 | while(1); |
mikawataru | 3:9cd74af355cc | 114 | } |
mikawataru | 6:15204813ef87 | 115 | /*開放用関数 RATE_OPEN[Hz]で判定を行う*/ |
mikawataru | 3:9cd74af355cc | 116 | void _Open(){ |
mikawataru | 3:9cd74af355cc | 117 | myled = 1 << (Phase-1); |
mikawataru | 3:9cd74af355cc | 118 | switch (Phase) { |
mikawataru | 3:9cd74af355cc | 119 | case STANDBY://スタンバイモード(発射判定不可) |
mikawataru | 7:5f693667d0e9 | 120 | if(sw==0)Phase = LAUNCH; |
mikawataru | 3:9cd74af355cc | 121 | break; |
mikawataru | 7:5f693667d0e9 | 122 | case LAUNCH://点火モード(発射判定可) |
mikawataru | 7:5f693667d0e9 | 123 | alt_launch = alt; |
mikawataru | 7:5f693667d0e9 | 124 | if((-1)*a_buff>ACC_LAUNCH){ |
mikawataru | 7:5f693667d0e9 | 125 | if(cnt_launch++==3) Phase = FLIGHT; |
mikawataru | 7:5f693667d0e9 | 126 | t_launch = timer.read(); |
mikawataru | 7:5f693667d0e9 | 127 | alt_max = alt; |
mikawataru | 7:5f693667d0e9 | 128 | }else{ |
mikawataru | 7:5f693667d0e9 | 129 | if(timer.read()>t_launch+1.0) cnt_launch = 0; |
mikawataru | 3:9cd74af355cc | 130 | } |
mikawataru | 4:cc266df87f3e | 131 | break; |
mikawataru | 7:5f693667d0e9 | 132 | case FLIGHT://飛翔中(パラシュート開放判定) |
mikawataru | 7:5f693667d0e9 | 133 | if(alt>alt_max) alt_max = alt; |
mikawataru | 7:5f693667d0e9 | 134 | if(alt<alt_max-DROP_TOP){ |
mikawataru | 7:5f693667d0e9 | 135 | if(cnt_top++==3){ |
mikawataru | 7:5f693667d0e9 | 136 | _Servo(1,UNLOCK); |
mikawataru | 7:5f693667d0e9 | 137 | Phase = DROP; |
mikawataru | 7:5f693667d0e9 | 138 | } |
mikawataru | 7:5f693667d0e9 | 139 | t_top = timer.read(); |
mikawataru | 7:5f693667d0e9 | 140 | }else{ |
mikawataru | 7:5f693667d0e9 | 141 | if(timer.read()>t_top+1.0) cnt_top = 0; |
mikawataru | 7:5f693667d0e9 | 142 | } |
mikawataru | 7:5f693667d0e9 | 143 | if(timer.read()-t_launch>TIME_TOP){ |
mikawataru | 7:5f693667d0e9 | 144 | _Servo(1,UNLOCK); |
mikawataru | 7:5f693667d0e9 | 145 | Phase = DROP; |
mikawataru | 7:5f693667d0e9 | 146 | } |
mikawataru | 7:5f693667d0e9 | 147 | break; |
mikawataru | 7:5f693667d0e9 | 148 | case DROP://降下中(缶サット開放判定) |
mikawataru | 7:5f693667d0e9 | 149 | if(alt < alt_launch+ALT_CAN){ |
mikawataru | 7:5f693667d0e9 | 150 | if(cnt_drop++==3)_Servo(2,UNLOCK); |
mikawataru | 7:5f693667d0e9 | 151 | t_drop = timer.read(); |
mikawataru | 7:5f693667d0e9 | 152 | }else{ |
mikawataru | 7:5f693667d0e9 | 153 | if(timer.read()>t_drop+1.0)cnt_drop = 0; |
mikawataru | 7:5f693667d0e9 | 154 | } |
mikawataru | 3:9cd74af355cc | 155 | break; |
mikawataru | 0:10a9c9c5ce83 | 156 | } |
mikawataru | 0:10a9c9c5ce83 | 157 | } |
mikawataru | 6:15204813ef87 | 158 | /*記録用関数 RATE_LOG[Hz]で記録を行う*/ |
mikawataru | 3:9cd74af355cc | 159 | void _Log(){ |
mikawataru | 3:9cd74af355cc | 160 | t[row][col] = timer.read(); |
mikawataru | 7:5f693667d0e9 | 161 | pressure[row][col] = ms5607.getPressure()/100; |
mikawataru | 3:9cd74af355cc | 162 | temperature[row][col] = ms5607.getTemperature(); |
mikawataru | 3:9cd74af355cc | 163 | mpu.getAccelero(&acc[row][col][0]); |
mikawataru | 3:9cd74af355cc | 164 | mpu.getGyro(&gyro[row][col][0]); |
mikawataru | 7:5f693667d0e9 | 165 | a_buff = acc[row][col][2]; |
mikawataru | 7:5f693667d0e9 | 166 | alt = getAlt(pressure[row][col],temperature[row][col]); |
mikawataru | 3:9cd74af355cc | 167 | fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", |
mikawataru | 3:9cd74af355cc | 168 | t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0], |
mikawataru | 3:9cd74af355cc | 169 | acc[row][col][1],acc[row][col][2],gyro[row][col][0],gyro[row][col][1],gyro[row][col][2] |
mikawataru | 3:9cd74af355cc | 170 | ); |
mikawataru | 3:9cd74af355cc | 171 | if(col++==RATE_LOG){ |
mikawataru | 3:9cd74af355cc | 172 | fclose(fp); |
mikawataru | 3:9cd74af355cc | 173 | fp = fopen("/sd/log.txt", "a"); |
mikawataru | 3:9cd74af355cc | 174 | row =! row; |
mikawataru | 3:9cd74af355cc | 175 | col = 0; |
mikawataru | 3:9cd74af355cc | 176 | } |
mikawataru | 3:9cd74af355cc | 177 | } |
mikawataru | 6:15204813ef87 | 178 | /*サーボ動作用関数 _Servo(int8_t 扉番号,float pwm波長)*/ |
mikawataru | 6:15204813ef87 | 179 | void _Servo(int8_t door_num, float motion){ |
mikawataru | 6:15204813ef87 | 180 | if(door_num==0){//全扉 |
mikawataru | 6:15204813ef87 | 181 | Door_1_1.pulsewidth(motion); |
mikawataru | 6:15204813ef87 | 182 | Door_1_2.pulsewidth(motion); |
mikawataru | 6:15204813ef87 | 183 | Door_2_1.pulsewidth(motion); |
mikawataru | 6:15204813ef87 | 184 | Door_2_2.pulsewidth(motion); |
mikawataru | 6:15204813ef87 | 185 | }else if(door_num==1){//扉1 |
mikawataru | 6:15204813ef87 | 186 | Door_1_1.pulsewidth(motion); |
mikawataru | 6:15204813ef87 | 187 | Door_1_2.pulsewidth(motion); |
mikawataru | 6:15204813ef87 | 188 | }else if(door_num==2){//扉2 |
mikawataru | 6:15204813ef87 | 189 | Door_2_1.pulsewidth(motion); |
mikawataru | 6:15204813ef87 | 190 | Door_2_2.pulsewidth(motion); |
mikawataru | 6:15204813ef87 | 191 | }else{ |
mikawataru | 10:c1d1ff35a6a1 | 192 | device.printf("error\r\n"); |
mikawataru | 6:15204813ef87 | 193 | } |
mikawataru | 3:9cd74af355cc | 194 | } |
mikawataru | 7:5f693667d0e9 | 195 | |
mikawataru | 7:5f693667d0e9 | 196 | float getAlt(float press, float temp){ |
mikawataru | 7:5f693667d0e9 | 197 | return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; |
mikawataru | 7:5f693667d0e9 | 198 | } |