![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2017年3月,伊豆大島共同打上実験 CORE_缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
main.cpp
- Committer:
- mikawataru
- Date:
- 2017-03-14
- Revision:
- 18:5fb8ce45ce51
- Parent:
- 17:5f7808444d96
File content as of revision 18:5fb8ce45ce51:
/* 2017年3月 伊豆大島共同打上実験 団体名:CORE チーム名:ヌペリオル 該当電装:ロケット搭載用 使用部品 ・LPC1768(マイコン) ・MPU6050(加速度・ジャイロセンサ) ・MS5607(気圧・気温センサ) ・MicroSDスロット ・MG995(サーボモータ)×4 使用ライブラリ ・MPU6050.h https://developer.mbed.org/teams/mbed/code/SDFileSystem/ ・MS5607.h https://developer.mbed.org/users/yamaguch/code/MS5607/ ・SDFileSystem.h https://developer.mbed.org/teams/mbed/code/SDFileSystem/ ピン配置 LPC1768 MPU6050 MS5607 MicroSD MG995_1 MG995_2 MG995_3 MG995_4 VIN--------------------------------------------------------------------4.5~9.0V -----------------------------------VIN------VIN------VIN------VIN------5V Vout-----VCC------VIN-----VDD------------------------------------------3.3V \-ADO \-PS GND------GND------GND-----VSS------GND------GND------GND------GND------0V \-CS p9-------SDA------SDA---------------------------------------------------- p10------SCL------SCL---------------------------------------------------- p11-----------------------CLK-------------------------------------------- p12-----------------------DAT0------------------------------------------- p13-----------------------CMD-------------------------------------------- p14-----------------------CD/DAT0---------------------------------------- p23--------------------------------CTRL---------------------------------- p24-----------------------------------------CTRL------------------------- p25--------------------------------------------------CTRL---------------- p26-----------------------------------------------------------CTRL------- */ #include "mbed.h" #include "math.h" #include "MS5607I2C.h" #include "MPU6050.h" #include "SDFileSystem.h" #define ACC_LAUNCH 4.0f//FIXME:本番は4g #define TOP_DROP_AMOUNT 1.5f #define ALT_UAV_DROP 150.0f//FIXME:本番は150m #define TIME_REACH_TOP 15.0f//FIXME:本番は15sec #define RATE_LOG 10 #define RATE_OPEN 10 /*サーボ動作*/ #define LOCK 0 #define UNLOCK 1 /*モード定義*/ #define STANDBY 0 #define TEST 1 #define FLIGHT 2 /*開放フェーズ定義*/ #define SETUP 0 #define LAUNCH 1 #define RISE 2 #define DROP 3 /**/ #define P0 1013.25f//海面気圧[hPa] #define ACC 4096.0f//加速度オフセット値 /*ピン指定*/ MS5607I2C ms5607(p9, p10, false); MPU6050 mpu(p9,p10); BusOut myled(LED1,LED2,LED3,LED4); SDFileSystem sd(p11, p12, p13, p14, "sd"); Serial device(USBTX, USBRX); PwmOut Door_1_1(p23); PwmOut Door_1_2(p24); PwmOut Door_2_1(p25); PwmOut Door_2_2(p26); /*タイマー類*/ Timer timer; Ticker loop_log; Ticker loop_open; /*ログカウンタ*/ bool row = 0; int8_t col = 0; /*ログ格納用*/ float pressure[2][RATE_LOG],temperature[2][RATE_LOG]; float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3]; float t[2][RATE_LOG]; FILE *fp; /*フェーズ変数*/ int8_t Phase = SETUP; int8_t Mode = STANDBY; /*判定用*/ float alt_buff[RATE_OPEN]; float alt_max,alt_launch; float t_drop,t_top,t_launch; int8_t cnt_judge = 0; int8_t col_open = 0; /*入力用*/ int8_t input_buff[3] = {}; int8_t input_cnt = 0; /*関数*/ void _Open(); void _Log(); void _Servo(int8_t door_num, int8_t motion); void _Input(); float _Measure_Alt(float press, float temp); float _Median(float data[], int num); /*---------------------------------------------------------------------*/ int main() { mpu.setAcceleroRange(2); mpu.setGyroRange(2); timer.start(); Door_1_1.period_ms(20); Door_1_2.period_ms(20); Door_2_1.period_ms(20); Door_2_2.period_ms(20); // _Servo(1,UNLOCK);//todo:当日は状態記憶に仕様変更予定? // _Servo(2,UNLOCK);// fp = fopen("/sd/log.txt", "a"); fprintf(fp, "Phase,time,pressure,temperature,ax,ay,az,gx,gy,gz\r\n"); device.attach(&_Input,Serial::RxIrq); loop_open.attach(&_Open,1.0/RATE_OPEN); while(1); } /*開放用関数 RATE_OPEN[Hz]で判定を行う*/ void _Open(){ myled = 1 << (Phase-1); switch (Phase) { case SETUP://セットアップモード(発射判定不可) break; case LAUNCH://点火モード(発射判定可) float acc_buff = (float)mpu.getAcceleroRawZ()/(ACC*0.981); alt_buff[col_open] = ms5607.getAltitude(); if(acc_buff>ACC_LAUNCH){ if(cnt_judge++==3){ Phase = RISE; alt_launch = _Median(alt_buff, RATE_OPEN); cnt_judge = 0; } t_launch = timer.read(); alt_max = alt_launch; }else{ if(timer.read()>t_launch+1.0) cnt_judge = 0; } break; case RISE://上昇中(パラシュート開放判定) float alt_rising = ms5607.getAltitude(); if( alt_rising>alt_max && alt_rising-alt_max < 10.0 ) alt_max = alt_rising; if(alt_rising<alt_max-TOP_DROP_AMOUNT){ if(cnt_judge++==3){ _Servo(2,UNLOCK); Phase = DROP; cnt_judge = 0; } t_top = timer.read(); }else{ if(timer.read()>t_top+1.0) cnt_judge = 0; } if(timer.read()-t_launch>TIME_REACH_TOP){ _Servo(2,UNLOCK); Phase = DROP; cnt_judge = 0; } break; case DROP://降下中(缶サット開放判定) float alt_droping = ms5607.getAltitude(); if(alt_droping < alt_launch+ALT_UAV_DROP){ if(cnt_judge++==3){ Phase++; _Servo(1,UNLOCK); cnt_judge = 0; } t_drop = timer.read(); }else{ if(timer.read()>t_drop+1.0)cnt_judge = 0; } break; } if(col_open++==RATE_OPEN) col_open = 0; } /*記録用関数 RATE_LOG[Hz]で記録を行う*/ void _Log(){ if(t[row][col] = timer.read()>=30.0*60.0){ timer.reset(); t[row][col] = timer.read(); } pressure[row][col] = ms5607.getPressure()/100; temperature[row][col] = ms5607.getTemperature(); mpu.getAccelero(&acc[row][col][0]); mpu.getGyro(&gyro[row][col][0]); fprintf(fp,"%d,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", Phase,t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0]/10.0, acc[row][col][1]/10.0,acc[row][col][2]/10.0,gyro[row][col][0],gyro[row][col][1],gyro[row][col][2] ); if(col++==RATE_LOG){ col = 0; row =! row; fclose(fp); fp = fopen("/sd/log.txt", "a"); } } /*サーボ動作用関数 _Servo(int8_t 扉番号,int8_t 動作)*/ void _Servo(int8_t door_num, int8_t motion){ if(door_num==1){//缶サット if(motion==UNLOCK){ Door_1_1.pulsewidth(0.0015); wait(0.2); Door_1_2.pulsewidth(0.0015); }else if(motion==LOCK){ Door_1_1.pulsewidth(0.0024); wait(0.2); Door_1_2.pulsewidth(0.0005); }else{ device.printf("error%f\r\n",motion); } }else if(door_num==2){//パラシュート if(motion==UNLOCK){ Door_2_1.pulsewidth(0.0015); wait(0.2); Door_2_2.pulsewidth(0.0015); }else if(motion==LOCK){ Door_2_1.pulsewidth(0.0024); wait(0.2); Door_2_2.pulsewidth(0.0005); }else{ device.printf("error%f\r\n",motion); } }else{ device.printf("servo error:%d\r\n",door_num); } } /*入力用*/ // FIXME:android端末だと入力崩れが起こる void _Input(){ input_buff[input_cnt] = device.getc(); device.printf("\r\n"); switch (Mode) { case STANDBY: if(input_cnt==0){ if(input_buff[0]=='S'){ device.printf("P : Parachute\r\n"); device.printf("C : CanSat\r\n"); }else if(input_buff[0]=='M'){ device.printf("S : STANDBY\r\n"); device.printf("F : FLIGHT\r\n"); }else{ device.printf("This command is not found >> %c\r\n",input_buff[0]); device.printf(">>MAINMENU<<\r\n"); device.printf("S : Servo Operation\r\n"); device.printf("M : Mode Change\r\n"); device.printf("-->>"); return; } }else if(input_cnt==1){ if(input_buff[0]=='S'){ if(input_buff[1]=='P'||input_buff[1]=='C'){ device.printf("U : UNLOCK\r\n"); device.printf("L : LOCK\r\n"); }else{ device.printf("This command is not found >> %c\r\n",input_buff[1]); device.printf("P : Parachute\r\n"); device.printf("C : CanSat\r\n"); device.printf("-->>"); return; } }else if(input_buff[0]=='M'){ if(input_buff[1]=='S'){ Mode = STANDBY; device.printf(">>MAINMENU<<\r\n"); device.printf("S : Servo Operation\r\n"); device.printf("M : Mode Change\r\n"); }else if(input_buff[1]=='F'){ Mode = FLIGHT; Phase = LAUNCH; loop_log.attach(&_Log,1.0/RATE_LOG); device.printf("FLIGHT-Mode ON!\r\n"); device.printf("***alert***\r\n"); device.printf("You will be able to reset only!\r\n"); }else{ device.printf("This command is not found >> %c\r\n",input_buff[1]); device.printf("S : STANDBY\r\n"); device.printf("F : FLIGHT\r\n"); device.printf("-->>"); return; } input_cnt = 0; } }else if(input_cnt==2){ if(input_buff[2]=='U'){ if(input_buff[1]=='P') _Servo(2,UNLOCK); if(input_buff[1]=='C') _Servo(1,UNLOCK); }else if(input_buff[2]=='L'){ if(input_buff[1]=='P') _Servo(2,LOCK); if(input_buff[1]=='C') _Servo(1,LOCK); }else{ device.printf("This command is not found >> %c\r\n",input_buff[2]); device.printf("U : UNLOCK\r\n"); device.printf("L : LOCK\r\n"); device.printf("-->>"); return; } input_cnt = 0; device.printf(">>MAINMENU<<\r\n"); device.printf("S : Servo Operation\r\n"); device.printf("M : Mode Change\r\n"); device.printf("-->>"); return; } device.printf("-->>"); input_cnt++; break; case FLIGHT://reset only break; } } /*その他雑関数*/ float _Measure_Alt(float press/*[hPa]*/, float temp/*[℃]*/){ return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; } float _Median(float data[], int num){ float median; float *sort = (float *)malloc(sizeof(float)*num); for(int i=0; i<num; i++) sort[i] = data[i]; for(int i=0; i<num; i++){ for(int j=0; j<num-i-1; j++){ if(sort[j]>sort[j+1]){ float buff = sort[j+1]; sort[j+1] = sort[j]; sort[j] = buff; } } } if(num%2!=0)median = sort[num/2]; else median = (sort[num/2-1]+sort[num/2])/2.0; free(sort); return median; }