CORE / Mbed 2 deprecated mission

2018.08能代CORE'S キッチン　ミッション用プログラム

Dependencies:   BMP180 mbed

Fork of mission by hitomi shio

main.cpp@1:9833a893b719, 2018-08-05 (annotated)

Committer:
ShioHitomi
Date:
Sun Aug 05 03:15:22 2018 +0000
Revision:
1:9833a893b719
Parent:
0:31fa273aa56d
Child:
2:b145797d04fb
mission

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ShioHitomi 0:31fa273aa56d 1 #include "mbed.h"
ShioHitomi 0:31fa273aa56d 2 #include "math.h"
ShioHitomi 0:31fa273aa56d 3 #include "BMP180.h"
ShioHitomi 0:31fa273aa56d 4
ShioHitomi 0:31fa273aa56d 5 #define p0 1013.25f//海面気圧
ShioHitomi 1:9833a893b719 6 #define TIME_TOP 3
ShioHitomi 1:9833a893b719 7 #define TIME_WATER 5
ShioHitomi 1:9833a893b719 8 #define TIME_BUZZER 16
ShioHitomi 0:31fa273aa56d 9 #define NUM 5
ShioHitomi 0:31fa273aa56d 10 #define RATE 10
ShioHitomi 0:31fa273aa56d 11
ShioHitomi 0:31fa273aa56d 12 enum PHASE{EGG,RISE,DROP,BUZZER} Phase;
ShioHitomi 0:31fa273aa56d 13
ShioHitomi 0:31fa273aa56d 14 //Serial pc(USBTX,USBRX);
ShioHitomi 0:31fa273aa56d 15
ShioHitomi 0:31fa273aa56d 16 PwmOut servo_crack1(p26);
ShioHitomi 0:31fa273aa56d 17 PwmOut servo_crack2(p25);
ShioHitomi 0:31fa273aa56d 18 PwmOut servo_open1(p24);
ShioHitomi 0:31fa273aa56d 19 PwmOut servo_open2(p23);
ShioHitomi 0:31fa273aa56d 20 DigitalIn rec1(p5);
ShioHitomi 0:31fa273aa56d 21 DigitalIn rec2(p6);
ShioHitomi 0:31fa273aa56d 22 DigitalOut heat(p22);
ShioHitomi 1:9833a893b719 23 DigitalOut buzzer(p21);
ShioHitomi 0:31fa273aa56d 24 DigitalOut myled1(LED1);
ShioHitomi 0:31fa273aa56d 25 DigitalOut myled2(LED2);
ShioHitomi 0:31fa273aa56d 26 DigitalOut myled3(LED3);
ShioHitomi 1:9833a893b719 27 DigitalOut myled4(LED4);
ShioHitomi 0:31fa273aa56d 28 BMP180 bmp (p28, p27);
ShioHitomi 0:31fa273aa56d 29 LocalFileSystem local("local");
ShioHitomi 0:31fa273aa56d 30
ShioHitomi 0:31fa273aa56d 31 Timer timer_acc;
ShioHitomi 0:31fa273aa56d 32 Timer timer_top;
ShioHitomi 0:31fa273aa56d 33 Timer timer_all;
ShioHitomi 0:31fa273aa56d 34 Ticker tic_mission;
ShioHitomi 0:31fa273aa56d 35 Ticker tic_buzzer;
ShioHitomi 0:31fa273aa56d 36
ShioHitomi 0:31fa273aa56d 37 void _mission();
ShioHitomi 0:31fa273aa56d 38 void _heater_acc();
ShioHitomi 0:31fa273aa56d 39 void _buzzer();
ShioHitomi 0:31fa273aa56d 40 float _getAlt(float press, float temp);
ShioHitomi 0:31fa273aa56d 41 float _median(float data[], int num);
ShioHitomi 0:31fa273aa56d 42 float Med_alt,Maeno_alt;
ShioHitomi 0:31fa273aa56d 43 float Pressure,Temperature,Altitude;
ShioHitomi 0:31fa273aa56d 44 float alt[NUM],max=-1000;
ShioHitomi 0:31fa273aa56d 45 int Cnt_top=0,Cnt_water=0;
ShioHitomi 0:31fa273aa56d 46 int altitude;
ShioHitomi 0:31fa273aa56d 47 int med_alt;
ShioHitomi 0:31fa273aa56d 48
ShioHitomi 1:9833a893b719 49 int main(){
ShioHitomi 1:9833a893b719 50 buzzer=0;
ShioHitomi 1:9833a893b719 51 _heater_acc();
ShioHitomi 1:9833a893b719 52 Phase=RISE;
ShioHitomi 1:9833a893b719 53 tic_mission.attach(&_mission,1.0/10);
ShioHitomi 1:9833a893b719 54 //tic_buzzer.attach(&_buzzer,1.0/RATE);
ShioHitomi 0:31fa273aa56d 55 while(1){
ShioHitomi 1:9833a893b719 56 _buzzer();
ShioHitomi 1:9833a893b719 57 /*if(Phase=BUZZER){
ShioHitomi 1:9833a893b719 58 tic_mission.detach();*/
ShioHitomi 0:31fa273aa56d 59 if(timer_top.read()>=TIME_BUZZER){
ShioHitomi 0:31fa273aa56d 60 buzzer=0;
ShioHitomi 0:31fa273aa56d 61 }
ShioHitomi 0:31fa273aa56d 62 }
ShioHitomi 0:31fa273aa56d 63 }
ShioHitomi 0:31fa273aa56d 64
ShioHitomi 0:31fa273aa56d 65
ShioHitomi 0:31fa273aa56d 66 void _mission(){
ShioHitomi 1:9833a893b719 67 myled4=!myled4;
ShioHitomi 0:31fa273aa56d 68 switch(Phase){
ShioHitomi 0:31fa273aa56d 69 case EGG:
ShioHitomi 1:9833a893b719 70 //buzzer=0;
ShioHitomi 0:31fa273aa56d 71 myled1=0;
ShioHitomi 0:31fa273aa56d 72 myled2=0;
ShioHitomi 0:31fa273aa56d 73 timer_all.start();
ShioHitomi 1:9833a893b719 74 //FILE *fp;
ShioHitomi 1:9833a893b719 75 //fp = fopen("/local/out.txt", "a");
ShioHitomi 1:9833a893b719 76 //fprintf(fp,"%f:ヒーターON\n\r",timer_all);
ShioHitomi 1:9833a893b719 77 timer_acc.start();
ShioHitomi 1:9833a893b719 78 servo_crack1.pulsewidth(0.005);
ShioHitomi 1:9833a893b719 79 servo_crack2.pulsewidth(0.024);
ShioHitomi 1:9833a893b719 80 //fprintf(fp, "%f:サーボ＿割る\n\r",timer_all);
ShioHitomi 1:9833a893b719 81 wait(1.0);
ShioHitomi 1:9833a893b719 82 servo_open1.pulsewidth(0.005);
ShioHitomi 1:9833a893b719 83 servo_open2.pulsewidth(0.024);
ShioHitomi 1:9833a893b719 84 //fprintf(fp, "%f:サーボ＿開く\n\r",timer_all);
ShioHitomi 1:9833a893b719 85 //fclose(fp);
ShioHitomi 1:9833a893b719 86 Phase=RISE;
ShioHitomi 1:9833a893b719 87 break;
ShioHitomi 1:9833a893b719 88 case RISE:
ShioHitomi 0:31fa273aa56d 89 bmp.Initialize(BMP180_OSS_ULTRA_LOW_POWER);
ShioHitomi 0:31fa273aa56d 90 //pc.printf("Temperature ,Pressure, Altitude\r\n");
ShioHitomi 0:31fa273aa56d 91 int j;
ShioHitomi 0:31fa273aa56d 92 for(j=0;j<NUM;j++){
ShioHitomi 0:31fa273aa56d 93 bmp.ReadData(&Temperature,&Pressure);
ShioHitomi 0:31fa273aa56d 94 altitude = _getAlt(Pressure,Temperature);
ShioHitomi 0:31fa273aa56d 95 alt[j]=altitude;
ShioHitomi 0:31fa273aa56d 96 }
ShioHitomi 0:31fa273aa56d 97 Med_alt=_median(alt,NUM);
ShioHitomi 0:31fa273aa56d 98 //pc.printf("%f,%f\n\r",Med_alt,max);
ShioHitomi 0:31fa273aa56d 99 if(med_alt>max){
ShioHitomi 0:31fa273aa56d 100 max=Med_alt;
ShioHitomi 0:31fa273aa56d 101 Cnt_top=0;
ShioHitomi 0:31fa273aa56d 102 }else if(max-Med_alt>=2.0){
ShioHitomi 0:31fa273aa56d 103 Cnt_top++;
ShioHitomi 0:31fa273aa56d 104 }
ShioHitomi 0:31fa273aa56d 105 //pc.printf("%d\n\r",Cnt_top);
ShioHitomi 0:31fa273aa56d 106 if(Cnt_top>=3||timer_acc.read()>=TIME_TOP){
ShioHitomi 1:9833a893b719 107 //fp = fopen("/local/out.txt", "a");
ShioHitomi 0:31fa273aa56d 108 //pc.printf("頂点判定");
ShioHitomi 1:9833a893b719 109 //fprintf(fp, "%f:頂点判定\n\r",timer_all);
ShioHitomi 0:31fa273aa56d 110 myled1=1;
ShioHitomi 0:31fa273aa56d 111 timer_top.start();
ShioHitomi 1:9833a893b719 112 //tic_buzzer.attach(&_buzzer,1.0/100);
ShioHitomi 0:31fa273aa56d 113 Phase=DROP;
ShioHitomi 0:31fa273aa56d 114 }
ShioHitomi 0:31fa273aa56d 115 break;
ShioHitomi 0:31fa273aa56d 116 case DROP:
ShioHitomi 0:31fa273aa56d 117 Maeno_alt=Med_alt;
ShioHitomi 0:31fa273aa56d 118 int i;
ShioHitomi 0:31fa273aa56d 119 for(i=0;i<NUM;i++){
ShioHitomi 0:31fa273aa56d 120 bmp.ReadData(&Temperature,&Pressure);
ShioHitomi 0:31fa273aa56d 121 altitude = _getAlt(Pressure,Temperature);
ShioHitomi 0:31fa273aa56d 122 alt[i]=altitude;
ShioHitomi 0:31fa273aa56d 123 }
ShioHitomi 0:31fa273aa56d 124 Med_alt=_median(alt,NUM);
ShioHitomi 0:31fa273aa56d 125 //pc.printf("%f,%f\n\r",Med_alt,max);
ShioHitomi 0:31fa273aa56d 126 if(Maeno_alt-Med_alt>=-1.0&&Maeno_alt-Med_alt<=1.0){
ShioHitomi 0:31fa273aa56d 127 Cnt_water++;
ShioHitomi 0:31fa273aa56d 128 }else{
ShioHitomi 0:31fa273aa56d 129 Cnt_water=0;
ShioHitomi 0:31fa273aa56d 130 }
ShioHitomi 0:31fa273aa56d 131 //pc.printf("%d\n\r",Cnt_water);
ShioHitomi 1:9833a893b719 132 if(/*Cnt_water>=3||*/timer_top.read()>=TIME_WATER){
ShioHitomi 1:9833a893b719 133 //fp = fopen("/local/out.txt", "a");
ShioHitomi 0:31fa273aa56d 134 //pc.printf("着水判定");
ShioHitomi 0:31fa273aa56d 135 myled2=1;
ShioHitomi 0:31fa273aa56d 136 heat=0;
ShioHitomi 0:31fa273aa56d 137 buzzer=1;
ShioHitomi 1:9833a893b719 138 //tic_buzzer.attach(&_buzzer,0.5);
ShioHitomi 1:9833a893b719 139 //fprintf(fp, "%f:ヒーターOFF\nブザーON\n\r",timer_all);
ShioHitomi 1:9833a893b719 140 //fclose(fp);
ShioHitomi 0:31fa273aa56d 141 Phase=BUZZER;
ShioHitomi 1:9833a893b719 142 //
ShioHitomi 0:31fa273aa56d 143 tic_mission.detach();
ShioHitomi 0:31fa273aa56d 144 }
ShioHitomi 0:31fa273aa56d 145 //break;
ShioHitomi 0:31fa273aa56d 146
ShioHitomi 0:31fa273aa56d 147 /*case BUZZER:
ShioHitomi 1:9833a893b719 148 if(timer_top.read()>=TIME_BUZZER){
ShioHitomi 0:31fa273aa56d 149 fp = fopen("/local/out.txt", "a");
ShioHitomi 0:31fa273aa56d 150 buzzer=0;
ShioHitomi 0:31fa273aa56d 151 fprintf(fp, "%f:ブザーOFF\n\r",timer_all);
ShioHitomi 0:31fa273aa56d 152 fclose(fp);
ShioHitomi 0:31fa273aa56d 153 }
ShioHitomi 0:31fa273aa56d 154 break;*/
ShioHitomi 1:9833a893b719 155 }
ShioHitomi 0:31fa273aa56d 156 }
ShioHitomi 0:31fa273aa56d 157
ShioHitomi 0:31fa273aa56d 158 void _heater_acc(){
ShioHitomi 0:31fa273aa56d 159 heat=0;
ShioHitomi 0:31fa273aa56d 160 while(1){
ShioHitomi 0:31fa273aa56d 161 if(rec1==1){
ShioHitomi 0:31fa273aa56d 162 heat=1;
ShioHitomi 0:31fa273aa56d 163 }else if(rec1==0){
ShioHitomi 0:31fa273aa56d 164 heat=0;
ShioHitomi 0:31fa273aa56d 165 }
ShioHitomi 0:31fa273aa56d 166 if(rec2==1){
ShioHitomi 0:31fa273aa56d 167 break;
ShioHitomi 0:31fa273aa56d 168 }
ShioHitomi 0:31fa273aa56d 169 }
ShioHitomi 0:31fa273aa56d 170 }
ShioHitomi 0:31fa273aa56d 171
ShioHitomi 0:31fa273aa56d 172 void _buzzer(){
ShioHitomi 0:31fa273aa56d 173 myled3=!myled3;
ShioHitomi 0:31fa273aa56d 174 if(rec2==1){
ShioHitomi 1:9833a893b719 175 buzzer=1;
ShioHitomi 0:31fa273aa56d 176 }else if(rec2==0){
ShioHitomi 0:31fa273aa56d 177 buzzer=0;
ShioHitomi 0:31fa273aa56d 178 }
ShioHitomi 0:31fa273aa56d 179 }
ShioHitomi 0:31fa273aa56d 180
ShioHitomi 0:31fa273aa56d 181 float _getAlt(float press, float temp){
ShioHitomi 0:31fa273aa56d 182 return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
ShioHitomi 0:31fa273aa56d 183 }
ShioHitomi 0:31fa273aa56d 184
ShioHitomi 0:31fa273aa56d 185 float _median(float data[], int num){
ShioHitomi 0:31fa273aa56d 186 float *data_cpy, ans;
ShioHitomi 0:31fa273aa56d 187 data_cpy = new float[num];
ShioHitomi 0:31fa273aa56d 188 memcpy(data_cpy,data,sizeof(float)*num);
ShioHitomi 0:31fa273aa56d 189
ShioHitomi 0:31fa273aa56d 190 for(int i=0; i<num; i++){
ShioHitomi 0:31fa273aa56d 191 for(int j=0; j<num-i-1; j++){
ShioHitomi 0:31fa273aa56d 192 if(data_cpy[j]>data_cpy[j+1]){
ShioHitomi 0:31fa273aa56d 193 float buff = data_cpy[j+1];
ShioHitomi 0:31fa273aa56d 194 data_cpy[j+1] = data_cpy[j];
ShioHitomi 0:31fa273aa56d 195 data_cpy[j] = buff;
ShioHitomi 0:31fa273aa56d 196 }
ShioHitomi 0:31fa273aa56d 197 }
ShioHitomi 0:31fa273aa56d 198 }
ShioHitomi 0:31fa273aa56d 199
ShioHitomi 0:31fa273aa56d 200 if(num%2!=0) ans = data_cpy[num/2];
ShioHitomi 0:31fa273aa56d 201 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
ShioHitomi 0:31fa273aa56d 202 delete[] data_cpy;
ShioHitomi 0:31fa273aa56d 203 return ans;
ShioHitomi 0:31fa273aa56d 204 }