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