CORE / Mbed 2 deprecated mission

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

Dependencies:   BMP180 mbed

Fork of mission by hitomi shio

main.cpp@0:31fa273aa56d, 2018-08-03 (annotated)

Committer:
ShioHitomi
Date:
Fri Aug 03 01:09:43 2018 +0000
Revision:
0:31fa273aa56d
Child:
1:9833a893b719
CORE'S???????????

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