Haruki Sashida
/
ARLISS_2018_ver2
ARLISS 2018 parachute board
main.cpp
- Committer:
- sashida_h
- Date:
- 2017-11-02
- Revision:
- 2:a4895f7c3058
- Parent:
- 1:b9ea35d93329
File content as of revision 2:a4895f7c3058:
/* 説明 Nucleo-F303K8とBMP180を使った気温・気圧・高度計算のサンプルプログラム ライブラリ https://developer.mbed.org/users/spiridion/code/BMP180/ 以下ピン配置 Nucleo BMP180 GND-----GND-------0V +3V3----VIN D4------SDA D5------SCL */ #include "mbed.h" #include "MPU6050.h" #include "math.h" #include "BMP180.h" #define LUNCH_G 1.0 #define TIMER1 500 #define TIMER2 30 #define ALT_LEAF 300 #define UNLOCK 0 #define LOCK 1 #define RATE_OPEN 1 enum PHASE{LUNCH=1,RISE=3,DROP=7} Phase; DigitalIn pr(dp10); DigitalInOut le(dp11); MPU6050 mpu(dp5, dp27); BMP180 bmp(dp5, dp27); PwmOut servo_para(dp1); PwmOut servo_leaf(dp18); DigitalOut myled(dp14); DigitalOut myled2(dp28); Timer timer1; Timer timer2; Ticker tic_open; //高度取得 float Alt_buff[10]; float Alt_gnd,Alt_drop; float pressure,temperature; float p0 = 1013.25; //カウント変数 int Cnt_buff; int i; int Cnt_para; //加速度取得 float a_abs; float a[3]; float Acc_buff[10]; float Acc_lnc; //その他 int t1, t2; bool tf_para = true; bool tf_leaf = true; void _para(int motion); void _leaf(int motion); void kaihou(void); /*雑関数*/ float median(float data[], int num); float get_Alt(float press, float temp); int _input(char c); int main() { le.input(); wait(1); //良い子は待つ if(pr == 1){ //途中で電源落ちたら Phase = RISE; Cnt_buff = 0; timer1.start(); goto yabai; } myled2 = 1; while(1){ while(pr== 0 && le==0); wait(0.2); //LPC1768の信号待ち if(pr==1 && le==0){ if(tf_para == true){ _para(UNLOCK); myled = 1; // myled2 = 0; tf_para = false; } else{ _para(LOCK); myled = 0; // myled2 = 0; tf_para = true; } wait(1); } if(pr==0 && le==1){ if(tf_leaf == true){ _leaf(UNLOCK); myled2 = 0; tf_leaf = false; } else{ _leaf(LOCK); myled2 = 0; tf_leaf = true; } wait(1); } if(pr==1 && le==1) break; } le.output(); le = 0; wait(2.5); //LPC1768の初期化処理待ち Phase = LUNCH; yabai: mpu.setAcceleroRange(0); bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER); Cnt_buff = 0; for(i=0; i<10; i++){ bmp.ReadData(&temperature,&pressure); Alt_buff[Cnt_buff]=get_Alt(temperature, pressure); Cnt_buff++; } Alt_gnd = median(Alt_buff, 10); Cnt_buff = 0; tic_open.attach(&kaihou,1.0/RATE_OPEN); while(1); } void kaihou(void){ switch(Phase){ case LUNCH: mpu.getAccelero(a); Acc_buff[Cnt_buff] = sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2)); Cnt_buff++; //myled2=!myled2; if(Cnt_buff == 10){ myled2 = 0; Acc_lnc = median(Acc_buff, 10); Cnt_buff = 0; //myled2 = 0; } if(Acc_lnc>LUNCH_G){ Phase = RISE; //myled2 = 0; timer1.start(); le = 1; //LPC1768に発射をお知らせする } break; case RISE: if(Cnt_buff == 0){ bmp.ReadData(&temperature,&pressure); Alt_buff[Cnt_buff]=get_Alt(temperature, pressure); //まず高度取る } bmp.ReadData(&temperature,&pressure); Alt_buff[Cnt_buff+1] = get_Alt(temperature, pressure); if(Alt_buff[Cnt_buff]>Alt_buff[Cnt_buff+1]) Cnt_para++; Cnt_buff++; if(Cnt_buff == 9){ //myled2 = 1; t1 = timer1.read(); if(Cnt_para>=5 || t1>TIMER1){ _para(UNLOCK); myled2 =0; le = 0; //LPC1768にパラ放出をお知らせする timer1.stop(); timer2.start(); Phase = DROP; } Cnt_buff = 0; } break; case DROP: bmp.ReadData(&temperature,&pressure); Alt_buff[Cnt_buff]=get_Alt(temperature, pressure); Cnt_buff ++; if(Cnt_buff == 9){ Alt_drop = median(Alt_buff, 10)-Alt_gnd; t2 = timer2.read(); //twe.printf("%d\r\n",t2); myled2 = !myled2; if(Alt_drop<ALT_LEAF && t2>TIMER2){ _leaf(UNLOCK); le = 1 ; //LPC1768にリーフィングをお知らせする myled2 = 1; } Cnt_buff = 0; } break ; } } void _para(int motion){ if(motion==UNLOCK){ servo_para.pulsewidth(0.0005); // pulse servo out sita }else if(motion==LOCK){ servo_para.pulsewidth(0.0025); // pulse servo outu sita } } void _leaf(int motion){ if(motion==UNLOCK){ servo_leaf.pulsewidth(0.0006); // pulse servo out }else if(motion==LOCK){ servo_leaf.pulsewidth(0.0024); // pulse servo out } } float median(float data[], int num){//todo:処理時間計測 float *data_cpy, ans; data_cpy = new float[num]; memcpy(data_cpy,data,sizeof(float)*num); for(int i=0; i<num; i++){ for(int j=0; j<num-i-1; j++){ if(data_cpy[j]>data_cpy[j+1]){ float buff = data_cpy[j+1]; data_cpy[j+1] = data_cpy[j]; data_cpy[j] = buff; } } } if(num%2!=0) ans = data_cpy[num/2]; else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; delete[] data_cpy; return ans; } float get_Alt(float temp, float press){ return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; }