![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2017年3月,伊豆大島共同打上実験 CORE_缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
main.cpp@16:0a239047eb29, 2017-03-13 (annotated)
- Committer:
- mikawataru
- Date:
- Mon Mar 13 12:41:09 2017 +0000
- Revision:
- 16:0a239047eb29
- Parent:
- 15:01d3969b89d0
- Child:
- 17:5f7808444d96
fixed servo motion
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 | 14:cbee1bfdfca7 | 46 | |
mikawataru | 15:01d3969b89d0 | 47 | #define ACC_LAUNCH 0//FIXME:本番は4g |
mikawataru | 14:cbee1bfdfca7 | 48 | #define TOP_DROP_AMOUNT 1.5 |
mikawataru | 16:0a239047eb29 | 49 | #define ALT_UAV_DROP 1.0//FIXME:本番は150m |
mikawataru | 14:cbee1bfdfca7 | 50 | #define TIME_REACH_TOP 15 |
mikawataru | 14:cbee1bfdfca7 | 51 | |
mikawataru | 14:cbee1bfdfca7 | 52 | #define RATE_LOG 10 |
mikawataru | 14:cbee1bfdfca7 | 53 | #define RATE_OPEN 10 |
mikawataru | 3:9cd74af355cc | 54 | /*サーボ動作*/ |
mikawataru | 16:0a239047eb29 | 55 | #define LOCK 0 |
mikawataru | 16:0a239047eb29 | 56 | #define UNLOCK 1 |
mikawataru | 14:cbee1bfdfca7 | 57 | /*モード定義*/ |
mikawataru | 14:cbee1bfdfca7 | 58 | #define STANDBY 0 |
mikawataru | 14:cbee1bfdfca7 | 59 | #define TEST 1 |
mikawataru | 14:cbee1bfdfca7 | 60 | #define FLIGHT 2 |
mikawataru | 14:cbee1bfdfca7 | 61 | /*開放フェーズ定義*/ |
mikawataru | 14:cbee1bfdfca7 | 62 | #define SETUP 0 |
mikawataru | 14:cbee1bfdfca7 | 63 | #define LAUNCH 1 |
mikawataru | 14:cbee1bfdfca7 | 64 | #define RISE 2 |
mikawataru | 14:cbee1bfdfca7 | 65 | #define DROP 3 |
mikawataru | 12:211d2c6d2afc | 66 | /**/ |
mikawataru | 14:cbee1bfdfca7 | 67 | #define P0 1013.25f//海面気圧[hPa] |
mikawataru | 14:cbee1bfdfca7 | 68 | #define ACC 4096.0f//加速度オフセット値 |
mikawataru | 7:5f693667d0e9 | 69 | /*ピン指定*/ |
mikawataru | 14:cbee1bfdfca7 | 70 | MS5607I2C ms5607(p9, p10, false); |
mikawataru | 14:cbee1bfdfca7 | 71 | MPU6050 mpu(p9,p10); |
mikawataru | 14:cbee1bfdfca7 | 72 | BusOut myled(LED1,LED2,LED3,LED4); |
mikawataru | 14:cbee1bfdfca7 | 73 | SDFileSystem sd(p11, p12, p13, p14, "sd"); |
mikawataru | 14:cbee1bfdfca7 | 74 | Serial device(USBTX, USBRX); |
mikawataru | 14:cbee1bfdfca7 | 75 | PwmOut Door_1_1(p23); |
mikawataru | 14:cbee1bfdfca7 | 76 | PwmOut Door_1_2(p24); |
mikawataru | 14:cbee1bfdfca7 | 77 | PwmOut Door_2_1(p25); |
mikawataru | 14:cbee1bfdfca7 | 78 | PwmOut Door_2_2(p26); |
mikawataru | 7:5f693667d0e9 | 79 | /*タイマー類*/ |
mikawataru | 14:cbee1bfdfca7 | 80 | Timer timer; |
mikawataru | 3:9cd74af355cc | 81 | Ticker loop_log; |
mikawataru | 3:9cd74af355cc | 82 | Ticker loop_open; |
mikawataru | 5:bcf55d8fe7a7 | 83 | /*ログカウンタ*/ |
mikawataru | 14:cbee1bfdfca7 | 84 | bool row = 0; |
mikawataru | 3:9cd74af355cc | 85 | int8_t col = 0; |
mikawataru | 5:bcf55d8fe7a7 | 86 | /*ログ格納用*/ |
mikawataru | 11:b61d0fcc2ed3 | 87 | float altitude[2][RATE_LOG],pressure[2][RATE_LOG],temperature[2][RATE_LOG]; |
mikawataru | 3:9cd74af355cc | 88 | float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3]; |
mikawataru | 3:9cd74af355cc | 89 | float t[2][RATE_LOG]; |
mikawataru | 7:5f693667d0e9 | 90 | FILE *fp; |
mikawataru | 11:b61d0fcc2ed3 | 91 | /*フェーズ変数*/ |
mikawataru | 14:cbee1bfdfca7 | 92 | int8_t Phase = SETUP; |
mikawataru | 14:cbee1bfdfca7 | 93 | int8_t Mode = STANDBY; |
mikawataru | 7:5f693667d0e9 | 94 | /*判定用*/ |
mikawataru | 14:cbee1bfdfca7 | 95 | float alt_buff[RATE_OPEN]; |
mikawataru | 14:cbee1bfdfca7 | 96 | float alt_max,alt_launch; |
mikawataru | 14:cbee1bfdfca7 | 97 | float t_drop,t_top,t_launch; |
mikawataru | 11:b61d0fcc2ed3 | 98 | int8_t cnt_judge = 0; |
mikawataru | 14:cbee1bfdfca7 | 99 | int8_t col_open = 0; |
mikawataru | 12:211d2c6d2afc | 100 | /*入力用*/ |
mikawataru | 12:211d2c6d2afc | 101 | int8_t input_buff[3] = {}; |
mikawataru | 14:cbee1bfdfca7 | 102 | int8_t input_cnt = 0; |
mikawataru | 7:5f693667d0e9 | 103 | /*関数*/ |
mikawataru | 14:cbee1bfdfca7 | 104 | void _Open(); |
mikawataru | 16:0a239047eb29 | 105 | void _Servo(int8_t door_num, int8_t motion); |
mikawataru | 14:cbee1bfdfca7 | 106 | void _Log(); |
mikawataru | 14:cbee1bfdfca7 | 107 | void _Input(); |
mikawataru | 14:cbee1bfdfca7 | 108 | float _Measure_Alt(float press, float temp); |
mikawataru | 11:b61d0fcc2ed3 | 109 | float _Median(float data[], int num); |
mikawataru | 11:b61d0fcc2ed3 | 110 | |
mikawataru | 7:5f693667d0e9 | 111 | /*---------------------------------------------------------------------*/ |
mikawataru | 0:10a9c9c5ce83 | 112 | int main() { |
mikawataru | 11:b61d0fcc2ed3 | 113 | mpu.setAcceleroRange(2); |
mikawataru | 11:b61d0fcc2ed3 | 114 | mpu.setGyroRange(2); |
mikawataru | 1:ec75f428c6b3 | 115 | timer.start(); |
mikawataru | 5:bcf55d8fe7a7 | 116 | Door_1_1.period_ms(20); |
mikawataru | 5:bcf55d8fe7a7 | 117 | Door_1_2.period_ms(20); |
mikawataru | 5:bcf55d8fe7a7 | 118 | Door_2_1.period_ms(20); |
mikawataru | 5:bcf55d8fe7a7 | 119 | Door_2_2.period_ms(20); |
mikawataru | 16:0a239047eb29 | 120 | // _Servo(1,UNLOCK);//todo:当日は状態記憶に仕様変更予定? |
mikawataru | 16:0a239047eb29 | 121 | // _Servo(2,UNLOCK);// |
mikawataru | 2:b6eb08d059cc | 122 | fp = fopen("/sd/log.txt", "w"); |
mikawataru | 1:ec75f428c6b3 | 123 | fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n"); |
mikawataru | 12:211d2c6d2afc | 124 | device.attach(&_Input,Serial::RxIrq); |
mikawataru | 15:01d3969b89d0 | 125 | loop_open.attach(&_Open,1.0/RATE_OPEN); |
mikawataru | 3:9cd74af355cc | 126 | while(1); |
mikawataru | 3:9cd74af355cc | 127 | } |
mikawataru | 6:15204813ef87 | 128 | /*開放用関数 RATE_OPEN[Hz]で判定を行う*/ |
mikawataru | 3:9cd74af355cc | 129 | void _Open(){ |
mikawataru | 3:9cd74af355cc | 130 | myled = 1 << (Phase-1); |
mikawataru | 16:0a239047eb29 | 131 | device.printf("%f,%f,%f\r\n",alt_buff[col_open],alt_launch,alt_max); |
mikawataru | 3:9cd74af355cc | 132 | switch (Phase) { |
mikawataru | 14:cbee1bfdfca7 | 133 | case SETUP://セットアップモード(発射判定不可) |
mikawataru | 3:9cd74af355cc | 134 | break; |
mikawataru | 7:5f693667d0e9 | 135 | case LAUNCH://点火モード(発射判定可) |
mikawataru | 14:cbee1bfdfca7 | 136 | float acc_buff = (float)mpu.getAcceleroRawZ()/(ACC*0.981); |
mikawataru | 14:cbee1bfdfca7 | 137 | alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature()); |
mikawataru | 16:0a239047eb29 | 138 | if(acc_buff>(float)ACC_LAUNCH){ |
mikawataru | 11:b61d0fcc2ed3 | 139 | if(cnt_judge++==3){ |
mikawataru | 14:cbee1bfdfca7 | 140 | Phase = RISE; |
mikawataru | 14:cbee1bfdfca7 | 141 | alt_launch = _Median(alt_buff, RATE_OPEN); |
mikawataru | 11:b61d0fcc2ed3 | 142 | cnt_judge = 0; |
mikawataru | 11:b61d0fcc2ed3 | 143 | } |
mikawataru | 7:5f693667d0e9 | 144 | t_launch = timer.read(); |
mikawataru | 11:b61d0fcc2ed3 | 145 | alt_max = alt_launch; |
mikawataru | 7:5f693667d0e9 | 146 | }else{ |
mikawataru | 11:b61d0fcc2ed3 | 147 | if(timer.read()>t_launch+1.0) cnt_judge = 0; |
mikawataru | 3:9cd74af355cc | 148 | } |
mikawataru | 4:cc266df87f3e | 149 | break; |
mikawataru | 14:cbee1bfdfca7 | 150 | case RISE://上昇中(パラシュート開放判定) |
mikawataru | 14:cbee1bfdfca7 | 151 | alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature()); |
mikawataru | 14:cbee1bfdfca7 | 152 | if(alt_buff[col_open]>alt_max) alt_max = alt_buff[col_open]; |
mikawataru | 16:0a239047eb29 | 153 | if(alt_buff[col_open]<alt_max-(float)TOP_DROP_AMOUNT){ |
mikawataru | 11:b61d0fcc2ed3 | 154 | if(cnt_judge++==3){ |
mikawataru | 16:0a239047eb29 | 155 | _Servo(2,UNLOCK); |
mikawataru | 7:5f693667d0e9 | 156 | Phase = DROP; |
mikawataru | 11:b61d0fcc2ed3 | 157 | cnt_judge = 0; |
mikawataru | 7:5f693667d0e9 | 158 | } |
mikawataru | 7:5f693667d0e9 | 159 | t_top = timer.read(); |
mikawataru | 7:5f693667d0e9 | 160 | }else{ |
mikawataru | 11:b61d0fcc2ed3 | 161 | if(timer.read()>t_top+1.0) cnt_judge = 0; |
mikawataru | 7:5f693667d0e9 | 162 | } |
mikawataru | 16:0a239047eb29 | 163 | /*FIXME:当日は有効化 |
mikawataru | 14:cbee1bfdfca7 | 164 | if(timer.read()-t_launch>TIME_REACH_TOP){ |
mikawataru | 16:0a239047eb29 | 165 | _Servo(2,UNLOCK); |
mikawataru | 7:5f693667d0e9 | 166 | Phase = DROP; |
mikawataru | 11:b61d0fcc2ed3 | 167 | cnt_judge = 0; |
mikawataru | 7:5f693667d0e9 | 168 | } |
mikawataru | 16:0a239047eb29 | 169 | */ |
mikawataru | 7:5f693667d0e9 | 170 | break; |
mikawataru | 7:5f693667d0e9 | 171 | case DROP://降下中(缶サット開放判定) |
mikawataru | 16:0a239047eb29 | 172 | if(alt_buff[col_open] < alt_launch+(float)ALT_UAV_DROP){ |
mikawataru | 11:b61d0fcc2ed3 | 173 | if(cnt_judge++==3){ |
mikawataru | 16:0a239047eb29 | 174 | Phase++; |
mikawataru | 16:0a239047eb29 | 175 | _Servo(1,UNLOCK); |
mikawataru | 11:b61d0fcc2ed3 | 176 | cnt_judge = 0; |
mikawataru | 11:b61d0fcc2ed3 | 177 | } |
mikawataru | 7:5f693667d0e9 | 178 | t_drop = timer.read(); |
mikawataru | 7:5f693667d0e9 | 179 | }else{ |
mikawataru | 11:b61d0fcc2ed3 | 180 | if(timer.read()>t_drop+1.0)cnt_judge = 0; |
mikawataru | 7:5f693667d0e9 | 181 | } |
mikawataru | 3:9cd74af355cc | 182 | break; |
mikawataru | 0:10a9c9c5ce83 | 183 | } |
mikawataru | 11:b61d0fcc2ed3 | 184 | if(col_open++==RATE_OPEN) col_open = 0; |
mikawataru | 0:10a9c9c5ce83 | 185 | } |
mikawataru | 6:15204813ef87 | 186 | /*記録用関数 RATE_LOG[Hz]で記録を行う*/ |
mikawataru | 3:9cd74af355cc | 187 | void _Log(){ |
mikawataru | 14:cbee1bfdfca7 | 188 | t[row][col] = timer.read(); |
mikawataru | 14:cbee1bfdfca7 | 189 | pressure[row][col] = ms5607.getPressure()/100; |
mikawataru | 3:9cd74af355cc | 190 | temperature[row][col] = ms5607.getTemperature(); |
mikawataru | 3:9cd74af355cc | 191 | mpu.getAccelero(&acc[row][col][0]); |
mikawataru | 3:9cd74af355cc | 192 | mpu.getGyro(&gyro[row][col][0]); |
mikawataru | 16:0a239047eb29 | 193 | fprintf(fp,"%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", |
mikawataru | 11:b61d0fcc2ed3 | 194 | t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0]/10.0, |
mikawataru | 11:b61d0fcc2ed3 | 195 | acc[row][col][1]/10.0,acc[row][col][2]/10.0,gyro[row][col][0],gyro[row][col][1],gyro[row][col][2] |
mikawataru | 3:9cd74af355cc | 196 | ); |
mikawataru | 3:9cd74af355cc | 197 | if(col++==RATE_LOG){ |
mikawataru | 3:9cd74af355cc | 198 | fclose(fp); |
mikawataru | 14:cbee1bfdfca7 | 199 | fp = fopen("/sd/log.txt", "a"); |
mikawataru | 3:9cd74af355cc | 200 | row =! row; |
mikawataru | 14:cbee1bfdfca7 | 201 | col = 0; |
mikawataru | 3:9cd74af355cc | 202 | } |
mikawataru | 3:9cd74af355cc | 203 | } |
mikawataru | 16:0a239047eb29 | 204 | /*サーボ動作用関数 _Servo(int8_t 扉番号,int8_t 動作)*/ |
mikawataru | 16:0a239047eb29 | 205 | void _Servo(int8_t door_num, int8_t motion){ |
mikawataru | 16:0a239047eb29 | 206 | if(door_num==1){//扉1 |
mikawataru | 16:0a239047eb29 | 207 | if(motion==UNLOCK){ |
mikawataru | 16:0a239047eb29 | 208 | Door_1_1.pulsewidth(0.0015); |
mikawataru | 16:0a239047eb29 | 209 | wait(0.2); |
mikawataru | 16:0a239047eb29 | 210 | Door_1_2.pulsewidth(0.0015); |
mikawataru | 16:0a239047eb29 | 211 | }else if(motion==LOCK){ |
mikawataru | 16:0a239047eb29 | 212 | Door_1_1.pulsewidth(0.0024); |
mikawataru | 16:0a239047eb29 | 213 | wait(0.2); |
mikawataru | 16:0a239047eb29 | 214 | Door_1_2.pulsewidth(0.0005); |
mikawataru | 16:0a239047eb29 | 215 | }else{ |
mikawataru | 16:0a239047eb29 | 216 | device.printf("error%f\r\n",motion); |
mikawataru | 16:0a239047eb29 | 217 | } |
mikawataru | 6:15204813ef87 | 218 | }else if(door_num==2){//扉2 |
mikawataru | 16:0a239047eb29 | 219 | if(motion==UNLOCK){ |
mikawataru | 16:0a239047eb29 | 220 | Door_2_1.pulsewidth(0.0015); |
mikawataru | 16:0a239047eb29 | 221 | wait(0.2); |
mikawataru | 16:0a239047eb29 | 222 | Door_2_2.pulsewidth(0.0015); |
mikawataru | 16:0a239047eb29 | 223 | }else if(motion==LOCK){ |
mikawataru | 16:0a239047eb29 | 224 | Door_2_1.pulsewidth(0.0024); |
mikawataru | 16:0a239047eb29 | 225 | wait(0.2); |
mikawataru | 16:0a239047eb29 | 226 | Door_2_2.pulsewidth(0.0005); |
mikawataru | 16:0a239047eb29 | 227 | }else{ |
mikawataru | 16:0a239047eb29 | 228 | device.printf("error%f\r\n",motion); |
mikawataru | 16:0a239047eb29 | 229 | } |
mikawataru | 6:15204813ef87 | 230 | }else{ |
mikawataru | 12:211d2c6d2afc | 231 | device.printf("servo error:%d\r\n",door_num); |
mikawataru | 6:15204813ef87 | 232 | } |
mikawataru | 3:9cd74af355cc | 233 | } |
mikawataru | 7:5f693667d0e9 | 234 | |
mikawataru | 12:211d2c6d2afc | 235 | /*入力用*/ |
mikawataru | 15:01d3969b89d0 | 236 | // TODO:テストモード実装 |
mikawataru | 15:01d3969b89d0 | 237 | // FIXME:android端末だと入力崩れが起こる |
mikawataru | 12:211d2c6d2afc | 238 | void _Input(){ |
mikawataru | 14:cbee1bfdfca7 | 239 | input_buff[input_cnt] = device.getc(); |
mikawataru | 14:cbee1bfdfca7 | 240 | device.printf("\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 241 | device.printf("input_cnt:%d\r\n",input_cnt); |
mikawataru | 15:01d3969b89d0 | 242 | device.printf("input_buff:%s\r\n",input_buff); |
mikawataru | 14:cbee1bfdfca7 | 243 | switch (Mode) { |
mikawataru | 14:cbee1bfdfca7 | 244 | case STANDBY: |
mikawataru | 14:cbee1bfdfca7 | 245 | if(input_cnt==0){ |
mikawataru | 14:cbee1bfdfca7 | 246 | if(input_buff[0]=='S'){ |
mikawataru | 14:cbee1bfdfca7 | 247 | device.printf("P >> Parachute\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 248 | device.printf("C >> CanSat\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 249 | }else if(input_buff[0]=='M'){ |
mikawataru | 14:cbee1bfdfca7 | 250 | device.printf("S >> STANDBY\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 251 | device.printf("T >> TEST\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 252 | device.printf("F >> FLIGHT\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 253 | }else{ |
mikawataru | 14:cbee1bfdfca7 | 254 | device.printf("This command is not found >> %c\r\n",input_buff[0]); |
mikawataru | 14:cbee1bfdfca7 | 255 | device.printf(">>MAINMENU<<\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 256 | device.printf("S >> Servo Operation\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 257 | device.printf("M >> Mode Change\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 258 | device.printf("-->>"); |
mikawataru | 14:cbee1bfdfca7 | 259 | return; |
mikawataru | 14:cbee1bfdfca7 | 260 | } |
mikawataru | 14:cbee1bfdfca7 | 261 | }else if(input_cnt==1){ |
mikawataru | 14:cbee1bfdfca7 | 262 | if(input_buff[0]=='S'){ |
mikawataru | 14:cbee1bfdfca7 | 263 | if(input_buff[1]=='P'||input_buff[1]=='C'){ |
mikawataru | 14:cbee1bfdfca7 | 264 | device.printf("U >> UNLOCK\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 265 | device.printf("L >> LOCK\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 266 | }else{ |
mikawataru | 14:cbee1bfdfca7 | 267 | device.printf("This command is not found >> %c\r\n",input_buff[1]); |
mikawataru | 14:cbee1bfdfca7 | 268 | device.printf("P >> Parachute\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 269 | device.printf("C >> CanSat\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 270 | device.printf("-->>"); |
mikawataru | 14:cbee1bfdfca7 | 271 | return; |
mikawataru | 14:cbee1bfdfca7 | 272 | } |
mikawataru | 14:cbee1bfdfca7 | 273 | }else if(input_buff[0]=='M'){ |
mikawataru | 14:cbee1bfdfca7 | 274 | if(input_buff[1]=='S'){ |
mikawataru | 14:cbee1bfdfca7 | 275 | Mode = STANDBY; |
mikawataru | 14:cbee1bfdfca7 | 276 | device.printf(">>MAINMENU<<\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 277 | device.printf("S >> Servo Operation\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 278 | device.printf("M >> Mode Change\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 279 | device.printf("-->>"); |
mikawataru | 14:cbee1bfdfca7 | 280 | }else if(input_buff[1]=='T'){ |
mikawataru | 14:cbee1bfdfca7 | 281 | Mode = TEST; |
mikawataru | 15:01d3969b89d0 | 282 | Phase = RISE; |
mikawataru | 15:01d3969b89d0 | 283 | alt_launch = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature()); |
mikawataru | 14:cbee1bfdfca7 | 284 | }else if(input_buff[1]=='F'){ |
mikawataru | 14:cbee1bfdfca7 | 285 | Mode = FLIGHT; |
mikawataru | 15:01d3969b89d0 | 286 | Phase = LAUNCH; |
mikawataru | 15:01d3969b89d0 | 287 | loop_log.attach(&_Log,1.0/RATE_LOG); |
mikawataru | 14:cbee1bfdfca7 | 288 | device.printf("FLIGHT-Mode ON!\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 289 | device.printf("***alert***\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 290 | device.printf("You will be able to reset only!\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 291 | }else{ |
mikawataru | 14:cbee1bfdfca7 | 292 | device.printf("This command is not found >> %c\r\n",input_buff[1]); |
mikawataru | 14:cbee1bfdfca7 | 293 | device.printf("S >> STANDBY\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 294 | device.printf("T >> TEST\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 295 | device.printf("F >> FLIGHT\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 296 | device.printf("-->>"); |
mikawataru | 14:cbee1bfdfca7 | 297 | return; |
mikawataru | 14:cbee1bfdfca7 | 298 | } |
mikawataru | 14:cbee1bfdfca7 | 299 | input_cnt = 0; |
mikawataru | 14:cbee1bfdfca7 | 300 | } |
mikawataru | 14:cbee1bfdfca7 | 301 | }else if(input_cnt==2){ |
mikawataru | 14:cbee1bfdfca7 | 302 | if(input_buff[2]=='U'){ |
mikawataru | 16:0a239047eb29 | 303 | if(input_buff[1]=='P') _Servo(2,UNLOCK); |
mikawataru | 16:0a239047eb29 | 304 | if(input_buff[1]=='C') _Servo(1,UNLOCK); |
mikawataru | 14:cbee1bfdfca7 | 305 | }else if(input_buff[2]=='L'){ |
mikawataru | 16:0a239047eb29 | 306 | if(input_buff[1]=='P') _Servo(2,LOCK); |
mikawataru | 16:0a239047eb29 | 307 | if(input_buff[1]=='C') _Servo(1,LOCK); |
mikawataru | 14:cbee1bfdfca7 | 308 | }else{ |
mikawataru | 14:cbee1bfdfca7 | 309 | device.printf("This command is not found >> %c\r\n",input_buff[2]); |
mikawataru | 14:cbee1bfdfca7 | 310 | device.printf("U >> UNLOCK\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 311 | device.printf("L >> LOCK\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 312 | device.printf("-->>"); |
mikawataru | 14:cbee1bfdfca7 | 313 | return; |
mikawataru | 14:cbee1bfdfca7 | 314 | } |
mikawataru | 14:cbee1bfdfca7 | 315 | input_cnt = 0; |
mikawataru | 14:cbee1bfdfca7 | 316 | device.printf(">>MAINMENU<<\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 317 | device.printf("S >> Servo Operation\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 318 | device.printf("M >> Mode Change\r\n"); |
mikawataru | 14:cbee1bfdfca7 | 319 | device.printf("-->>"); |
mikawataru | 14:cbee1bfdfca7 | 320 | return; |
mikawataru | 12:211d2c6d2afc | 321 | } |
mikawataru | 14:cbee1bfdfca7 | 322 | device.printf("-->>"); |
mikawataru | 14:cbee1bfdfca7 | 323 | input_cnt++; |
mikawataru | 12:211d2c6d2afc | 324 | break; |
mikawataru | 14:cbee1bfdfca7 | 325 | case TEST: |
mikawataru | 12:211d2c6d2afc | 326 | break; |
mikawataru | 14:cbee1bfdfca7 | 327 | case FLIGHT://reset only |
mikawataru | 12:211d2c6d2afc | 328 | break; |
mikawataru | 12:211d2c6d2afc | 329 | } |
mikawataru | 12:211d2c6d2afc | 330 | } |
mikawataru | 12:211d2c6d2afc | 331 | |
mikawataru | 12:211d2c6d2afc | 332 | /*その他雑関数*/ |
mikawataru | 14:cbee1bfdfca7 | 333 | float _Measure_Alt(float press/*[hPa]*/, float temp/*[℃]*/){ |
mikawataru | 14:cbee1bfdfca7 | 334 | return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; |
mikawataru | 7:5f693667d0e9 | 335 | } |
mikawataru | 14:cbee1bfdfca7 | 336 | float _Median(float data[], int num){ |
mikawataru | 11:b61d0fcc2ed3 | 337 | float median; |
mikawataru | 11:b61d0fcc2ed3 | 338 | float *sort = (float *)malloc(sizeof(float)*num); |
mikawataru | 11:b61d0fcc2ed3 | 339 | for(int i=0; i<num; i++) sort[i] = data[i]; |
mikawataru | 11:b61d0fcc2ed3 | 340 | for(int i=0; i<num; i++){ |
mikawataru | 11:b61d0fcc2ed3 | 341 | for(int j=0; j<num-i-1; j++){ |
mikawataru | 11:b61d0fcc2ed3 | 342 | if(sort[j]>sort[j+1]){ |
mikawataru | 11:b61d0fcc2ed3 | 343 | float buff = sort[j+1]; |
mikawataru | 11:b61d0fcc2ed3 | 344 | sort[j+1] = sort[j]; |
mikawataru | 11:b61d0fcc2ed3 | 345 | sort[j] = buff; |
mikawataru | 11:b61d0fcc2ed3 | 346 | } |
mikawataru | 11:b61d0fcc2ed3 | 347 | } |
mikawataru | 11:b61d0fcc2ed3 | 348 | } |
mikawataru | 11:b61d0fcc2ed3 | 349 | if(num%2!=0)median = sort[num/2]; |
mikawataru | 11:b61d0fcc2ed3 | 350 | else median = (sort[num/2-1]+sort[num/2])/2.0; |
mikawataru | 11:b61d0fcc2ed3 | 351 | free(sort); |
mikawataru | 11:b61d0fcc2ed3 | 352 | return median; |
mikawataru | 11:b61d0fcc2ed3 | 353 | } |