CORE / Mbed 2 deprecated mission

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

Dependencies:   BMP180 mbed

Fork of mission by hitomi shio

main.cpp@4:82660534b32d, 2018-08-20 (annotated)

Committer:
ShioHitomi
Date:
Mon Aug 20 16:36:21 2018 +0000
Revision:
4:82660534b32d
Parent:
3:0562feabce7b
Child:
5:b4ebd3587dae
2018.08?????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ShioHitomi 0:31fa273aa56d 1 #include "mbed.h"
ShioHitomi 3:0562feabce7b 2 #include "math.h"
ShioHitomi 0:31fa273aa56d 3 #include "BMP180.h"
ShioHitomi 0:31fa273aa56d 4
ShioHitomi 0:31fa273aa56d 5 #define p0 1013.25f//海面気圧
ShioHitomi 4:82660534b32d 6 #define TIME_TOP 17
ShioHitomi 4:82660534b32d 7 #define TIME_WATER 94
ShioHitomi 4:82660534b32d 8 #define TIME_BUZZER 274
ShioHitomi 0:31fa273aa56d 9 #define NUM 5
ShioHitomi 4:82660534b32d 10 #define RATE 20
ShioHitomi 4:82660534b32d 11 #define JUDGE_ALT 2.0
ShioHitomi 0:31fa273aa56d 12
ShioHitomi 4:82660534b32d 13 enum PHASE{EGG,SERVO,CLOSE,RISE,DROP,BUZZER} Phase;
ShioHitomi 4:82660534b32d 14 enum MOVE{HEAT,CRACK,OPEN,HUTA,TOP,HEAT_BUZZER} Move;
ShioHitomi 4:82660534b32d 15
ShioHitomi 4:82660534b32d 16 PwmOut servo_crack1(p21);
ShioHitomi 4:82660534b32d 17 PwmOut servo_crack2(p22);
ShioHitomi 4:82660534b32d 18 PwmOut servo_open1(p23);
ShioHitomi 4:82660534b32d 19 PwmOut servo_open2(p24);
ShioHitomi 4:82660534b32d 20 DigitalIn rec1(p18);
ShioHitomi 4:82660534b32d 21 DigitalIn rec2(p19);
ShioHitomi 4:82660534b32d 22 DigitalOut heat(p25);
ShioHitomi 4:82660534b32d 23 DigitalOut buzzer(p16);
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 3:0562feabce7b 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 3:0562feabce7b 36
ShioHitomi 3:0562feabce7b 37 bool Save;
ShioHitomi 0:31fa273aa56d 38 void _mission();
ShioHitomi 3:0562feabce7b 39 void _heater_acc();
ShioHitomi 0:31fa273aa56d 40 void _buzzer();
ShioHitomi 3:0562feabce7b 41 void _data();
ShioHitomi 3:0562feabce7b 42 void _servo();
ShioHitomi 0:31fa273aa56d 43 float _getAlt(float press, float temp);
ShioHitomi 0:31fa273aa56d 44 float _median(float data[], int num);
ShioHitomi 0:31fa273aa56d 45 float Med_alt,Maeno_alt;
ShioHitomi 0:31fa273aa56d 46 float Pressure,Temperature,Altitude;
ShioHitomi 4:82660534b32d 47 float alt[NUM],max=-5000;
ShioHitomi 3:0562feabce7b 48 float t;
ShioHitomi 0:31fa273aa56d 49 int Cnt_top=0,Cnt_water=0;
ShioHitomi 3:0562feabce7b 50 int u=0;
ShioHitomi 3:0562feabce7b 51
ShioHitomi 2:b145797d04fb 52 FILE *fp;
ShioHitomi 0:31fa273aa56d 53
ShioHitomi 1:9833a893b719 54 int main(){
ShioHitomi 3:0562feabce7b 55 bmp.Initialize(BMP180_OSS_ULTRA_LOW_POWER);
ShioHitomi 3:0562feabce7b 56 timer_all.start();
ShioHitomi 3:0562feabce7b 57 servo_crack1.period_ms(20);
ShioHitomi 3:0562feabce7b 58 servo_crack2.period_ms(20);
ShioHitomi 3:0562feabce7b 59 servo_open1.period_ms(20);
ShioHitomi 3:0562feabce7b 60 servo_open2.period_ms(20);
ShioHitomi 4:82660534b32d 61 servo_crack1.pulsewidth(0.001);
ShioHitomi 4:82660534b32d 62 servo_crack2.pulsewidth(0.001);
ShioHitomi 4:82660534b32d 63 for(float j=0;j<0.00009;j+=0.00001){
ShioHitomi 4:82660534b32d 64 servo_open1.pulsewidth(0.00099-j);
ShioHitomi 4:82660534b32d 65 wait(0.02);
ShioHitomi 4:82660534b32d 66 servo_open2.pulsewidth(0.00094-j);
ShioHitomi 4:82660534b32d 67 wait(0.02);
ShioHitomi 4:82660534b32d 68 }
ShioHitomi 2:b145797d04fb 69 fp = fopen("/local/out.txt", "a");
ShioHitomi 2:b145797d04fb 70 fprintf(fp,"%f:Hello,World\n\r");
ShioHitomi 3:0562feabce7b 71 fclose(fp);
ShioHitomi 1:9833a893b719 72 buzzer=0;
ShioHitomi 1:9833a893b719 73 _heater_acc();
ShioHitomi 3:0562feabce7b 74 Phase=EGG;
ShioHitomi 3:0562feabce7b 75 tic_mission.attach(&_mission,1.0/20);
ShioHitomi 3:0562feabce7b 76 while(1){
ShioHitomi 3:0562feabce7b 77 if(Save){
ShioHitomi 3:0562feabce7b 78 _data();
ShioHitomi 3:0562feabce7b 79 }
ShioHitomi 4:82660534b32d 80 if(timer_top.read()>=TIME_BUZZER){
ShioHitomi 0:31fa273aa56d 81 buzzer=0;
ShioHitomi 4:82660534b32d 82 }
ShioHitomi 3:0562feabce7b 83 }
ShioHitomi 0:31fa273aa56d 84 }
ShioHitomi 0:31fa273aa56d 85
ShioHitomi 0:31fa273aa56d 86
ShioHitomi 0:31fa273aa56d 87 void _mission(){
ShioHitomi 3:0562feabce7b 88 if(Phase==BUZZER){
ShioHitomi 2:b145797d04fb 89 _buzzer();
ShioHitomi 2:b145797d04fb 90 }
ShioHitomi 1:9833a893b719 91 myled4=!myled4;
ShioHitomi 0:31fa273aa56d 92 switch(Phase){
ShioHitomi 0:31fa273aa56d 93 case EGG:
ShioHitomi 0:31fa273aa56d 94 myled1=0;
ShioHitomi 0:31fa273aa56d 95 myled2=0;
ShioHitomi 4:82660534b32d 96 for(float i=0;i<0.00045;i+=0.00005){
ShioHitomi 4:82660534b32d 97 servo_crack1.pulsewidth(0.001+i);
ShioHitomi 4:82660534b32d 98 wait(0.02);
ShioHitomi 4:82660534b32d 99 servo_crack2.pulsewidth(0.001+i);
ShioHitomi 4:82660534b32d 100 wait(0.02);
ShioHitomi 4:82660534b32d 101 }
ShioHitomi 3:0562feabce7b 102 t=timer_all.read();
ShioHitomi 3:0562feabce7b 103 Move=CRACK;
ShioHitomi 4:82660534b32d 104 _data();
ShioHitomi 3:0562feabce7b 105 Phase=SERVO;
ShioHitomi 4:82660534b32d 106 break;
ShioHitomi 3:0562feabce7b 107
ShioHitomi 4:82660534b32d 108 case SERVO:
ShioHitomi 4:82660534b32d 109 if(timer_all.read()-t>=0.5){
ShioHitomi 4:82660534b32d 110 for(float j=0;j<0.00009;j+=0.00001){
ShioHitomi 4:82660534b32d 111 servo_open1.pulsewidth(0.0009+j);
ShioHitomi 4:82660534b32d 112 wait(0.02);
ShioHitomi 4:82660534b32d 113 servo_open2.pulsewidth(0.00085+j);
ShioHitomi 4:82660534b32d 114 wait(0.02);
ShioHitomi 4:82660534b32d 115 }
ShioHitomi 3:0562feabce7b 116 Move=OPEN;
ShioHitomi 4:82660534b32d 117 _data();
ShioHitomi 3:0562feabce7b 118 Save=true;
ShioHitomi 4:82660534b32d 119 Phase=CLOSE;
ShioHitomi 4:82660534b32d 120 }
ShioHitomi 4:82660534b32d 121 break;
ShioHitomi 4:82660534b32d 122
ShioHitomi 4:82660534b32d 123 case CLOSE:
ShioHitomi 4:82660534b32d 124 if(timer_all.read()-t>=1.5){
ShioHitomi 4:82660534b32d 125 for(float j=0;j<0.00009;j+=0.00001){
ShioHitomi 4:82660534b32d 126 servo_open1.pulsewidth(0.00099-j);
ShioHitomi 4:82660534b32d 127 wait(0.02);
ShioHitomi 4:82660534b32d 128 servo_open2.pulsewidth(0.00094-j);
ShioHitomi 4:82660534b32d 129 wait(0.02);
ShioHitomi 4:82660534b32d 130 }
ShioHitomi 4:82660534b32d 131 Move=HUTA;
ShioHitomi 4:82660534b32d 132 _data();
ShioHitomi 4:82660534b32d 133 Save=true;
ShioHitomi 3:0562feabce7b 134 Phase=RISE;
ShioHitomi 3:0562feabce7b 135 }
ShioHitomi 1:9833a893b719 136 break;
ShioHitomi 4:82660534b32d 137
ShioHitomi 1:9833a893b719 138 case RISE:
ShioHitomi 3:0562feabce7b 139 for(int j=0;j<NUM;j++){
ShioHitomi 0:31fa273aa56d 140 bmp.ReadData(&Temperature,&Pressure);
ShioHitomi 3:0562feabce7b 141 alt[j]=_getAlt(Pressure,Temperature);
ShioHitomi 0:31fa273aa56d 142 }
ShioHitomi 0:31fa273aa56d 143 Med_alt=_median(alt,NUM);
ShioHitomi 3:0562feabce7b 144 if(Med_alt>max){
ShioHitomi 0:31fa273aa56d 145 max=Med_alt;
ShioHitomi 0:31fa273aa56d 146 Cnt_top=0;
ShioHitomi 4:82660534b32d 147 }else if(max-Med_alt>=JUDGE_ALT){
ShioHitomi 4:82660534b32d 148 Cnt_top++;
ShioHitomi 0:31fa273aa56d 149 }
ShioHitomi 0:31fa273aa56d 150 if(Cnt_top>=3||timer_acc.read()>=TIME_TOP){
ShioHitomi 0:31fa273aa56d 151 myled1=1;
ShioHitomi 0:31fa273aa56d 152 timer_top.start();
ShioHitomi 0:31fa273aa56d 153 Phase=DROP;
ShioHitomi 3:0562feabce7b 154 Move=TOP;
ShioHitomi 3:0562feabce7b 155 Save=true;
ShioHitomi 3:0562feabce7b 156 _data();
ShioHitomi 0:31fa273aa56d 157 }
ShioHitomi 0:31fa273aa56d 158 break;
ShioHitomi 0:31fa273aa56d 159 case DROP:
ShioHitomi 0:31fa273aa56d 160 Maeno_alt=Med_alt;
ShioHitomi 3:0562feabce7b 161 for(int i=0;i<NUM;i++){
ShioHitomi 0:31fa273aa56d 162 bmp.ReadData(&Temperature,&Pressure);
ShioHitomi 3:0562feabce7b 163 alt[i]=_getAlt(Pressure,Temperature);
ShioHitomi 0:31fa273aa56d 164 }
ShioHitomi 0:31fa273aa56d 165 Med_alt=_median(alt,NUM);
ShioHitomi 4:82660534b32d 166 if(Maeno_alt-Med_alt>=-5.0&&Maeno_alt-Med_alt<=5.0){
ShioHitomi 0:31fa273aa56d 167 Cnt_water++;
ShioHitomi 0:31fa273aa56d 168 }else{
ShioHitomi 0:31fa273aa56d 169 Cnt_water=0;
ShioHitomi 0:31fa273aa56d 170 }
ShioHitomi 4:82660534b32d 171 if(Cnt_water>=3||timer_top.read()>=TIME_WATER){
ShioHitomi 0:31fa273aa56d 172 myled2=1;
ShioHitomi 3:0562feabce7b 173 buzzer=1;
ShioHitomi 4:82660534b32d 174 //heat=0;
ShioHitomi 3:0562feabce7b 175 Move=HEAT_BUZZER;
ShioHitomi 4:82660534b32d 176 _data();
ShioHitomi 3:0562feabce7b 177 Save=true;
ShioHitomi 0:31fa273aa56d 178 Phase=BUZZER;
ShioHitomi 0:31fa273aa56d 179 }
ShioHitomi 4:82660534b32d 180 break;
ShioHitomi 3:0562feabce7b 181 }
ShioHitomi 0:31fa273aa56d 182 }
ShioHitomi 0:31fa273aa56d 183
ShioHitomi 0:31fa273aa56d 184 void _heater_acc(){
ShioHitomi 0:31fa273aa56d 185 heat=0;
ShioHitomi 4:82660534b32d 186 while(1){
ShioHitomi 3:0562feabce7b 187 if(u!=rec1){
ShioHitomi 3:0562feabce7b 188 heat=!heat;
ShioHitomi 3:0562feabce7b 189 u=rec1;
ShioHitomi 3:0562feabce7b 190 Move=HEAT;
ShioHitomi 3:0562feabce7b 191 Save=true;
ShioHitomi 3:0562feabce7b 192 _data();
ShioHitomi 3:0562feabce7b 193 }
ShioHitomi 4:82660534b32d 194 if(rec2==1){
ShioHitomi 0:31fa273aa56d 195 heat=1;
ShioHitomi 3:0562feabce7b 196 timer_acc.start();
ShioHitomi 0:31fa273aa56d 197 break;
ShioHitomi 0:31fa273aa56d 198 }
ShioHitomi 0:31fa273aa56d 199 }
ShioHitomi 0:31fa273aa56d 200 }
ShioHitomi 0:31fa273aa56d 201
ShioHitomi 0:31fa273aa56d 202 void _buzzer(){
ShioHitomi 0:31fa273aa56d 203 myled3=!myled3;
ShioHitomi 0:31fa273aa56d 204 if(rec2==1){
ShioHitomi 1:9833a893b719 205 buzzer=1;
ShioHitomi 0:31fa273aa56d 206 }else if(rec2==0){
ShioHitomi 0:31fa273aa56d 207 buzzer=0;
ShioHitomi 0:31fa273aa56d 208 }
ShioHitomi 0:31fa273aa56d 209 }
ShioHitomi 0:31fa273aa56d 210
ShioHitomi 0:31fa273aa56d 211 float _getAlt(float press, float temp){
ShioHitomi 0:31fa273aa56d 212 return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
ShioHitomi 0:31fa273aa56d 213 }
ShioHitomi 0:31fa273aa56d 214
ShioHitomi 0:31fa273aa56d 215 float _median(float data[], int num){
ShioHitomi 0:31fa273aa56d 216 float *data_cpy, ans;
ShioHitomi 2:b145797d04fb 217 data_cpy = new float[num];
ShioHitomi 0:31fa273aa56d 218 memcpy(data_cpy,data,sizeof(float)*num);
ShioHitomi 0:31fa273aa56d 219
ShioHitomi 0:31fa273aa56d 220 for(int i=0; i<num; i++){
ShioHitomi 0:31fa273aa56d 221 for(int j=0; j<num-i-1; j++){
ShioHitomi 0:31fa273aa56d 222 if(data_cpy[j]>data_cpy[j+1]){
ShioHitomi 0:31fa273aa56d 223 float buff = data_cpy[j+1];
ShioHitomi 0:31fa273aa56d 224 data_cpy[j+1] = data_cpy[j];
ShioHitomi 0:31fa273aa56d 225 data_cpy[j] = buff;
ShioHitomi 0:31fa273aa56d 226 }
ShioHitomi 0:31fa273aa56d 227 }
ShioHitomi 0:31fa273aa56d 228 }
ShioHitomi 0:31fa273aa56d 229
ShioHitomi 0:31fa273aa56d 230 if(num%2!=0) ans = data_cpy[num/2];
ShioHitomi 0:31fa273aa56d 231 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
ShioHitomi 0:31fa273aa56d 232 delete[] data_cpy;
ShioHitomi 0:31fa273aa56d 233 return ans;
ShioHitomi 3:0562feabce7b 234 }
ShioHitomi 3:0562feabce7b 235
ShioHitomi 3:0562feabce7b 236 void _data(){
ShioHitomi 3:0562feabce7b 237 Save=false;
ShioHitomi 3:0562feabce7b 238 fp = fopen("/local/out.txt", "a");
ShioHitomi 3:0562feabce7b 239 fprintf(fp,"%f:%d\n\r",timer_all.read(),Move);
ShioHitomi 3:0562feabce7b 240 fclose(fp);
ShioHitomi 4:82660534b32d 241 }