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