Haruki Sashida
/
ARLISS_2018_ver2
ARLISS 2018 parachute board
Diff: main.cpp
- Revision:
- 2:a4895f7c3058
- Parent:
- 1:b9ea35d93329
--- a/main.cpp Mon Oct 10 11:21:32 2016 +0000 +++ b/main.cpp Thu Nov 02 17:13:22 2017 +0000 @@ -14,34 +14,262 @@ */ #include "mbed.h" +#include "MPU6050.h" #include "math.h" #include "BMP180.h" -#define p0 1013.25f//海面気圧 + +#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; -DigitalOut myled(LED1); -Serial pc(USBTX,USBRX); -BMP180 bmp(PB_7, PB_6); -Timer timer; +//高度取得 +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 getAlt(float press, float temp); +//加速度取得 +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() { - float pressure,temperature,altitude; - float time; - bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); - pc.printf("time, temperature ,pressure, altitude\r\n"); - timer.start(); + + 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); - while(1) { - bmp.ReadData(&temperature,&pressure); - altitude = getAlt(pressure,temperature); - time = timer.read(); - pc.printf("%f, %f, %f, %f \r\n",time, temperature, pressure, altitude); - myled =! myled; - wait(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 ; } } -float getAlt(float press, float temp){ +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; } \ No newline at end of file