CORE
/
mission
2018.08能代CORE'S キッチン ミッション用プログラム
Fork of mission by
main.cpp@5:b4ebd3587dae, 2018-09-01 (annotated)
- Committer:
- ShioHitomi
- Date:
- Sat Sep 01 11:06:57 2018 +0000
- Revision:
- 5:b4ebd3587dae
- Parent:
- 4:82660534b32d
mission
Who changed what in which revision?
User | Revision | Line number | New 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 | 5:b4ebd3587dae | 6 | #define TIME_TOP 16 |
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 | } |