2017.11伊豆大島共同打ち上げ実験の開放用プログラム
Dependencies: BMP180 MPU6050 mbed
Fork of Sample_BMP180 by
main.cpp
- Committer:
- oichan
- Date:
- 2017-10-20
- Revision:
- 2:6238109d564d
- Parent:
- 1:b9ea35d93329
- Child:
- 3:217b56515009
File content as of revision 2:6238109d564d:
#include "mbed.h" #include "math.h" #include "MPU6050.h" #include "BMP180.h" #define LAUNCH_JUDGE_ACC_g 3.0 #define PARA_OPEN_TIME_s 5.0 #define LEAF_UNLOCK_TIME_s 5.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 mg995(dp1); PwmOut sg92(dp18); 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; float t; int main() { pc.printf("Hello!"); mg995.period_ms(20); sg92.period_ms(20); _close(); _leaf_lock(); mpu.setAcceleroRange(0); bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); _flight(); while(1); } /*フライトモード用関数 */ void _flight(){ /* 地上高度取得 */ 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){ 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(); 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(Alt_cnt > 6 || para_timer.read() >= PARA_OPEN_TIME_s){ _open(); leaf_timer.start(); 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){ _leaf_unlock(); leaf_timer.stop(); for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); break; } } } /* サーボ用関数 */ void _close() {mg995.pulsewidth(0.0006);} void _open() {mg995.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; }