2017.11伊豆大島共同打ち上げ実験の開放用プログラム
Dependencies: BMP180 MPU6050 mbed
Fork of 2017_11_1114 by
main.cpp
- Committer:
- oichan
- Date:
- 2017-11-10
- Revision:
- 8:b10169df8f5f
- Parent:
- 7:4efaca1e9acd
- Child:
- 9:5eda5aad3916
File content as of revision 8:b10169df8f5f:
#include "mbed.h" #include "math.h" #include "MPU6050.h" #include "BMP180.h" #define LAUNCH_JUDGE_ACC_g 3.0 #define PARA_OPEN_TIME_s 20.0 #define PARA_OPEN_CNT 7 #define PARA_OPEN_FILTER_s 8.0 #define LEAF_UNLOCK_TIME_s 85.0 #define LEAF_UNLOCK_ALT_m 300.0 #define p0 1013.25f BMP180 bmp(dp5,dp27); MPU6050 mpu(dp5,dp27); Timer para_timer; Timer leaf_timer; Ticker readtimer; PwmOut mg996(dp1); PwmOut sg92(dp18); DigitalIn oshirase1(dp11); DigitalInOut oshirase2(dp10); //DigitalOut myled(LED1); //Serial pc(USBTX,USBRX); /* 自作関数 */ void _flight(); void _open(); void _close(); void _leaf_lock(); void _leaf_unlock(); float _getAlt(); float _getAcc(); float _median(float data[], int num); /* グローバル変数 */ int Alt_cnt; float a[3],Acc; float Pressure,Temperature,Altitude; float Alt_buff[10],Alt_median_buff[10],Acc_buff[10]; float Land_Alt; bool Mg996Open = true; bool Sg92Open = true; int main() { // pc.printf("Hello!\r\n"); mg996.period_ms(20); sg92.period_ms(20); mpu.setAcceleroRange(0); bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); oshirase2.input(); wait(0.5); while(1){ if(oshirase1==0 && oshirase2==1){ _flight(); break; }else if(oshirase1==1 && oshirase2==0){ while(1){ if(oshirase1==1 && oshirase2==1){ if(Mg996Open){ _close(); Mg996Open = false; }else{ _open(); Mg996Open = true; } break; }else if(oshirase1==0 && oshirase2==0){ if(Sg92Open){ _leaf_lock(); Sg92Open = false; }else{ _leaf_unlock(); Sg92Open = true; } break; } } } } while(1); } /* フライトモード用関数 */ void _flight(){ // pc.printf("flight mode on\r\n"); oshirase2.output(); oshirase2 = 0; _close(); _leaf_lock(); /* 地上高度取得 */ for(int i=0;i<10;i++){ Alt_buff[i] = _getAlt(); } Land_Alt = _median(Alt_buff,10); // pc.printf("Land_Alt = %f\r\n",Land_Alt); // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); /* 発射判定 */ while(1){ // myled = !myled; for(int i=0;i<10;i++){ Acc_buff[i] = _getAcc(); } Acc = _median(Acc_buff,10); if(Acc >= LAUNCH_JUDGE_ACC_g){ para_timer.start(); oshirase2 = 1; // for(int i=0;i<10;i++)pc.printf("%f\n\r",Acc_buff[i]); // pc.printf("Acc = %f\r\n",Acc); break; } } /* 開放判定 */ while(1){ Alt_cnt = 0; for(int i=0;i<10;i++){ Alt_buff[i] = _getAlt(); } Alt_median_buff[0] = _median(Alt_buff,10); for(int i=1;i<10;i++){ for(int j=0;j<10;j++){ Alt_buff[j] = _getAlt(); } Alt_median_buff[i] = _median(Alt_buff,10); // pc.printf("%f\r\n",Alt_median_buff[i]); if(Alt_median_buff[i] < Alt_median_buff[i-1]-0.1) Alt_cnt++; } // pc.printf("%d\r\n",Alt_cnt); if(para_timer.read() >= PARA_OPEN_FILTER_s){ if(Alt_cnt >= PARA_OPEN_CNT || para_timer.read() >= PARA_OPEN_TIME_s){ _open(); leaf_timer.start(); oshirase2 = 0; para_timer.stop(); // pc.printf("para open\r\n"); // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); break; } } } /* リーフィング解除判定 */ while(1){ for(int i=0;i<10;i++){ Alt_buff[i] = _getAlt(); } Altitude = _median(Alt_buff,10); if(Altitude - Land_Alt <= LEAF_UNLOCK_ALT_m || leaf_timer.read() >= LEAF_UNLOCK_TIME_s){ _leaf_unlock(); oshirase2 = 1; leaf_timer.stop(); // pc.printf("leafing unlock\r\n"); // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); break; } } } /* サーボ用関数 */ void _close() {mg996.pulsewidth(0.0005);} void _open() {mg996.pulsewidth(0.0024);} void _leaf_lock() {sg92.pulsewidth(0.0024);} void _leaf_unlock() {sg92.pulsewidth(0.0005);} /* 高度取得関数 */ float _getAlt(){ float altitude; bmp.ReadData(&Temperature,&Pressure); altitude = (pow((p0/Pressure), (1.0f/5.257f))-1.0f)*(Temperature+273.15f)/0.0065f; // pc.printf("%f, %f, %f\r\n",Pressure,Temperature,altitude); return altitude; } /* 合成加速度取得関数 */ float _getAcc(){ mpu.getAccelero(a); return sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2)); } /* 中央値計算関数 */ float _median(float data[], int num){//todo:処理時間計測 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; }