2017.11伊豆大島共同打ち上げ実験の開放用プログラム
Dependencies: BMP180 MPU6050 mbed
Fork of 2017_11_1114 by
main.cpp
- Committer:
- oichan
- Date:
- 2017-10-22
- Revision:
- 5:a5cf6914e7c4
- Parent:
- 4:ae20e73f4924
- Child:
- 6:e0c25c3976ae
File content as of revision 5:a5cf6914e7c4:
#include "mbed.h" #include "math.h" #include "MPU6050.h" #include "BMP180.h" #define LAUNCH_JUDGE_ACC_g 3.0 #define PARA_OPEN_TIME_s 10.0 #define PARA_OPEN_FILTER_s 6.0 #define LEAF_UNLOCK_TIME_s 10.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(dp18); PwmOut sg92(dp1); 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],Acc_buff[10]; float Land_Alt; bool Mg996Open = true; bool Sg92Open = true; int main() { // pc.printf("Hello!"); mg996.period_ms(20); sg92.period_ms(20); _close(); _leaf_lock(); 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(){ oshirase2.output(); oshirase2 = 0; /* 地上高度取得 */ for(int i=0;i<10;i++){ Alt_buff[i] = _getAlt(); } Land_Alt = _median(Alt_buff,10); // 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 = _getAcc(); Acc_buff[i] = Acc; } 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]); break; } } /* 開放判定 */ while(1){ Alt_cnt = 0; Alt_buff[0] = _getAlt(); for(int i=1;i<10;i++){ Alt_buff[i] = _getAlt(); if(Alt_buff[i] < Alt_buff[i-1]) Alt_cnt++; } if(para_timer.read() >= PARA_OPEN_FILTER_s){ if(Alt_cnt > 6 || para_timer.read() >= PARA_OPEN_TIME_s){ _open(); leaf_timer.start(); oshirase2 = 0; para_timer.stop(); // 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 <= LEAF_UNLOCK_ALT_m || leaf_timer.read() >= LEAF_UNLOCK_TIME_s){//TODO:現在の高度から地上高度を引く _leaf_unlock(); leaf_timer.stop(); // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); break; } } } /* サーボ用関数 */ void _close() {mg996.pulsewidth(0.0006);} void _open() {mg996.pulsewidth(0.0023);} void _leaf_lock() {sg92.pulsewidth(0.0005);} void _leaf_unlock() {sg92.pulsewidth(0.0023);} /* 高度取得関数 */ float _getAlt(){ bmp.ReadData(&Temperature,&Pressure); return (pow((p0/Pressure), (1.0f/5.257f))-1.0f)*(Temperature+273.15f)/0.0065f; } /* 加速度取得関数 */ 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; }