201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
main.cpp@2:a4895f7c3058, 2017-11-02 (annotated)
- Committer:
- sashida_h
- Date:
- Thu Nov 02 17:13:22 2017 +0000
- Revision:
- 2:a4895f7c3058
- Parent:
- 1:b9ea35d93329
201711?????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mikawataru | 1:b9ea35d93329 | 1 | /* |
mikawataru | 1:b9ea35d93329 | 2 | 説明 |
mikawataru | 1:b9ea35d93329 | 3 | Nucleo-F303K8とBMP180を使った気温・気圧・高度計算のサンプルプログラム |
mikawataru | 1:b9ea35d93329 | 4 | |
mikawataru | 1:b9ea35d93329 | 5 | ライブラリ |
mikawataru | 1:b9ea35d93329 | 6 | https://developer.mbed.org/users/spiridion/code/BMP180/ |
mikawataru | 1:b9ea35d93329 | 7 | |
mikawataru | 1:b9ea35d93329 | 8 | 以下ピン配置 |
mikawataru | 1:b9ea35d93329 | 9 | Nucleo BMP180 |
mikawataru | 1:b9ea35d93329 | 10 | GND-----GND-------0V |
mikawataru | 1:b9ea35d93329 | 11 | +3V3----VIN |
mikawataru | 1:b9ea35d93329 | 12 | D4------SDA |
mikawataru | 1:b9ea35d93329 | 13 | D5------SCL |
mikawataru | 1:b9ea35d93329 | 14 | |
mikawataru | 1:b9ea35d93329 | 15 | */ |
mikawataru | 0:0ff20d8e9090 | 16 | #include "mbed.h" |
sashida_h | 2:a4895f7c3058 | 17 | #include "MPU6050.h" |
mikawataru | 0:0ff20d8e9090 | 18 | #include "math.h" |
mikawataru | 0:0ff20d8e9090 | 19 | #include "BMP180.h" |
sashida_h | 2:a4895f7c3058 | 20 | |
sashida_h | 2:a4895f7c3058 | 21 | #define LUNCH_G 1.0 |
sashida_h | 2:a4895f7c3058 | 22 | #define TIMER1 500 |
sashida_h | 2:a4895f7c3058 | 23 | #define TIMER2 30 |
sashida_h | 2:a4895f7c3058 | 24 | #define ALT_LEAF 300 |
sashida_h | 2:a4895f7c3058 | 25 | #define UNLOCK 0 |
sashida_h | 2:a4895f7c3058 | 26 | #define LOCK 1 |
sashida_h | 2:a4895f7c3058 | 27 | #define RATE_OPEN 1 |
sashida_h | 2:a4895f7c3058 | 28 | |
sashida_h | 2:a4895f7c3058 | 29 | enum PHASE{LUNCH=1,RISE=3,DROP=7} Phase; |
sashida_h | 2:a4895f7c3058 | 30 | |
sashida_h | 2:a4895f7c3058 | 31 | DigitalIn pr(dp10); |
sashida_h | 2:a4895f7c3058 | 32 | DigitalInOut le(dp11); |
sashida_h | 2:a4895f7c3058 | 33 | MPU6050 mpu(dp5, dp27); |
sashida_h | 2:a4895f7c3058 | 34 | BMP180 bmp(dp5, dp27); |
sashida_h | 2:a4895f7c3058 | 35 | PwmOut servo_para(dp1); |
sashida_h | 2:a4895f7c3058 | 36 | PwmOut servo_leaf(dp18); |
sashida_h | 2:a4895f7c3058 | 37 | DigitalOut myled(dp14); |
sashida_h | 2:a4895f7c3058 | 38 | DigitalOut myled2(dp28); |
sashida_h | 2:a4895f7c3058 | 39 | |
sashida_h | 2:a4895f7c3058 | 40 | Timer timer1; |
sashida_h | 2:a4895f7c3058 | 41 | Timer timer2; |
sashida_h | 2:a4895f7c3058 | 42 | Ticker tic_open; |
mikawataru | 0:0ff20d8e9090 | 43 | |
sashida_h | 2:a4895f7c3058 | 44 | //高度取得 |
sashida_h | 2:a4895f7c3058 | 45 | float Alt_buff[10]; |
sashida_h | 2:a4895f7c3058 | 46 | float Alt_gnd,Alt_drop; |
sashida_h | 2:a4895f7c3058 | 47 | float pressure,temperature; |
sashida_h | 2:a4895f7c3058 | 48 | float p0 = 1013.25; |
sashida_h | 2:a4895f7c3058 | 49 | //カウント変数 |
sashida_h | 2:a4895f7c3058 | 50 | int Cnt_buff; |
sashida_h | 2:a4895f7c3058 | 51 | int i; |
sashida_h | 2:a4895f7c3058 | 52 | int Cnt_para; |
mikawataru | 0:0ff20d8e9090 | 53 | |
sashida_h | 2:a4895f7c3058 | 54 | //加速度取得 |
sashida_h | 2:a4895f7c3058 | 55 | float a_abs; |
sashida_h | 2:a4895f7c3058 | 56 | float a[3]; |
sashida_h | 2:a4895f7c3058 | 57 | float Acc_buff[10]; |
sashida_h | 2:a4895f7c3058 | 58 | float Acc_lnc; |
sashida_h | 2:a4895f7c3058 | 59 | //その他 |
sashida_h | 2:a4895f7c3058 | 60 | int t1, t2; |
sashida_h | 2:a4895f7c3058 | 61 | bool tf_para = true; |
sashida_h | 2:a4895f7c3058 | 62 | bool tf_leaf = true; |
sashida_h | 2:a4895f7c3058 | 63 | |
sashida_h | 2:a4895f7c3058 | 64 | void _para(int motion); |
sashida_h | 2:a4895f7c3058 | 65 | void _leaf(int motion); |
sashida_h | 2:a4895f7c3058 | 66 | void kaihou(void); |
sashida_h | 2:a4895f7c3058 | 67 | /*雑関数*/ |
sashida_h | 2:a4895f7c3058 | 68 | float median(float data[], int num); |
sashida_h | 2:a4895f7c3058 | 69 | float get_Alt(float press, float temp); |
sashida_h | 2:a4895f7c3058 | 70 | int _input(char c); |
mikawataru | 0:0ff20d8e9090 | 71 | |
mikawataru | 0:0ff20d8e9090 | 72 | int main() { |
sashida_h | 2:a4895f7c3058 | 73 | |
sashida_h | 2:a4895f7c3058 | 74 | le.input(); |
sashida_h | 2:a4895f7c3058 | 75 | |
sashida_h | 2:a4895f7c3058 | 76 | wait(1); //良い子は待つ |
sashida_h | 2:a4895f7c3058 | 77 | if(pr == 1){ //途中で電源落ちたら |
sashida_h | 2:a4895f7c3058 | 78 | Phase = RISE; |
sashida_h | 2:a4895f7c3058 | 79 | Cnt_buff = 0; |
sashida_h | 2:a4895f7c3058 | 80 | timer1.start(); |
sashida_h | 2:a4895f7c3058 | 81 | goto yabai; |
sashida_h | 2:a4895f7c3058 | 82 | } |
sashida_h | 2:a4895f7c3058 | 83 | |
sashida_h | 2:a4895f7c3058 | 84 | myled2 = 1; |
sashida_h | 2:a4895f7c3058 | 85 | |
sashida_h | 2:a4895f7c3058 | 86 | |
sashida_h | 2:a4895f7c3058 | 87 | while(1){ |
sashida_h | 2:a4895f7c3058 | 88 | |
sashida_h | 2:a4895f7c3058 | 89 | |
sashida_h | 2:a4895f7c3058 | 90 | while(pr== 0 && le==0); |
sashida_h | 2:a4895f7c3058 | 91 | wait(0.2); //LPC1768の信号待ち |
sashida_h | 2:a4895f7c3058 | 92 | if(pr==1 && le==0){ |
sashida_h | 2:a4895f7c3058 | 93 | if(tf_para == true){ |
sashida_h | 2:a4895f7c3058 | 94 | _para(UNLOCK); |
sashida_h | 2:a4895f7c3058 | 95 | myled = 1; |
sashida_h | 2:a4895f7c3058 | 96 | // myled2 = 0; |
sashida_h | 2:a4895f7c3058 | 97 | tf_para = false; |
sashida_h | 2:a4895f7c3058 | 98 | } |
sashida_h | 2:a4895f7c3058 | 99 | else{ |
sashida_h | 2:a4895f7c3058 | 100 | _para(LOCK); |
sashida_h | 2:a4895f7c3058 | 101 | myled = 0; |
sashida_h | 2:a4895f7c3058 | 102 | // myled2 = 0; |
sashida_h | 2:a4895f7c3058 | 103 | tf_para = true; |
sashida_h | 2:a4895f7c3058 | 104 | } |
sashida_h | 2:a4895f7c3058 | 105 | wait(1); |
sashida_h | 2:a4895f7c3058 | 106 | } |
sashida_h | 2:a4895f7c3058 | 107 | if(pr==0 && le==1){ |
sashida_h | 2:a4895f7c3058 | 108 | if(tf_leaf == true){ |
sashida_h | 2:a4895f7c3058 | 109 | _leaf(UNLOCK); |
sashida_h | 2:a4895f7c3058 | 110 | myled2 = 0; |
sashida_h | 2:a4895f7c3058 | 111 | tf_leaf = false; |
sashida_h | 2:a4895f7c3058 | 112 | } |
sashida_h | 2:a4895f7c3058 | 113 | else{ |
sashida_h | 2:a4895f7c3058 | 114 | _leaf(LOCK); |
sashida_h | 2:a4895f7c3058 | 115 | myled2 = 0; |
sashida_h | 2:a4895f7c3058 | 116 | tf_leaf = true; |
sashida_h | 2:a4895f7c3058 | 117 | } |
sashida_h | 2:a4895f7c3058 | 118 | wait(1); |
sashida_h | 2:a4895f7c3058 | 119 | } |
sashida_h | 2:a4895f7c3058 | 120 | |
sashida_h | 2:a4895f7c3058 | 121 | |
sashida_h | 2:a4895f7c3058 | 122 | if(pr==1 && le==1) break; |
sashida_h | 2:a4895f7c3058 | 123 | |
sashida_h | 2:a4895f7c3058 | 124 | } |
sashida_h | 2:a4895f7c3058 | 125 | |
sashida_h | 2:a4895f7c3058 | 126 | le.output(); |
sashida_h | 2:a4895f7c3058 | 127 | le = 0; |
sashida_h | 2:a4895f7c3058 | 128 | wait(2.5); //LPC1768の初期化処理待ち |
sashida_h | 2:a4895f7c3058 | 129 | |
sashida_h | 2:a4895f7c3058 | 130 | Phase = LUNCH; |
sashida_h | 2:a4895f7c3058 | 131 | |
sashida_h | 2:a4895f7c3058 | 132 | yabai: |
sashida_h | 2:a4895f7c3058 | 133 | |
sashida_h | 2:a4895f7c3058 | 134 | mpu.setAcceleroRange(0); |
sashida_h | 2:a4895f7c3058 | 135 | bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER); |
sashida_h | 2:a4895f7c3058 | 136 | |
sashida_h | 2:a4895f7c3058 | 137 | |
sashida_h | 2:a4895f7c3058 | 138 | Cnt_buff = 0; |
sashida_h | 2:a4895f7c3058 | 139 | for(i=0; i<10; i++){ |
sashida_h | 2:a4895f7c3058 | 140 | bmp.ReadData(&temperature,&pressure); |
sashida_h | 2:a4895f7c3058 | 141 | Alt_buff[Cnt_buff]=get_Alt(temperature, pressure); |
sashida_h | 2:a4895f7c3058 | 142 | Cnt_buff++; |
sashida_h | 2:a4895f7c3058 | 143 | } |
sashida_h | 2:a4895f7c3058 | 144 | Alt_gnd = median(Alt_buff, 10); |
sashida_h | 2:a4895f7c3058 | 145 | |
sashida_h | 2:a4895f7c3058 | 146 | Cnt_buff = 0; |
sashida_h | 2:a4895f7c3058 | 147 | |
sashida_h | 2:a4895f7c3058 | 148 | |
sashida_h | 2:a4895f7c3058 | 149 | tic_open.attach(&kaihou,1.0/RATE_OPEN); |
sashida_h | 2:a4895f7c3058 | 150 | |
sashida_h | 2:a4895f7c3058 | 151 | while(1); |
mikawataru | 0:0ff20d8e9090 | 152 | |
sashida_h | 2:a4895f7c3058 | 153 | |
sashida_h | 2:a4895f7c3058 | 154 | } |
sashida_h | 2:a4895f7c3058 | 155 | |
sashida_h | 2:a4895f7c3058 | 156 | |
sashida_h | 2:a4895f7c3058 | 157 | void kaihou(void){ |
sashida_h | 2:a4895f7c3058 | 158 | |
sashida_h | 2:a4895f7c3058 | 159 | switch(Phase){ |
sashida_h | 2:a4895f7c3058 | 160 | |
sashida_h | 2:a4895f7c3058 | 161 | case LUNCH: |
sashida_h | 2:a4895f7c3058 | 162 | |
sashida_h | 2:a4895f7c3058 | 163 | mpu.getAccelero(a); |
sashida_h | 2:a4895f7c3058 | 164 | Acc_buff[Cnt_buff] = sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2)); |
sashida_h | 2:a4895f7c3058 | 165 | Cnt_buff++; |
sashida_h | 2:a4895f7c3058 | 166 | |
sashida_h | 2:a4895f7c3058 | 167 | //myled2=!myled2; |
sashida_h | 2:a4895f7c3058 | 168 | |
sashida_h | 2:a4895f7c3058 | 169 | if(Cnt_buff == 10){ |
sashida_h | 2:a4895f7c3058 | 170 | myled2 = 0; |
sashida_h | 2:a4895f7c3058 | 171 | Acc_lnc = median(Acc_buff, 10); |
sashida_h | 2:a4895f7c3058 | 172 | Cnt_buff = 0; |
sashida_h | 2:a4895f7c3058 | 173 | //myled2 = 0; |
sashida_h | 2:a4895f7c3058 | 174 | } |
sashida_h | 2:a4895f7c3058 | 175 | if(Acc_lnc>LUNCH_G){ |
sashida_h | 2:a4895f7c3058 | 176 | Phase = RISE; |
sashida_h | 2:a4895f7c3058 | 177 | //myled2 = 0; |
sashida_h | 2:a4895f7c3058 | 178 | timer1.start(); |
sashida_h | 2:a4895f7c3058 | 179 | le = 1; //LPC1768に発射をお知らせする |
sashida_h | 2:a4895f7c3058 | 180 | } |
sashida_h | 2:a4895f7c3058 | 181 | |
sashida_h | 2:a4895f7c3058 | 182 | break; |
sashida_h | 2:a4895f7c3058 | 183 | |
sashida_h | 2:a4895f7c3058 | 184 | |
sashida_h | 2:a4895f7c3058 | 185 | case RISE: |
sashida_h | 2:a4895f7c3058 | 186 | if(Cnt_buff == 0){ |
sashida_h | 2:a4895f7c3058 | 187 | bmp.ReadData(&temperature,&pressure); |
sashida_h | 2:a4895f7c3058 | 188 | Alt_buff[Cnt_buff]=get_Alt(temperature, pressure); //まず高度取る |
sashida_h | 2:a4895f7c3058 | 189 | } |
sashida_h | 2:a4895f7c3058 | 190 | |
sashida_h | 2:a4895f7c3058 | 191 | bmp.ReadData(&temperature,&pressure); |
sashida_h | 2:a4895f7c3058 | 192 | Alt_buff[Cnt_buff+1] = get_Alt(temperature, pressure); |
sashida_h | 2:a4895f7c3058 | 193 | if(Alt_buff[Cnt_buff]>Alt_buff[Cnt_buff+1]) Cnt_para++; |
sashida_h | 2:a4895f7c3058 | 194 | Cnt_buff++; |
sashida_h | 2:a4895f7c3058 | 195 | |
sashida_h | 2:a4895f7c3058 | 196 | if(Cnt_buff == 9){ |
sashida_h | 2:a4895f7c3058 | 197 | //myled2 = 1; |
sashida_h | 2:a4895f7c3058 | 198 | t1 = timer1.read(); |
sashida_h | 2:a4895f7c3058 | 199 | |
sashida_h | 2:a4895f7c3058 | 200 | if(Cnt_para>=5 || t1>TIMER1){ |
sashida_h | 2:a4895f7c3058 | 201 | _para(UNLOCK); |
sashida_h | 2:a4895f7c3058 | 202 | myled2 =0; |
sashida_h | 2:a4895f7c3058 | 203 | le = 0; //LPC1768にパラ放出をお知らせする |
sashida_h | 2:a4895f7c3058 | 204 | timer1.stop(); |
sashida_h | 2:a4895f7c3058 | 205 | timer2.start(); |
sashida_h | 2:a4895f7c3058 | 206 | Phase = DROP; |
sashida_h | 2:a4895f7c3058 | 207 | } |
sashida_h | 2:a4895f7c3058 | 208 | Cnt_buff = 0; |
sashida_h | 2:a4895f7c3058 | 209 | |
sashida_h | 2:a4895f7c3058 | 210 | } |
sashida_h | 2:a4895f7c3058 | 211 | break; |
sashida_h | 2:a4895f7c3058 | 212 | |
sashida_h | 2:a4895f7c3058 | 213 | case DROP: |
sashida_h | 2:a4895f7c3058 | 214 | |
sashida_h | 2:a4895f7c3058 | 215 | bmp.ReadData(&temperature,&pressure); |
sashida_h | 2:a4895f7c3058 | 216 | Alt_buff[Cnt_buff]=get_Alt(temperature, pressure); |
sashida_h | 2:a4895f7c3058 | 217 | Cnt_buff ++; |
sashida_h | 2:a4895f7c3058 | 218 | if(Cnt_buff == 9){ |
sashida_h | 2:a4895f7c3058 | 219 | Alt_drop = median(Alt_buff, 10)-Alt_gnd; |
sashida_h | 2:a4895f7c3058 | 220 | t2 = timer2.read(); |
sashida_h | 2:a4895f7c3058 | 221 | //twe.printf("%d\r\n",t2); |
sashida_h | 2:a4895f7c3058 | 222 | myled2 = !myled2; |
sashida_h | 2:a4895f7c3058 | 223 | |
sashida_h | 2:a4895f7c3058 | 224 | if(Alt_drop<ALT_LEAF && t2>TIMER2){ |
sashida_h | 2:a4895f7c3058 | 225 | _leaf(UNLOCK); |
sashida_h | 2:a4895f7c3058 | 226 | le = 1 ; //LPC1768にリーフィングをお知らせする |
sashida_h | 2:a4895f7c3058 | 227 | myled2 = 1; |
sashida_h | 2:a4895f7c3058 | 228 | |
sashida_h | 2:a4895f7c3058 | 229 | } |
sashida_h | 2:a4895f7c3058 | 230 | Cnt_buff = 0; |
sashida_h | 2:a4895f7c3058 | 231 | } |
sashida_h | 2:a4895f7c3058 | 232 | |
sashida_h | 2:a4895f7c3058 | 233 | break ; |
mikawataru | 0:0ff20d8e9090 | 234 | } |
mikawataru | 0:0ff20d8e9090 | 235 | } |
mikawataru | 0:0ff20d8e9090 | 236 | |
sashida_h | 2:a4895f7c3058 | 237 | void _para(int motion){ |
sashida_h | 2:a4895f7c3058 | 238 | if(motion==UNLOCK){ |
sashida_h | 2:a4895f7c3058 | 239 | servo_para.pulsewidth(0.0005); // pulse servo out sita |
sashida_h | 2:a4895f7c3058 | 240 | }else if(motion==LOCK){ |
sashida_h | 2:a4895f7c3058 | 241 | servo_para.pulsewidth(0.0025); // pulse servo outu sita |
sashida_h | 2:a4895f7c3058 | 242 | } |
sashida_h | 2:a4895f7c3058 | 243 | } |
sashida_h | 2:a4895f7c3058 | 244 | void _leaf(int motion){ |
sashida_h | 2:a4895f7c3058 | 245 | if(motion==UNLOCK){ |
sashida_h | 2:a4895f7c3058 | 246 | servo_leaf.pulsewidth(0.0006); // pulse servo out |
sashida_h | 2:a4895f7c3058 | 247 | }else if(motion==LOCK){ |
sashida_h | 2:a4895f7c3058 | 248 | servo_leaf.pulsewidth(0.0024); // pulse servo out |
sashida_h | 2:a4895f7c3058 | 249 | } |
sashida_h | 2:a4895f7c3058 | 250 | } |
sashida_h | 2:a4895f7c3058 | 251 | |
sashida_h | 2:a4895f7c3058 | 252 | float median(float data[], int num){//todo:処理時間計測 |
sashida_h | 2:a4895f7c3058 | 253 | float *data_cpy, ans; |
sashida_h | 2:a4895f7c3058 | 254 | data_cpy = new float[num]; |
sashida_h | 2:a4895f7c3058 | 255 | memcpy(data_cpy,data,sizeof(float)*num); |
sashida_h | 2:a4895f7c3058 | 256 | |
sashida_h | 2:a4895f7c3058 | 257 | for(int i=0; i<num; i++){ |
sashida_h | 2:a4895f7c3058 | 258 | for(int j=0; j<num-i-1; j++){ |
sashida_h | 2:a4895f7c3058 | 259 | if(data_cpy[j]>data_cpy[j+1]){ |
sashida_h | 2:a4895f7c3058 | 260 | float buff = data_cpy[j+1]; |
sashida_h | 2:a4895f7c3058 | 261 | data_cpy[j+1] = data_cpy[j]; |
sashida_h | 2:a4895f7c3058 | 262 | data_cpy[j] = buff; |
sashida_h | 2:a4895f7c3058 | 263 | } |
sashida_h | 2:a4895f7c3058 | 264 | } |
sashida_h | 2:a4895f7c3058 | 265 | } |
sashida_h | 2:a4895f7c3058 | 266 | |
sashida_h | 2:a4895f7c3058 | 267 | if(num%2!=0) ans = data_cpy[num/2]; |
sashida_h | 2:a4895f7c3058 | 268 | else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; |
sashida_h | 2:a4895f7c3058 | 269 | delete[] data_cpy; |
sashida_h | 2:a4895f7c3058 | 270 | return ans; |
sashida_h | 2:a4895f7c3058 | 271 | } |
sashida_h | 2:a4895f7c3058 | 272 | |
sashida_h | 2:a4895f7c3058 | 273 | float get_Alt(float temp, float press){ |
mikawataru | 0:0ff20d8e9090 | 274 | return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; |
mikawataru | 0:0ff20d8e9090 | 275 | } |