Haruki Sashida / Mbed 2 deprecated 201903_pf_lower_judgment

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?

UserRevisionLine numberNew 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 }