2017.11伊豆大島共同打ち上げ実験の開放用プログラム
Dependencies: BMP180 MPU6050 mbed
Fork of 2017_11_1114 by
main.cpp@2:6238109d564d, 2017-10-20 (annotated)
- Committer:
- oichan
- Date:
- Fri Oct 20 03:49:28 2017 +0000
- Revision:
- 2:6238109d564d
- Parent:
- 1:b9ea35d93329
- Child:
- 3:217b56515009
2017.11?????????????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mikawataru | 0:0ff20d8e9090 | 1 | #include "mbed.h" |
mikawataru | 0:0ff20d8e9090 | 2 | #include "math.h" |
oichan | 2:6238109d564d | 3 | #include "MPU6050.h" |
mikawataru | 0:0ff20d8e9090 | 4 | #include "BMP180.h" |
oichan | 2:6238109d564d | 5 | |
oichan | 2:6238109d564d | 6 | #define LAUNCH_JUDGE_ACC_g 3.0 |
oichan | 2:6238109d564d | 7 | #define PARA_OPEN_TIME_s 5.0 |
oichan | 2:6238109d564d | 8 | #define LEAF_UNLOCK_TIME_s 5.0 |
oichan | 2:6238109d564d | 9 | #define LEAF_UNLOCK_ALT_m 300.0 |
oichan | 2:6238109d564d | 10 | #define p0 1013.25f |
oichan | 2:6238109d564d | 11 | |
oichan | 2:6238109d564d | 12 | BMP180 bmp(dp5,dp27); |
oichan | 2:6238109d564d | 13 | MPU6050 mpu(dp5,dp27); |
oichan | 2:6238109d564d | 14 | Timer para_timer; |
oichan | 2:6238109d564d | 15 | Timer leaf_timer; |
oichan | 2:6238109d564d | 16 | Ticker readtimer; |
oichan | 2:6238109d564d | 17 | PwmOut mg995(dp1); |
oichan | 2:6238109d564d | 18 | PwmOut sg92(dp18); |
oichan | 2:6238109d564d | 19 | DigitalOut myled(LED1); |
oichan | 2:6238109d564d | 20 | Serial pc(USBTX,USBRX); |
mikawataru | 0:0ff20d8e9090 | 21 | |
oichan | 2:6238109d564d | 22 | /* 自作関数 */ |
oichan | 2:6238109d564d | 23 | void _flight(); |
oichan | 2:6238109d564d | 24 | void _open(); |
oichan | 2:6238109d564d | 25 | void _close(); |
oichan | 2:6238109d564d | 26 | void _leaf_lock(); |
oichan | 2:6238109d564d | 27 | void _leaf_unlock(); |
oichan | 2:6238109d564d | 28 | float _getAlt(); |
oichan | 2:6238109d564d | 29 | float _getAcc(); |
oichan | 2:6238109d564d | 30 | float _median(float data[], int num); |
mikawataru | 0:0ff20d8e9090 | 31 | |
oichan | 2:6238109d564d | 32 | /* グローバル変数 */ |
oichan | 2:6238109d564d | 33 | int Alt_cnt; |
oichan | 2:6238109d564d | 34 | float a[3],acc; |
oichan | 2:6238109d564d | 35 | float pressure,temperature,altitude; |
oichan | 2:6238109d564d | 36 | float Alt_buff[10],Acc_buff[10]; |
oichan | 2:6238109d564d | 37 | float land_alt; |
oichan | 2:6238109d564d | 38 | float t; |
oichan | 2:6238109d564d | 39 | |
mikawataru | 0:0ff20d8e9090 | 40 | |
mikawataru | 0:0ff20d8e9090 | 41 | int main() { |
oichan | 2:6238109d564d | 42 | pc.printf("Hello!"); |
oichan | 2:6238109d564d | 43 | mg995.period_ms(20); |
oichan | 2:6238109d564d | 44 | sg92.period_ms(20); |
oichan | 2:6238109d564d | 45 | _close(); |
oichan | 2:6238109d564d | 46 | _leaf_lock(); |
oichan | 2:6238109d564d | 47 | mpu.setAcceleroRange(0); |
mikawataru | 0:0ff20d8e9090 | 48 | bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); |
oichan | 2:6238109d564d | 49 | _flight(); |
oichan | 2:6238109d564d | 50 | while(1); |
oichan | 2:6238109d564d | 51 | } |
oichan | 2:6238109d564d | 52 | |
oichan | 2:6238109d564d | 53 | |
oichan | 2:6238109d564d | 54 | /*フライトモード用関数 */ |
oichan | 2:6238109d564d | 55 | void _flight(){ |
mikawataru | 0:0ff20d8e9090 | 56 | |
oichan | 2:6238109d564d | 57 | /* 地上高度取得 */ |
oichan | 2:6238109d564d | 58 | for(int i=0;i<10;i++){ |
oichan | 2:6238109d564d | 59 | Alt_buff[i] = _getAlt(); |
oichan | 2:6238109d564d | 60 | } |
oichan | 2:6238109d564d | 61 | land_alt = _median(Alt_buff,10); |
oichan | 2:6238109d564d | 62 | for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); |
oichan | 2:6238109d564d | 63 | |
oichan | 2:6238109d564d | 64 | /* 発射判定 */ |
oichan | 2:6238109d564d | 65 | while(1){ |
oichan | 2:6238109d564d | 66 | for(int i=0;i<10;i++){ |
oichan | 2:6238109d564d | 67 | acc = _getAcc(); |
oichan | 2:6238109d564d | 68 | Acc_buff[i] = acc; |
oichan | 2:6238109d564d | 69 | } |
oichan | 2:6238109d564d | 70 | acc = _median(Acc_buff,10); |
oichan | 2:6238109d564d | 71 | if(acc >= LAUNCH_JUDGE_ACC_g){ |
oichan | 2:6238109d564d | 72 | para_timer.start(); |
oichan | 2:6238109d564d | 73 | for(int i=0;i<10;i++)pc.printf("%f\n\r",Acc_buff[i]); |
oichan | 2:6238109d564d | 74 | break; |
oichan | 2:6238109d564d | 75 | } |
oichan | 2:6238109d564d | 76 | } |
oichan | 2:6238109d564d | 77 | |
oichan | 2:6238109d564d | 78 | /* 開放判定 */ |
oichan | 2:6238109d564d | 79 | while(1){ |
oichan | 2:6238109d564d | 80 | Alt_cnt = 0; |
oichan | 2:6238109d564d | 81 | Alt_buff[0] = _getAlt(); |
oichan | 2:6238109d564d | 82 | for(int i=1;i<10;i++){ |
oichan | 2:6238109d564d | 83 | Alt_buff[i] = _getAlt(); |
oichan | 2:6238109d564d | 84 | if(Alt_buff[i] < Alt_buff[i-1]) Alt_cnt++; |
oichan | 2:6238109d564d | 85 | } |
oichan | 2:6238109d564d | 86 | if(Alt_cnt > 6 || para_timer.read() >= PARA_OPEN_TIME_s){ |
oichan | 2:6238109d564d | 87 | _open(); |
oichan | 2:6238109d564d | 88 | leaf_timer.start(); |
oichan | 2:6238109d564d | 89 | para_timer.stop(); |
oichan | 2:6238109d564d | 90 | for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); |
oichan | 2:6238109d564d | 91 | break; |
oichan | 2:6238109d564d | 92 | } |
oichan | 2:6238109d564d | 93 | } |
oichan | 2:6238109d564d | 94 | |
oichan | 2:6238109d564d | 95 | /* リーフィング解除判定 */ |
oichan | 2:6238109d564d | 96 | while(1){ |
oichan | 2:6238109d564d | 97 | for(int i=0;i<10;i++){ |
oichan | 2:6238109d564d | 98 | Alt_buff[i] = _getAlt(); |
oichan | 2:6238109d564d | 99 | } |
oichan | 2:6238109d564d | 100 | altitude = _median(Alt_buff,10); |
oichan | 2:6238109d564d | 101 | if(altitude <= LEAF_UNLOCK_ALT_m || leaf_timer.read() >= LEAF_UNLOCK_TIME_s){ |
oichan | 2:6238109d564d | 102 | _leaf_unlock(); |
oichan | 2:6238109d564d | 103 | leaf_timer.stop(); |
oichan | 2:6238109d564d | 104 | for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); |
oichan | 2:6238109d564d | 105 | break; |
oichan | 2:6238109d564d | 106 | } |
mikawataru | 0:0ff20d8e9090 | 107 | } |
mikawataru | 0:0ff20d8e9090 | 108 | } |
mikawataru | 0:0ff20d8e9090 | 109 | |
oichan | 2:6238109d564d | 110 | |
oichan | 2:6238109d564d | 111 | /* サーボ用関数 */ |
oichan | 2:6238109d564d | 112 | void _close() {mg995.pulsewidth(0.0006);} |
oichan | 2:6238109d564d | 113 | void _open() {mg995.pulsewidth(0.0023);} |
oichan | 2:6238109d564d | 114 | void _leaf_lock() {sg92.pulsewidth(0.0005);} |
oichan | 2:6238109d564d | 115 | void _leaf_unlock() {sg92.pulsewidth(0.0023);} |
oichan | 2:6238109d564d | 116 | |
oichan | 2:6238109d564d | 117 | |
oichan | 2:6238109d564d | 118 | /* 高度取得関数 */ |
oichan | 2:6238109d564d | 119 | float _getAlt(){ |
oichan | 2:6238109d564d | 120 | bmp.ReadData(&temperature,&pressure); |
oichan | 2:6238109d564d | 121 | return (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f; |
oichan | 2:6238109d564d | 122 | } |
oichan | 2:6238109d564d | 123 | |
oichan | 2:6238109d564d | 124 | |
oichan | 2:6238109d564d | 125 | /* 加速度取得関数 */ |
oichan | 2:6238109d564d | 126 | float _getAcc(){ |
oichan | 2:6238109d564d | 127 | mpu.getAccelero(a); |
oichan | 2:6238109d564d | 128 | return sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2)); |
oichan | 2:6238109d564d | 129 | } |
oichan | 2:6238109d564d | 130 | |
oichan | 2:6238109d564d | 131 | |
oichan | 2:6238109d564d | 132 | /* 中央値計算関数 */ |
oichan | 2:6238109d564d | 133 | float _median(float data[], int num){//todo:処理時間計測 |
oichan | 2:6238109d564d | 134 | float *data_cpy, ans; |
oichan | 2:6238109d564d | 135 | data_cpy = new float[num]; |
oichan | 2:6238109d564d | 136 | memcpy(data_cpy,data,sizeof(float)*num); |
oichan | 2:6238109d564d | 137 | |
oichan | 2:6238109d564d | 138 | for(int i=0; i<num; i++){ |
oichan | 2:6238109d564d | 139 | for(int j=0; j<num-i-1; j++){ |
oichan | 2:6238109d564d | 140 | if(data_cpy[j]>data_cpy[j+1]){ |
oichan | 2:6238109d564d | 141 | float buff = data_cpy[j+1]; |
oichan | 2:6238109d564d | 142 | data_cpy[j+1] = data_cpy[j]; |
oichan | 2:6238109d564d | 143 | data_cpy[j] = buff; |
oichan | 2:6238109d564d | 144 | } |
oichan | 2:6238109d564d | 145 | } |
oichan | 2:6238109d564d | 146 | } |
oichan | 2:6238109d564d | 147 | |
oichan | 2:6238109d564d | 148 | if(num%2!=0) ans = data_cpy[num/2]; |
oichan | 2:6238109d564d | 149 | else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; |
oichan | 2:6238109d564d | 150 | delete[] data_cpy; |
oichan | 2:6238109d564d | 151 | return ans; |
oichan | 2:6238109d564d | 152 | } |