CORE / Mbed 2 deprecated 2017_03_Kaiho

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

Dependencies:   MPU6050 MS5607 mbed SDFileSystem

main.cpp@13:f73b4beded57, 2017-03-03 (annotated)

Committer:
mikawataru
Date:
Fri Mar 03 05:14:02 2017 +0000
Revision:
13:f73b4beded57
Parent:
12:211d2c6d2afc
Child:
14:cbee1bfdfca7
add mode transition function by serial device

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 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 12:211d2c6d2afc 49 #define ALT_CAN 150//缶サット開放高度[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 12:211d2c6d2afc 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 12:211d2c6d2afc 62 /*入力モード定義*/
mikawataru 12:211d2c6d2afc 63 #define BAN 0
mikawataru 12:211d2c6d2afc 64 #define NORMAL 1
mikawataru 12:211d2c6d2afc 65 #define SERVO_1 2
mikawataru 12:211d2c6d2afc 66 #define SERVO_2 3
mikawataru 12:211d2c6d2afc 67 #define MODE 4
mikawataru 13:f73b4beded57 68 #define DEBUG 5
mikawataru 12:211d2c6d2afc 69 /**/
mikawataru 8:a9e115560ba3 70 #define P0 1013.25f//海面気圧[hPa]
mikawataru 11:b61d0fcc2ed3 71 #define ACC 4096.0f//加速度オフセット値
mikawataru 7:5f693667d0e9 72 /*ピン指定*/
mikawataru 0:10a9c9c5ce83 73 MS5607I2C ms5607(p9, p10, false);
mikawataru 0:10a9c9c5ce83 74 MPU6050 mpu(p9,p10);
mikawataru 3:9cd74af355cc 75 BusOut myled(LED1,LED2,LED3,LED4);
mikawataru 7:5f693667d0e9 76 SDFileSystem sd(p11, p12, p13, p14, "sd");
mikawataru 10:c1d1ff35a6a1 77 Serial device(USBTX, USBRX);
mikawataru 5:bcf55d8fe7a7 78 PwmOut Door_1_1(p23);
mikawataru 5:bcf55d8fe7a7 79 PwmOut Door_1_2(p24);
mikawataru 5:bcf55d8fe7a7 80 PwmOut Door_2_1(p25);
mikawataru 5:bcf55d8fe7a7 81 PwmOut Door_2_2(p26);
mikawataru 7:5f693667d0e9 82 /*タイマー類*/
mikawataru 1:ec75f428c6b3 83 Timer timer;
mikawataru 3:9cd74af355cc 84 Ticker loop_log;
mikawataru 3:9cd74af355cc 85 Ticker loop_open;
mikawataru 5:bcf55d8fe7a7 86 /*ログカウンタ*/
mikawataru 5:bcf55d8fe7a7 87 bool row = 0;
mikawataru 3:9cd74af355cc 88 int8_t col = 0;
mikawataru 5:bcf55d8fe7a7 89 /*ログ格納用*/
mikawataru 11:b61d0fcc2ed3 90 float altitude[2][RATE_LOG],pressure[2][RATE_LOG],temperature[2][RATE_LOG];
mikawataru 3:9cd74af355cc 91 float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3];
mikawataru 3:9cd74af355cc 92 float t[2][RATE_LOG];
mikawataru 7:5f693667d0e9 93 FILE *fp;
mikawataru 11:b61d0fcc2ed3 94 /*フェーズ変数*/
mikawataru 5:bcf55d8fe7a7 95 int8_t Phase = STANDBY;
mikawataru 7:5f693667d0e9 96 /*判定用*/
mikawataru 11:b61d0fcc2ed3 97 float alt4open[RATE_OPEN],acc4open;
mikawataru 11:b61d0fcc2ed3 98 float alt_max,alt_launch;
mikawataru 7:5f693667d0e9 99 float t_drop,t_top,t_launch;
mikawataru 11:b61d0fcc2ed3 100 int8_t cnt_judge = 0;
mikawataru 11:b61d0fcc2ed3 101 int8_t col_open = 0;
mikawataru 12:211d2c6d2afc 102 /*入力用*/
mikawataru 12:211d2c6d2afc 103 int8_t Input_mode = NORMAL;
mikawataru 12:211d2c6d2afc 104 int8_t input_buff[3] = {};
mikawataru 7:5f693667d0e9 105 /*関数*/
mikawataru 3:9cd74af355cc 106 void _Open();
mikawataru 6:15204813ef87 107 void _Servo(int8_t door_num, float motion);
mikawataru 3:9cd74af355cc 108 void _Log();
mikawataru 12:211d2c6d2afc 109 void _Input();
mikawataru 11:b61d0fcc2ed3 110 float _GetAlt(float press, float temp);
mikawataru 11:b61d0fcc2ed3 111 float _Median(float data[], int num);
mikawataru 11:b61d0fcc2ed3 112
mikawataru 7:5f693667d0e9 113 /*---------------------------------------------------------------------*/
mikawataru 0:10a9c9c5ce83 114 int main() {
mikawataru 11:b61d0fcc2ed3 115 mpu.setAcceleroRange(2);
mikawataru 11:b61d0fcc2ed3 116 mpu.setGyroRange(2);
mikawataru 1:ec75f428c6b3 117 timer.start();
mikawataru 5:bcf55d8fe7a7 118 Door_1_1.period_ms(20);
mikawataru 5:bcf55d8fe7a7 119 Door_1_2.period_ms(20);
mikawataru 5:bcf55d8fe7a7 120 Door_2_1.period_ms(20);
mikawataru 5:bcf55d8fe7a7 121 Door_2_2.period_ms(20);
mikawataru 6:15204813ef87 122 _Servo(0,LOCK);
mikawataru 2:b6eb08d059cc 123 fp = fopen("/sd/log.txt", "w");
mikawataru 1:ec75f428c6b3 124 fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
mikawataru 12:211d2c6d2afc 125 device.attach(&_Input,Serial::RxIrq);
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 3:9cd74af355cc 131 switch (Phase) {
mikawataru 3:9cd74af355cc 132 case STANDBY://スタンバイモード（発射判定不可）
mikawataru 12:211d2c6d2afc 133 // Phase = LAUNCH;
mikawataru 3:9cd74af355cc 134 break;
mikawataru 7:5f693667d0e9 135 case LAUNCH://点火モード（発射判定可）
mikawataru 11:b61d0fcc2ed3 136 acc4open = (float)mpu.getAcceleroRawZ()/(ACC*0.981);
mikawataru 11:b61d0fcc2ed3 137 alt4open[col_open] = _GetAlt(ms5607.getPressure()/100,ms5607.getTemperature());
mikawataru 11:b61d0fcc2ed3 138 if(acc4open>ACC_LAUNCH){
mikawataru 11:b61d0fcc2ed3 139 if(cnt_judge++==3){
mikawataru 11:b61d0fcc2ed3 140 Phase = FLIGHT;
mikawataru 11:b61d0fcc2ed3 141 alt_launch = _Median(alt4open, 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 7:5f693667d0e9 150 case FLIGHT://飛翔中（パラシュート開放判定）
mikawataru 11:b61d0fcc2ed3 151 alt4open[col_open] = _GetAlt(ms5607.getPressure()/100,ms5607.getTemperature());
mikawataru 11:b61d0fcc2ed3 152 if(alt4open[col_open]>alt_max) alt_max = alt4open[col_open];
mikawataru 11:b61d0fcc2ed3 153 if(alt4open[col_open]<alt_max-DROP_TOP){
mikawataru 11:b61d0fcc2ed3 154 if(cnt_judge++==3){
mikawataru 7:5f693667d0e9 155 _Servo(1,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 7:5f693667d0e9 163 if(timer.read()-t_launch>TIME_TOP){
mikawataru 7:5f693667d0e9 164 _Servo(1,UNLOCK);
mikawataru 7:5f693667d0e9 165 Phase = DROP;
mikawataru 11:b61d0fcc2ed3 166 cnt_judge = 0;
mikawataru 7:5f693667d0e9 167 }
mikawataru 7:5f693667d0e9 168 break;
mikawataru 7:5f693667d0e9 169 case DROP://降下中（缶サット開放判定)
mikawataru 11:b61d0fcc2ed3 170 if(alt4open[col_open] < alt_launch+ALT_CAN){
mikawataru 11:b61d0fcc2ed3 171 if(cnt_judge++==3){
mikawataru 11:b61d0fcc2ed3 172 _Servo(2,UNLOCK);
mikawataru 11:b61d0fcc2ed3 173 cnt_judge = 0;
mikawataru 11:b61d0fcc2ed3 174 }
mikawataru 7:5f693667d0e9 175 t_drop = timer.read();
mikawataru 7:5f693667d0e9 176 }else{
mikawataru 11:b61d0fcc2ed3 177 if(timer.read()>t_drop+1.0)cnt_judge = 0;
mikawataru 7:5f693667d0e9 178 }
mikawataru 3:9cd74af355cc 179 break;
mikawataru 0:10a9c9c5ce83 180 }
mikawataru 11:b61d0fcc2ed3 181 if(col_open++==RATE_OPEN) col_open = 0;
mikawataru 0:10a9c9c5ce83 182 }
mikawataru 6:15204813ef87 183 /*記録用関数　RATE_LOG[Hz]で記録を行う*/
mikawataru 3:9cd74af355cc 184 void _Log(){
mikawataru 3:9cd74af355cc 185 t[row][col] = timer.read();
mikawataru 7:5f693667d0e9 186 pressure[row][col] = ms5607.getPressure()/100;
mikawataru 3:9cd74af355cc 187 temperature[row][col] = ms5607.getTemperature();
mikawataru 3:9cd74af355cc 188 mpu.getAccelero(&acc[row][col][0]);
mikawataru 3:9cd74af355cc 189 mpu.getGyro(&gyro[row][col][0]);
mikawataru 3:9cd74af355cc 190 fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",
mikawataru 11:b61d0fcc2ed3 191 t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0]/10.0,
mikawataru 11:b61d0fcc2ed3 192 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 193 );
mikawataru 3:9cd74af355cc 194 if(col++==RATE_LOG){
mikawataru 3:9cd74af355cc 195 fclose(fp);
mikawataru 3:9cd74af355cc 196 fp = fopen("/sd/log.txt", "a");
mikawataru 3:9cd74af355cc 197 row =! row;
mikawataru 3:9cd74af355cc 198 col = 0;
mikawataru 3:9cd74af355cc 199 }
mikawataru 3:9cd74af355cc 200 }
mikawataru 6:15204813ef87 201 /*サーボ動作用関数 _Servo(int8_t 扉番号,float pwm波長)*/
mikawataru 6:15204813ef87 202 void _Servo(int8_t door_num, float motion){
mikawataru 6:15204813ef87 203 if(door_num==0){//全扉
mikawataru 6:15204813ef87 204 Door_1_1.pulsewidth(motion);
mikawataru 6:15204813ef87 205 Door_1_2.pulsewidth(motion);
mikawataru 6:15204813ef87 206 Door_2_1.pulsewidth(motion);
mikawataru 6:15204813ef87 207 Door_2_2.pulsewidth(motion);
mikawataru 6:15204813ef87 208 }else if(door_num==1){//扉１
mikawataru 6:15204813ef87 209 Door_1_1.pulsewidth(motion);
mikawataru 6:15204813ef87 210 Door_1_2.pulsewidth(motion);
mikawataru 6:15204813ef87 211 }else if(door_num==2){//扉２
mikawataru 6:15204813ef87 212 Door_2_1.pulsewidth(motion);
mikawataru 6:15204813ef87 213 Door_2_2.pulsewidth(motion);
mikawataru 6:15204813ef87 214 }else{
mikawataru 12:211d2c6d2afc 215 device.printf("servo error:%d\r\n",door_num);
mikawataru 6:15204813ef87 216 }
mikawataru 3:9cd74af355cc 217 }
mikawataru 7:5f693667d0e9 218
mikawataru 12:211d2c6d2afc 219 /*入力用*/
mikawataru 12:211d2c6d2afc 220 void _Input(){
mikawataru 12:211d2c6d2afc 221 char c = device.getc();
mikawataru 12:211d2c6d2afc 222 switch (Input_mode) {
mikawataru 12:211d2c6d2afc 223 case NORMAL:
mikawataru 12:211d2c6d2afc 224 if(c=='s'){
mikawataru 12:211d2c6d2afc 225 Input_mode = SERVO_1;
mikawataru 12:211d2c6d2afc 226 device.printf("\r\nInput_mode:SERVO\r\n");
mikawataru 12:211d2c6d2afc 227 device.printf("\r\n0:All\r\n1:Parachute\r\n2:Cansat\r\n>>");
mikawataru 13:f73b4beded57 228 }else if(c=='c'){
mikawataru 13:f73b4beded57 229 Input_mode = MODE;
mikawataru 13:f73b4beded57 230 device.printf("\r\nn:NORMAL\r\nf:FLIGHT\r\nt:TEST\r\n>>");
mikawataru 12:211d2c6d2afc 231 }else{
mikawataru 13:f73b4beded57 232 device.printf("\r\nthis command is not found:%c\r\n",c);
mikawataru 13:f73b4beded57 233 device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>");
mikawataru 12:211d2c6d2afc 234 }
mikawataru 12:211d2c6d2afc 235 break;
mikawataru 12:211d2c6d2afc 236 case SERVO_1:
mikawataru 12:211d2c6d2afc 237 input_buff[0] = (int)(c-'0');
mikawataru 12:211d2c6d2afc 238 if(c=='0'||c=='1'||c=='2'){
mikawataru 12:211d2c6d2afc 239 Input_mode = SERVO_2;
mikawataru 13:f73b4beded57 240 device.printf("\r\n0:OPEN\r\n1:CLOSE\r\n>>");
mikawataru 13:f73b4beded57 241 }else{
mikawataru 13:f73b4beded57 242 device.printf("\r\nthis servo_num is not found:%d\r\n",input_buff[0]);
mikawataru 13:f73b4beded57 243 device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>");
mikawataru 13:f73b4beded57 244 Input_mode = NORMAL;
mikawataru 12:211d2c6d2afc 245 }
mikawataru 12:211d2c6d2afc 246 break;
mikawataru 12:211d2c6d2afc 247 case SERVO_2:
mikawataru 12:211d2c6d2afc 248 if(c=='0')_Servo(input_buff[0],UNLOCK);
mikawataru 12:211d2c6d2afc 249 else if(c=='1')_Servo(input_buff[0],LOCK);
mikawataru 13:f73b4beded57 250 else device.printf("\r\nthis motion_num is not found:%c\r\n",c);
mikawataru 13:f73b4beded57 251 device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>");
mikawataru 12:211d2c6d2afc 252 Input_mode = NORMAL;
mikawataru 12:211d2c6d2afc 253 break;
mikawataru 12:211d2c6d2afc 254 case MODE:
mikawataru 13:f73b4beded57 255 if(c=='n'){
mikawataru 13:f73b4beded57 256 Input_mode = NORMAL;
mikawataru 13:f73b4beded57 257 device.printf("\r\nInput_mode:NORMAL\r\ns:SERVO\r\nc:ChangeMode\r\n>>");
mikawataru 13:f73b4beded57 258 }else if(c=='f'){
mikawataru 13:f73b4beded57 259 Input_mode = BAN;
mikawataru 13:f73b4beded57 260 device.printf("\r\nyou will be able to reset only after this action!");
mikawataru 13:f73b4beded57 261 fp = fopen("/sd/log.txt", "w");
mikawataru 13:f73b4beded57 262 fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n");
mikawataru 13:f73b4beded57 263 Phase = LAUNCH;
mikawataru 13:f73b4beded57 264 loop_log.attach(&_Log,1.0/RATE_LOG);
mikawataru 13:f73b4beded57 265 loop_open.attach(&_Open,1.0/RATE_OPEN);
mikawataru 13:f73b4beded57 266 device.printf("\r\nFLIGHT-Mode ON!\r\n");
mikawataru 13:f73b4beded57 267 }else if(c=='d'){
mikawataru 13:f73b4beded57 268 Input_mode = DEBUG;
mikawataru 13:f73b4beded57 269 }
mikawataru 13:f73b4beded57 270 break;
mikawataru 13:f73b4beded57 271 case DEBUG:
mikawataru 13:f73b4beded57 272
mikawataru 12:211d2c6d2afc 273 break;
mikawataru 12:211d2c6d2afc 274 case BAN://reset only
mikawataru 12:211d2c6d2afc 275 break;
mikawataru 12:211d2c6d2afc 276 default:
mikawataru 12:211d2c6d2afc 277 break;
mikawataru 12:211d2c6d2afc 278 }
mikawataru 12:211d2c6d2afc 279 }
mikawataru 12:211d2c6d2afc 280
mikawataru 12:211d2c6d2afc 281 /*その他雑関数*/
mikawataru 13:f73b4beded57 282 float _GetAlt(float press, float temp){//高度計算
mikawataru 7:5f693667d0e9 283 return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
mikawataru 7:5f693667d0e9 284 }
mikawataru 13:f73b4beded57 285 float _Median(float data[], int num){//中央値計算
mikawataru 11:b61d0fcc2ed3 286 float median;
mikawataru 11:b61d0fcc2ed3 287 float *sort = (float *)malloc(sizeof(float)*num);
mikawataru 11:b61d0fcc2ed3 288 for(int i=0; i<num; i++) sort[i] = data[i];
mikawataru 11:b61d0fcc2ed3 289 for(int i=0; i<num; i++){
mikawataru 11:b61d0fcc2ed3 290 for(int j=0; j<num-i-1; j++){
mikawataru 11:b61d0fcc2ed3 291 if(sort[j]>sort[j+1]){
mikawataru 11:b61d0fcc2ed3 292 float buff = sort[j+1];
mikawataru 11:b61d0fcc2ed3 293 sort[j+1] = sort[j];
mikawataru 11:b61d0fcc2ed3 294 sort[j] = buff;
mikawataru 11:b61d0fcc2ed3 295 }
mikawataru 11:b61d0fcc2ed3 296 }
mikawataru 11:b61d0fcc2ed3 297 }
mikawataru 11:b61d0fcc2ed3 298 if(num%2!=0)median = sort[num/2];
mikawataru 11:b61d0fcc2ed3 299 else median = (sort[num/2-1]+sort[num/2])/2.0;
mikawataru 11:b61d0fcc2ed3 300 free(sort);
mikawataru 11:b61d0fcc2ed3 301 return median;
mikawataru 11:b61d0fcc2ed3 302 }