2017.11伊豆大島共同打ち上げ実験の開放用プログラム
Dependencies: BMP180 MPU6050 mbed
Fork of Sample_BMP180 by
main.cpp@4:ae20e73f4924, 2017-10-22 (annotated)
- Committer:
- oichan
- Date:
- Sun Oct 22 04:36:54 2017 +0000
- Revision:
- 4:ae20e73f4924
- Parent:
- 3:217b56515009
- Child:
- 5:a5cf6914e7c4
a little change
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 | 4:ae20e73f4924 | 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 | 4:ae20e73f4924 | 12 | BMP180 bmp(dp5,dp27); |
oichan | 4:ae20e73f4924 | 13 | MPU6050 mpu(dp5,dp27); |
oichan | 4:ae20e73f4924 | 14 | Timer para_timer; |
oichan | 4:ae20e73f4924 | 15 | Timer leaf_timer; |
oichan | 4:ae20e73f4924 | 16 | Ticker readtimer; |
oichan | 4:ae20e73f4924 | 17 | PwmOut mg996(dp1); |
oichan | 4:ae20e73f4924 | 18 | PwmOut sg92(dp2); |
oichan | 4:ae20e73f4924 | 19 | DigitalIn oshirase1(dp11); |
oichan | 4:ae20e73f4924 | 20 | DigitalInOut oshirase2(dp10); |
oichan | 4:ae20e73f4924 | 21 | DigitalOut myled(LED1); |
oichan | 4:ae20e73f4924 | 22 | //Serial pc(USBTX,USBRX); |
mikawataru | 0:0ff20d8e9090 | 23 | |
oichan | 2:6238109d564d | 24 | /* 自作関数 */ |
oichan | 2:6238109d564d | 25 | void _flight(); |
oichan | 2:6238109d564d | 26 | void _open(); |
oichan | 2:6238109d564d | 27 | void _close(); |
oichan | 2:6238109d564d | 28 | void _leaf_lock(); |
oichan | 2:6238109d564d | 29 | void _leaf_unlock(); |
oichan | 2:6238109d564d | 30 | float _getAlt(); |
oichan | 2:6238109d564d | 31 | float _getAcc(); |
oichan | 2:6238109d564d | 32 | float _median(float data[], int num); |
mikawataru | 0:0ff20d8e9090 | 33 | |
oichan | 2:6238109d564d | 34 | /* グローバル変数 */ |
oichan | 4:ae20e73f4924 | 35 | int Alt_cnt; |
oichan | 4:ae20e73f4924 | 36 | float a[3],Acc; |
oichan | 4:ae20e73f4924 | 37 | float Pressure,Temperature,Altitude; |
oichan | 4:ae20e73f4924 | 38 | float Alt_buff[10],Acc_buff[10]; |
oichan | 4:ae20e73f4924 | 39 | float Land_Alt; |
oichan | 4:ae20e73f4924 | 40 | bool Mg996Open = true; |
oichan | 4:ae20e73f4924 | 41 | bool Sg92Open = true; |
oichan | 2:6238109d564d | 42 | |
mikawataru | 0:0ff20d8e9090 | 43 | |
mikawataru | 0:0ff20d8e9090 | 44 | int main() { |
oichan | 3:217b56515009 | 45 | // pc.printf("Hello!"); |
oichan | 3:217b56515009 | 46 | mg996.period_ms(20); |
oichan | 2:6238109d564d | 47 | sg92.period_ms(20); |
oichan | 2:6238109d564d | 48 | _close(); |
oichan | 2:6238109d564d | 49 | _leaf_lock(); |
oichan | 2:6238109d564d | 50 | mpu.setAcceleroRange(0); |
mikawataru | 0:0ff20d8e9090 | 51 | bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); |
oichan | 4:ae20e73f4924 | 52 | oshirase2.input(); |
oichan | 4:ae20e73f4924 | 53 | while(1){ |
oichan | 4:ae20e73f4924 | 54 | if(oshirase1==0 && oshirase2==1){ |
oichan | 4:ae20e73f4924 | 55 | _flight(); |
oichan | 4:ae20e73f4924 | 56 | break; |
oichan | 4:ae20e73f4924 | 57 | }else if(oshirase1==1 && oshirase2==0){ |
oichan | 4:ae20e73f4924 | 58 | while(1){ |
oichan | 4:ae20e73f4924 | 59 | if(oshirase1==1 && oshirase2==1){ |
oichan | 4:ae20e73f4924 | 60 | if(Mg996Open){ |
oichan | 4:ae20e73f4924 | 61 | _close(); |
oichan | 4:ae20e73f4924 | 62 | Mg996Open = false; |
oichan | 4:ae20e73f4924 | 63 | }else{ |
oichan | 4:ae20e73f4924 | 64 | _open(); |
oichan | 4:ae20e73f4924 | 65 | Mg996Open = true; |
oichan | 4:ae20e73f4924 | 66 | } |
oichan | 4:ae20e73f4924 | 67 | break; |
oichan | 4:ae20e73f4924 | 68 | }else if(oshirase1==0 && oshirase2==0){ |
oichan | 4:ae20e73f4924 | 69 | if(Sg92Open){ |
oichan | 4:ae20e73f4924 | 70 | _leaf_lock(); |
oichan | 4:ae20e73f4924 | 71 | Sg92Open = false; |
oichan | 4:ae20e73f4924 | 72 | }else{ |
oichan | 4:ae20e73f4924 | 73 | _leaf_unlock(); |
oichan | 4:ae20e73f4924 | 74 | Sg92Open = true; |
oichan | 4:ae20e73f4924 | 75 | } |
oichan | 4:ae20e73f4924 | 76 | break; |
oichan | 4:ae20e73f4924 | 77 | } |
oichan | 4:ae20e73f4924 | 78 | } |
oichan | 4:ae20e73f4924 | 79 | } |
oichan | 4:ae20e73f4924 | 80 | } |
oichan | 2:6238109d564d | 81 | while(1); |
oichan | 2:6238109d564d | 82 | } |
oichan | 2:6238109d564d | 83 | |
oichan | 2:6238109d564d | 84 | |
oichan | 3:217b56515009 | 85 | /* フライトモード用関数 */ |
oichan | 2:6238109d564d | 86 | void _flight(){ |
oichan | 4:ae20e73f4924 | 87 | oshirase2.output(); |
oichan | 4:ae20e73f4924 | 88 | oshirase2 = 0; |
oichan | 4:ae20e73f4924 | 89 | |
oichan | 2:6238109d564d | 90 | /* 地上高度取得 */ |
oichan | 2:6238109d564d | 91 | for(int i=0;i<10;i++){ |
oichan | 2:6238109d564d | 92 | Alt_buff[i] = _getAlt(); |
oichan | 2:6238109d564d | 93 | } |
oichan | 4:ae20e73f4924 | 94 | Land_Alt = _median(Alt_buff,10); |
oichan | 3:217b56515009 | 95 | // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); |
oichan | 2:6238109d564d | 96 | |
oichan | 2:6238109d564d | 97 | /* 発射判定 */ |
oichan | 2:6238109d564d | 98 | while(1){ |
oichan | 3:217b56515009 | 99 | myled = !myled; |
oichan | 2:6238109d564d | 100 | for(int i=0;i<10;i++){ |
oichan | 4:ae20e73f4924 | 101 | Acc = _getAcc(); |
oichan | 4:ae20e73f4924 | 102 | Acc_buff[i] = Acc; |
oichan | 2:6238109d564d | 103 | } |
oichan | 4:ae20e73f4924 | 104 | Acc = _median(Acc_buff,10); |
oichan | 4:ae20e73f4924 | 105 | if(Acc >= LAUNCH_JUDGE_ACC_g){ |
oichan | 2:6238109d564d | 106 | para_timer.start(); |
oichan | 4:ae20e73f4924 | 107 | oshirase2 = 1; |
oichan | 3:217b56515009 | 108 | // for(int i=0;i<10;i++)pc.printf("%f\n\r",Acc_buff[i]); |
oichan | 2:6238109d564d | 109 | break; |
oichan | 2:6238109d564d | 110 | } |
oichan | 2:6238109d564d | 111 | } |
oichan | 2:6238109d564d | 112 | |
oichan | 2:6238109d564d | 113 | /* 開放判定 */ |
oichan | 2:6238109d564d | 114 | while(1){ |
oichan | 2:6238109d564d | 115 | Alt_cnt = 0; |
oichan | 2:6238109d564d | 116 | Alt_buff[0] = _getAlt(); |
oichan | 2:6238109d564d | 117 | for(int i=1;i<10;i++){ |
oichan | 2:6238109d564d | 118 | Alt_buff[i] = _getAlt(); |
oichan | 2:6238109d564d | 119 | if(Alt_buff[i] < Alt_buff[i-1]) Alt_cnt++; |
oichan | 2:6238109d564d | 120 | } |
oichan | 2:6238109d564d | 121 | if(Alt_cnt > 6 || para_timer.read() >= PARA_OPEN_TIME_s){ |
oichan | 2:6238109d564d | 122 | _open(); |
oichan | 2:6238109d564d | 123 | leaf_timer.start(); |
oichan | 4:ae20e73f4924 | 124 | oshirase2 = 0; |
oichan | 2:6238109d564d | 125 | para_timer.stop(); |
oichan | 3:217b56515009 | 126 | // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); |
oichan | 2:6238109d564d | 127 | break; |
oichan | 2:6238109d564d | 128 | } |
oichan | 2:6238109d564d | 129 | } |
oichan | 2:6238109d564d | 130 | |
oichan | 2:6238109d564d | 131 | /* リーフィング解除判定 */ |
oichan | 2:6238109d564d | 132 | while(1){ |
oichan | 2:6238109d564d | 133 | for(int i=0;i<10;i++){ |
oichan | 2:6238109d564d | 134 | Alt_buff[i] = _getAlt(); |
oichan | 2:6238109d564d | 135 | } |
oichan | 4:ae20e73f4924 | 136 | Altitude = _median(Alt_buff,10); |
oichan | 4:ae20e73f4924 | 137 | if(Altitude <= LEAF_UNLOCK_ALT_m || leaf_timer.read() >= LEAF_UNLOCK_TIME_s){//TODO:現在の高度から地上高度を引く |
oichan | 2:6238109d564d | 138 | _leaf_unlock(); |
oichan | 2:6238109d564d | 139 | leaf_timer.stop(); |
oichan | 3:217b56515009 | 140 | // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); |
oichan | 2:6238109d564d | 141 | break; |
oichan | 2:6238109d564d | 142 | } |
mikawataru | 0:0ff20d8e9090 | 143 | } |
mikawataru | 0:0ff20d8e9090 | 144 | } |
mikawataru | 0:0ff20d8e9090 | 145 | |
oichan | 2:6238109d564d | 146 | |
oichan | 2:6238109d564d | 147 | /* サーボ用関数 */ |
oichan | 3:217b56515009 | 148 | void _close() {mg996.pulsewidth(0.0006);} |
oichan | 3:217b56515009 | 149 | void _open() {mg996.pulsewidth(0.0023);} |
oichan | 2:6238109d564d | 150 | void _leaf_lock() {sg92.pulsewidth(0.0005);} |
oichan | 2:6238109d564d | 151 | void _leaf_unlock() {sg92.pulsewidth(0.0023);} |
oichan | 2:6238109d564d | 152 | |
oichan | 2:6238109d564d | 153 | |
oichan | 2:6238109d564d | 154 | /* 高度取得関数 */ |
oichan | 2:6238109d564d | 155 | float _getAlt(){ |
oichan | 4:ae20e73f4924 | 156 | bmp.ReadData(&Temperature,&Pressure); |
oichan | 4:ae20e73f4924 | 157 | return (pow((p0/Pressure), (1.0f/5.257f))-1.0f)*(Temperature+273.15f)/0.0065f; |
oichan | 2:6238109d564d | 158 | } |
oichan | 2:6238109d564d | 159 | |
oichan | 2:6238109d564d | 160 | |
oichan | 2:6238109d564d | 161 | /* 加速度取得関数 */ |
oichan | 2:6238109d564d | 162 | float _getAcc(){ |
oichan | 2:6238109d564d | 163 | mpu.getAccelero(a); |
oichan | 2:6238109d564d | 164 | return sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2)); |
oichan | 2:6238109d564d | 165 | } |
oichan | 2:6238109d564d | 166 | |
oichan | 2:6238109d564d | 167 | |
oichan | 2:6238109d564d | 168 | /* 中央値計算関数 */ |
oichan | 2:6238109d564d | 169 | float _median(float data[], int num){//todo:処理時間計測 |
oichan | 2:6238109d564d | 170 | float *data_cpy, ans; |
oichan | 2:6238109d564d | 171 | data_cpy = new float[num]; |
oichan | 2:6238109d564d | 172 | memcpy(data_cpy,data,sizeof(float)*num); |
oichan | 2:6238109d564d | 173 | |
oichan | 2:6238109d564d | 174 | for(int i=0; i<num; i++){ |
oichan | 2:6238109d564d | 175 | for(int j=0; j<num-i-1; j++){ |
oichan | 2:6238109d564d | 176 | if(data_cpy[j]>data_cpy[j+1]){ |
oichan | 2:6238109d564d | 177 | float buff = data_cpy[j+1]; |
oichan | 2:6238109d564d | 178 | data_cpy[j+1] = data_cpy[j]; |
oichan | 2:6238109d564d | 179 | data_cpy[j] = buff; |
oichan | 2:6238109d564d | 180 | } |
oichan | 2:6238109d564d | 181 | } |
oichan | 2:6238109d564d | 182 | } |
oichan | 2:6238109d564d | 183 | |
oichan | 2:6238109d564d | 184 | if(num%2!=0) ans = data_cpy[num/2]; |
oichan | 2:6238109d564d | 185 | else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; |
oichan | 2:6238109d564d | 186 | delete[] data_cpy; |
oichan | 2:6238109d564d | 187 | return ans; |
oichan | 2:6238109d564d | 188 | } |