2017.11伊豆大島共同打ち上げ実験の開放用プログラム
Dependencies: BMP180 MPU6050 mbed
Fork of 2017_11_1114 by
Diff: main.cpp
- Revision:
- 2:6238109d564d
- Parent:
- 1:b9ea35d93329
- Child:
- 3:217b56515009
diff -r b9ea35d93329 -r 6238109d564d main.cpp --- a/main.cpp Mon Oct 10 11:21:32 2016 +0000 +++ b/main.cpp Fri Oct 20 03:49:28 2017 +0000 @@ -1,47 +1,152 @@ -/* -説明 -Nucleo-F303K8とBMP180を使った気温・気圧・高度計算のサンプルプログラム - -ライブラリ -https://developer.mbed.org/users/spiridion/code/BMP180/ - -以下ピン配置 -Nucleo BMP180 -GND-----GND-------0V -+3V3----VIN -D4------SDA -D5------SCL - -*/ #include "mbed.h" #include "math.h" +#include "MPU6050.h" #include "BMP180.h" -#define p0 1013.25f//海面気圧 + +#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); -DigitalOut myled(LED1); -Serial pc(USBTX,USBRX); -BMP180 bmp(PB_7, PB_6); -Timer timer; +/* 自作関数 */ +void _flight(); +void _open(); +void _close(); +void _leaf_lock(); +void _leaf_unlock(); +float _getAlt(); +float _getAcc(); +float _median(float data[], int num); -float getAlt(float press, float temp); +/* グローバル変数 */ +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() { - float pressure,temperature,altitude; - float time; + 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); - pc.printf("time, temperature ,pressure, altitude\r\n"); - timer.start(); + _flight(); + while(1); +} + + +/*フライトモード用関数 */ +void _flight(){ - while(1) { - bmp.ReadData(&temperature,&pressure); - altitude = getAlt(pressure,temperature); - time = timer.read(); - pc.printf("%f, %f, %f, %f \r\n",time, temperature, pressure, altitude); - myled =! myled; - wait(1); + /* 地上高度取得 */ + 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; + } } } -float getAlt(float press, float temp){ - return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; -} \ No newline at end of file + +/* サーボ用関数 */ +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; +}