hitomi shio
/
mission
CORE'S キッチン ミッション
Diff: main.cpp
- Revision:
- 0:31fa273aa56d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Aug 03 01:09:43 2018 +0000 @@ -0,0 +1,196 @@ +#include "mbed.h" +#include "math.h" +#include "BMP180.h" + +#define p0 1013.25f//海面気圧 +#define TIME_TOP 14 +#define TIME_WATER 20 +#define TIME_BUZZER 25 +#define NUM 5 +#define RATE 10 + +enum PHASE{EGG,RISE,DROP,BUZZER} Phase; + +//Serial pc(USBTX,USBRX); + +PwmOut servo_crack1(p26); +PwmOut servo_crack2(p25); +PwmOut servo_open1(p24); +PwmOut servo_open2(p23); +DigitalIn rec1(p5); +DigitalIn rec2(p6); +DigitalOut heat(p22); +DigitalOut buzzer(p20); +DigitalOut myled1(LED1); +DigitalOut myled2(LED2); +DigitalOut myled3(LED3); +BMP180 bmp (p28, p27); +LocalFileSystem local("local"); + +Timer timer_acc; +Timer timer_top; +Timer timer_all; +Ticker tic_mission; +Ticker tic_buzzer; + +void _mission(); +void _heater_acc(); +void _buzzer(); +float _getAlt(float press, float temp); +float _median(float data[], int num); +float Med_alt,Maeno_alt; +float Pressure,Temperature,Altitude; +float alt[NUM],max=-1000; +int Cnt_top=0,Cnt_water=0; +int altitude; +int med_alt; + +int main(){ + _heater_acc(); + Phase=EGG; + tic_mission.attach(&_mission,1.0/RATE); + tic_buzzer.attach(&_buzzer,1.0/RATE); + while(1){ + if(timer_top.read()>=TIME_BUZZER){ + buzzer=0; + } + } +} + + +void _mission(){ + switch(Phase){ + case EGG: + buzzer=0; + myled1=0; + myled2=0; + timer_all.start(); + FILE *fp; + fp = fopen("/local/out.txt", "a"); + fprintf(fp,"%f:ヒーターON\n\r",timer_all); + timer_acc.start(); + servo_crack1.pulsewidth(0.005); + servo_crack2.pulsewidth(0.024); + fprintf(fp, "%f:サーボ_割る\n\r",timer_all); + wait(1.0); + servo_open1.pulsewidth(0.005); + servo_open2.pulsewidth(0.024); + fprintf(fp, "%f:サーボ_開く\n\r",timer_all); + fclose(fp); + Phase=RISE; + break; + case RISE: + bmp.Initialize(BMP180_OSS_ULTRA_LOW_POWER); + //pc.printf("Temperature ,Pressure, Altitude\r\n"); + int j; + for(j=0;j<NUM;j++){ + bmp.ReadData(&Temperature,&Pressure); + altitude = _getAlt(Pressure,Temperature); + alt[j]=altitude; + } + Med_alt=_median(alt,NUM); + //pc.printf("%f,%f\n\r",Med_alt,max); + if(med_alt>max){ + max=Med_alt; + Cnt_top=0; + }else if(max-Med_alt>=2.0){ + Cnt_top++; + } + //pc.printf("%d\n\r",Cnt_top); + if(Cnt_top>=3||timer_acc.read()>=TIME_TOP){ + fp = fopen("/local/out.txt", "a"); + //pc.printf("頂点判定"); + fprintf(fp, "%f:頂点判定\n\r",timer_all); + myled1=1; + timer_top.start(); + Phase=DROP; + } + break; + case DROP: + Maeno_alt=Med_alt; + int i; + for(i=0;i<NUM;i++){ + bmp.ReadData(&Temperature,&Pressure); + altitude = _getAlt(Pressure,Temperature); + alt[i]=altitude; + } + Med_alt=_median(alt,NUM); + //pc.printf("%f,%f\n\r",Med_alt,max); + if(Maeno_alt-Med_alt>=-1.0&&Maeno_alt-Med_alt<=1.0){ + Cnt_water++; + }else{ + Cnt_water=0; + } + //pc.printf("%d\n\r",Cnt_water); + if(Cnt_water>=3||timer_top.read()>=TIME_WATER){ + fp = fopen("/local/out.txt", "a"); + //pc.printf("着水判定"); + myled2=1; + heat=0; + buzzer=1; + fprintf(fp, "%f:ヒーターOFF\nブザーON\n\r",timer_all); + fclose(fp); + Phase=BUZZER; + tic_mission.detach(); + } + //break; + + /*case BUZZER: + if(timer_top>=TIME_BUZZER){ + fp = fopen("/local/out.txt", "a"); + buzzer=0; + fprintf(fp, "%f:ブザーOFF\n\r",timer_all); + fclose(fp); + } + break;*/ + } +} + +void _heater_acc(){ + heat=0; + while(1){ + if(rec1==1){ + heat=1; + }else if(rec1==0){ + heat=0; + } + if(rec2==1){ + break; + }else if(rec2==0){ + } + } +} + +void _buzzer(){ + myled3=!myled3; + if(rec2==1){ + buzzer=0; + }else if(rec2==0){ + buzzer=0; + } +} + +float _getAlt(float press, float temp){ + return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; +} + +float _median(float data[], int num){ + 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; +} \ No newline at end of file