201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
main.cpp
- Committer:
- Yukina
- Date:
- 2018-08-09
- Revision:
- 4:4b3ae90ec778
- Parent:
- 3:24a8901befb6
- Child:
- 5:f6e956e8a060
File content as of revision 4:4b3ae90ec778:
#include "mbed.h" #include "math.h" #include "MPU6050.h" #include "BMP180.h" #include "SDFileSystem.h" /*マクロ処理*/ //動作レート #define RATE_LOG 20 #define RATE_OPEN 50 //発射判定 #define ACC_JUDGE_LAUNCH 1.5 #define CNT_JUDGE_LAUNCH 5 //待機時間 #define TIME_GAP 3.0 //頂点判定 #define ALT_JUDGE_OPEN 1.5 #define CNT_JUDGE_OPEN 5 #define TIME_JUDGE_OPEN 30 //放出判定 #define TIME_JUDGE_OUT 60 #define ALT_JUDGE_OUT -1.5 #define CNT_JUDGE_OUT 5 //共通 #define TIME_JUDGE_CNT 1.5 #define p0 1013.25f //ログ //2018.06.28 edit #define NUM_DATA 20 MPU6050 mpu(p9,p10); BMP180 bmp(p9,p10); BusOut led(LED1,LED2,LED3,LED4); Serial pc(USBTX,USBRX); Serial twe(p28,p27); Serial gps(p13,p14); PwmOut servo1(p21); PwmOut servo2(p22); PwmOut servo3(p23); PwmOut servo4(p24); DigitalOut sw(p25); SDFileSystem sd(p5,p6,p7,p8,"sd"); LocalFileSystem local("local"); Timer timer_open; Timer timer_log; Ticker tic_open; Ticker tic_log; void _open(); void _log(); float _getAlt(); float _DMS2DEG(float raw_data); float _median(float data[],int num); int _input(char cha); float Alt_buf[10], Alt_gnd, Alt_md, Alt_max=-300; //TODO:Alt_maxの値を調整すること float Time_alt; float Data[2][NUM_DATA][8]; int Cnt=0, Cnt_acc=0, Cnt_alt=0; int Cnt_data=0; int cnt_gps; char gps_data[256]; bool Row; FILE *lfp; FILE *fp; enum PHASE{SETUP=0,LAUNCH=1,WAIT=3,RISE=7,DROP1=15,DROP2=9,SEA=10} Phase; enum SERVO{LOCK=0,UNLOCK=1} Servo; int main(){ /*センサ類初期化*/ bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); twe.baud(115200); sw=1; /*ファイル作成*/ mkdir("/sd/mydir",0777); fp=fopen("/sd/mydir/data.txt","a"); fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n"); fclose(fp); lfp=fopen("/local/data.txt","a"); fclose(lfp); /*サーボ調整*/ servo1.period_ms(20); servo2.period_ms(20); servo3.period_ms(20); servo4.period_ms(20); /*初めの挨拶*/ //pc.printf("POWER ON\r\n"); twe.printf("POWER ON\r\n"); /*入力待ち*/ while(1){ char cha = twe.getc(); if(_input(cha)==-1){ timer_log.start(); tic_log.attach(&_log, 1.0/RATE_LOG); break; } } /*フライト準備*/ Phase = SETUP; tic_open.attach(&_open, 1.0/RATE_OPEN); /*GPS*/ while(1){ /*GPS取得*/ if(Phase != RISE){ if(gps.readable()){ gps_data[cnt_gps] = gps.getc(); if(gps_data[cnt_gps] == '$' || cnt_gps ==256){ cnt_gps = 0; memset(gps_data,'\0',256); }else if(gps_data[cnt_gps] == '\r'){ float world_time, lon_east, lat_north; int rlock, sat_num; char lat,lon; if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){ if(rlock==1){ lat_north = _DMS2DEG(lat_north); lon_east = _DMS2DEG(lon_east); twe.printf("%s\r\n",gps_data); twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num); }else{ twe.printf("%s\r\n",gps_data); } } }else{ cnt_gps++; } } } if(timer_log.read()>=30.0*60.0){ timer_log.reset(); /*なんかずっとこればっか書き込まれた 2018/7/1 fp=fopen("/sd/mydir/data.txt","w"); fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n"); fclose(fp); */ } } } void _open(){ led = Phase; float acc[3],acc_buf[10],acc_abs,time_acc,time_alt; switch(Phase){ case SETUP: //地上高度取得 //pc.printf("Hello,World!\r\n"); twe.printf("Hello,World!\r\n"); for(Cnt=0;Cnt<10;Cnt++){ Alt_buf[Cnt] = _getAlt(); } Alt_gnd = _median(Alt_buf,10); /*データをローカルファイルに保存*/ lfp=fopen("/local/data.txt", "a"); fprintf(lfp,"地上高度:%f\r\n",Alt_gnd); fclose(lfp); timer_open.start(); Phase = LAUNCH; //pc.printf("Phase = LAUNCH\r\n"); twe.printf("Phase = LAUNCH\r\n"); break; case LAUNCH: //発射判定 for(Cnt=0;Cnt<10;Cnt++){ mpu.getAccelero(acc); acc_buf[Cnt] = sqrt(pow(acc[0]/9.81,2)+pow(acc[1]/9.81,2)+pow(acc[2]/9.81,2)); } acc_abs = _median(acc_buf,10); //pc.printf("acc:%f\r\n",acc_abs); //pc.printf("cnt:%d\r\n",Cnt_acc); //加速度判定 if(acc_abs>ACC_JUDGE_LAUNCH){ Cnt_acc++; time_acc = timer_open.read(); } if(timer_open.read()-time_acc > TIME_JUDGE_CNT) Cnt_acc = 0; if(Cnt_acc == CNT_JUDGE_LAUNCH){ //pc.printf("Phase = WAIT\r\n"); twe.printf("LAUNCHED\r\n"); timer_open.reset(); Phase = WAIT; } break; case WAIT: //待機時間 if(timer_open.read()>TIME_GAP) Phase = RISE; break; case RISE: //頂点判定 for(Cnt=0;Cnt<5;Cnt++){ Alt_buf[Cnt] = _getAlt(); //pc.printf("alt:%f\r\n",Alt_buf[Cnt]); } Alt_md = _median(Alt_buf,5); twe.printf("alt:%f\r\n",Alt_md); //twe.printf("cnt:%d\r\n",Cnt_alt); if(Alt_md > Alt_max){ Alt_max = Alt_md; Cnt_alt = 0; } else if((Alt_max-Alt_md) > ALT_JUDGE_OPEN){ Cnt_alt++; Time_alt = timer_open.read(); } if((timer_open.read()-Time_alt) > TIME_JUDGE_CNT){ Cnt_alt = 0; } if(/*(*/Cnt_alt == CNT_JUDGE_OPEN/*)||(timer_open.read() > TIME_JUDGE_OPEN)*/){ //pc.printf("Phase = DROP1\r\n"); twe.printf("OPEN PARA\r\n"); Cnt_alt = 0; /*ローカルファイルに最高高度を記録*/ lfp = fopen("/local/data.txt", "a"); fprintf(lfp,"最高高度:%f\r\n",Alt_max); fclose(lfp); servo1.pulsewidth(0.0006); //TODO:サーボホーンの角度調整 servo2.pulsewidth(0.0006); //TODO:サーボホーンの角度調整 Phase = DROP1; } break; case DROP1: //放出機構作動? for(Cnt=0;Cnt<5;Cnt++){ Alt_buf[Cnt] = _getAlt(); } Alt_md = _median(Alt_buf,5); twe.printf("Alt:%f\r\n",Alt_md); //twe.printf("Cnt:%d\r\n",Cnt_alt); if((Alt_md-Alt_gnd) > ALT_JUDGE_OUT){ Cnt_alt++; } if((Cnt_alt == CNT_JUDGE_OUT)||(timer_open.read() > TIME_JUDGE_OUT)){ //地上高度からの高度にする?到達高度低いかも? //pc.printf("Phase = DROP2\r\n"); twe.printf("GO\r\n"); servo3.pulsewidth(0.0006); //TODO:サーボホーンの角度調整 servo4.pulsewidth(0.0006); //TODO:サーボホーンの角度調整 Phase = DROP2; } break; case DROP2: //着水検知.サーボの電源落とす. //twe.printf("cnt:%d\r\n",Cnt_alt); wait(3.0); sw = 0; //pc.printf("Goodbye Servo\r\n"); twe.printf("FINISH\r\n"); Phase=SEA; break; } } void _log(){ Data[Row][Cnt_data][0]=Phase; Data[Row][Cnt_data][1]=timer_log.read(); mpu.getAccelero(&Data[Row][Cnt_data][2]); bmp.ReadData(&Data[Row][Cnt_data][5],&Data[Row][Cnt_data][6]); Data[Row][Cnt_data][7] = (pow((p0/Data[Row][Cnt_data][6]), (1.0f/5.257f))-1.0f)*(Data[Row][Cnt_data][5]+273.15f)/0.0065f; Cnt_data++; //pc.printf("Cnt_data:%d\r\n",Cnt_data); if(Cnt_data==NUM_DATA){ Cnt_data = 0; Row =! Row; fp = fopen("/sd/mydir/data.txt","a"); for(int i=0;i<NUM_DATA;i++){ fprintf(fp,"%2.0f,%f,%f,%f,%f,%f,%f,%f\r\n",Data[!Row][i][0],Data[!Row][i][1],Data[!Row][i][2],Data[!Row][i][3],Data[!Row][i][4],Data[!Row][i][5],Data[!Row][i][6],Data[!Row][i][7]); } fclose(fp); } } float _getAlt(){ float altitude,pressure,temperature; bmp.ReadData(&temperature,&pressure); altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f; return altitude; } int _input(char cha){ twe.printf("%c\r\n",cha); if(cha!='\0'){ // pc.printf("%c\r\n",c); } if(cha=='F'){ twe.printf("flight mode on\r\n"); return -1; }else if(cha=='u'){ //開放機構のUNLOCK servo1.pulsewidth(0.0006); servo2.pulsewidth(0.0006); }else if(cha=='l'){ //開放機構のLOCK servo1.pulsewidth(0.0024); servo2.pulsewidth(0.0024); }else if(cha=='a'){ //収穫機構のUNLOCK(AKERU) servo3.pulsewidth(0.0010); servo4.pulsewidth(0.0010); }else if(cha=='s'){ //収穫機構のLOCK(SHIMERU) servo3.pulsewidth(0.0024); servo4.pulsewidth(0.0024); } return 0; } float _DMS2DEG(float raw_data){ int d=(int)(raw_data/100); float m=(raw_data-(float)d*100); return (float)d+m/60; } float _median(float data[], int num){ float *data_cpy, ans; data_cpy = new float[num]; memcpy(data_cpy,data,sizeof(float)*num); for(int i=0; i<num; i++){ for(int j=0; j<num-i-1; j++){ if(data_cpy[j]>data_cpy[j+1]){ float buff = data_cpy[j+1]; data_cpy[j+1] = data_cpy[j]; data_cpy[j] = buff; } } } if(num%2!=0) ans = data_cpy[num/2]; else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; delete[] data_cpy; return ans; }