update
Dependencies: Stepper mbed SDFileSystem Ultrasonic PinDetect millis
main.cpp@9:f72ddee31041, 2019-01-18 (annotated)
- Committer:
- rschimpf78
- Date:
- Fri Jan 18 19:40:19 2019 +0000
- Revision:
- 9:f72ddee31041
- Parent:
- 8:3f4d78017fd9
update
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rschimpf78 | 2:e8f8095464cf | 1 | #include "mbed.h" |
rschimpf78 | 2:e8f8095464cf | 2 | #include "Stepper.h" |
rschimpf78 | 2:e8f8095464cf | 3 | #include "string" |
rschimpf78 | 3:fd2ca631ab44 | 4 | #include "hcsr04.h" |
rschimpf78 | 6:354264d1e4bb | 5 | #include "millis.h" |
rschimpf78 | 7:ede305192e3b | 6 | #include "SDFileSystem.h" |
rschimpf78 | 7:ede305192e3b | 7 | |
rschimpf78 | 7:ede305192e3b | 8 | SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd"); // mosi, miso, sclk, cs, name |
rschimpf78 | 3:fd2ca631ab44 | 9 | Serial pc(USBTX, USBRX); |
rschimpf78 | 7:ede305192e3b | 10 | HCSR04 pathsensor(PTC2,PTA2); //(trig,echo) |
rschimpf78 | 7:ede305192e3b | 11 | HCSR04 heightsensor(PTD2,PTD3); |
rschimpf78 | 3:fd2ca631ab44 | 12 | DigitalOut in1(PTC12); |
rschimpf78 | 3:fd2ca631ab44 | 13 | DigitalOut in2(D7); |
rschimpf78 | 3:fd2ca631ab44 | 14 | DigitalIn homeSwitch(D9); |
rschimpf78 | 7:ede305192e3b | 15 | DigitalIn magnetSwitch(D10); |
rschimpf78 | 3:fd2ca631ab44 | 16 | Stepper mot(D3,D4); //(D3 to PUL+, D4 to DIR+) |
rschimpf78 | 2:e8f8095464cf | 17 | DigitalOut en(D2); |
rschimpf78 | 0:88834eed3de0 | 18 | |
rschimpf78 | 7:ede305192e3b | 19 | volatile int plates = 0; |
rschimpf78 | 3:fd2ca631ab44 | 20 | volatile unsigned int sheets=0; |
rschimpf78 | 3:fd2ca631ab44 | 21 | volatile unsigned int path_sheets=0; |
rschimpf78 | 7:ede305192e3b | 22 | volatile int stop_error=0; |
rschimpf78 | 7:ede305192e3b | 23 | volatile int error_distance; |
rschimpf78 | 7:ede305192e3b | 24 | volatile int beam_distance; |
rschimpf78 | 7:ede305192e3b | 25 | volatile int broken_switch; |
rschimpf78 | 7:ede305192e3b | 26 | volatile int stage = 1; |
rschimpf78 | 7:ede305192e3b | 27 | |
rschimpf78 | 7:ede305192e3b | 28 | void switch_check(){ |
rschimpf78 | 7:ede305192e3b | 29 | stage = 1; |
rschimpf78 | 7:ede305192e3b | 30 | pc.printf("\nSwitch Check "); |
rschimpf78 | 7:ede305192e3b | 31 | if (homeSwitch == 1 && magnetSwitch == 1){ |
rschimpf78 | 7:ede305192e3b | 32 | stop_error = 1; |
rschimpf78 | 7:ede305192e3b | 33 | broken_switch = 2; //both switches broken |
rschimpf78 | 7:ede305192e3b | 34 | } |
rschimpf78 | 7:ede305192e3b | 35 | else if (homeSwitch == 0){ |
rschimpf78 | 7:ede305192e3b | 36 | stop_error = 1; |
rschimpf78 | 7:ede305192e3b | 37 | broken_switch = 1; //magnet switch isn't working |
rschimpf78 | 7:ede305192e3b | 38 | } |
rschimpf78 | 7:ede305192e3b | 39 | else{ |
rschimpf78 | 7:ede305192e3b | 40 | broken_switch = 0; |
rschimpf78 | 9:f72ddee31041 | 41 | stop_error = 0; |
rschimpf78 | 7:ede305192e3b | 42 | } |
rschimpf78 | 7:ede305192e3b | 43 | } |
rschimpf78 | 0:88834eed3de0 | 44 | |
rschimpf78 | 7:ede305192e3b | 45 | void plate_selection(){ |
rschimpf78 | 7:ede305192e3b | 46 | stage = 2; |
rschimpf78 | 3:fd2ca631ab44 | 47 | unsigned int dist; |
rschimpf78 | 3:fd2ca631ab44 | 48 | int er = 5; |
rschimpf78 | 7:ede305192e3b | 49 | int finp = 447; //sensor height initial |
rschimpf78 | 7:ede305192e3b | 50 | int tp = 25; |
rschimpf78 | 7:ede305192e3b | 51 | wait(1); |
rschimpf78 | 7:ede305192e3b | 52 | in1=1; |
rschimpf78 | 7:ede305192e3b | 53 | in2=0; |
rschimpf78 | 7:ede305192e3b | 54 | wait_ms(900); //stair fix |
rschimpf78 | 7:ede305192e3b | 55 | in1=1; |
rschimpf78 | 7:ede305192e3b | 56 | in2=1; |
rschimpf78 | 7:ede305192e3b | 57 | wait(1); |
rschimpf78 | 7:ede305192e3b | 58 | millisStart(); |
rschimpf78 | 7:ede305192e3b | 59 | long Duration2 = 0; |
rschimpf78 | 7:ede305192e3b | 60 | long StartTime2 = millis(); |
rschimpf78 | 2:e8f8095464cf | 61 | |
rschimpf78 | 7:ede305192e3b | 62 | while (plates!=sheets && Duration2 <= 55000) { |
rschimpf78 | 7:ede305192e3b | 63 | |
rschimpf78 | 3:fd2ca631ab44 | 64 | heightsensor.start(); |
rschimpf78 | 7:ede305192e3b | 65 | wait_ms(250); |
rschimpf78 | 3:fd2ca631ab44 | 66 | dist=heightsensor.get_dist_cm(); |
rschimpf78 | 3:fd2ca631ab44 | 67 | pc.printf("\nmm:%ld",dist); |
rschimpf78 | 7:ede305192e3b | 68 | long CurrentTime2 = millis(); |
rschimpf78 | 7:ede305192e3b | 69 | Duration2 = CurrentTime2-StartTime2; |
rschimpf78 | 3:fd2ca631ab44 | 70 | |
rschimpf78 | 7:ede305192e3b | 71 | if ( dist < 30 || dist > 500){ |
rschimpf78 | 7:ede305192e3b | 72 | error_distance = dist; |
rschimpf78 | 7:ede305192e3b | 73 | pc.printf("\n Distance Error: %ld", error_distance); |
rschimpf78 | 7:ede305192e3b | 74 | stop_error = 2; |
rschimpf78 | 7:ede305192e3b | 75 | break; |
rschimpf78 | 7:ede305192e3b | 76 | } |
rschimpf78 | 7:ede305192e3b | 77 | // pc.printf("\nin while loop"); |
rschimpf78 | 7:ede305192e3b | 78 | if (plates<sheets){ |
rschimpf78 | 3:fd2ca631ab44 | 79 | en = 0; |
rschimpf78 | 7:ede305192e3b | 80 | mot.setSpeed(500); |
rschimpf78 | 3:fd2ca631ab44 | 81 | mot.rotate(1); |
rschimpf78 | 7:ede305192e3b | 82 | } |
rschimpf78 | 7:ede305192e3b | 83 | if (plates>sheets){ |
rschimpf78 | 3:fd2ca631ab44 | 84 | en = 0; |
rschimpf78 | 7:ede305192e3b | 85 | mot.setSpeed(500); |
rschimpf78 | 3:fd2ca631ab44 | 86 | mot.rotate(0); |
rschimpf78 | 7:ede305192e3b | 87 | } |
rschimpf78 | 7:ede305192e3b | 88 | if (dist<=finp+er && dist>=finp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 89 | sheets = 1; |
rschimpf78 | 7:ede305192e3b | 90 | } |
rschimpf78 | 7:ede305192e3b | 91 | if (dist<= finp-tp+er && dist>=finp-tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 92 | sheets = 2; |
rschimpf78 | 7:ede305192e3b | 93 | } |
rschimpf78 | 7:ede305192e3b | 94 | if (dist<= finp-2*tp+er && dist>=finp-2*tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 95 | sheets = 3; |
rschimpf78 | 3:fd2ca631ab44 | 96 | } |
rschimpf78 | 7:ede305192e3b | 97 | if (dist<= finp-3*tp+er && dist>=finp-3*tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 98 | sheets = 4; |
rschimpf78 | 3:fd2ca631ab44 | 99 | } |
rschimpf78 | 7:ede305192e3b | 100 | if (dist<= finp-4*tp+er && dist>=finp-4*tp-er){ |
rschimpf78 | 7:ede305192e3b | 101 | sheets = 5; |
rschimpf78 | 7:ede305192e3b | 102 | |
rschimpf78 | 7:ede305192e3b | 103 | } |
rschimpf78 | 7:ede305192e3b | 104 | if (dist<= finp-5*tp+er && dist>=finp-5*tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 105 | sheets = 6; |
rschimpf78 | 7:ede305192e3b | 106 | |
rschimpf78 | 3:fd2ca631ab44 | 107 | } |
rschimpf78 | 7:ede305192e3b | 108 | if (dist<= finp-6*tp+er && dist>=finp-6*tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 109 | sheets = 7; |
rschimpf78 | 7:ede305192e3b | 110 | |
rschimpf78 | 3:fd2ca631ab44 | 111 | } |
rschimpf78 | 7:ede305192e3b | 112 | if (dist<= finp-7*tp+er && dist>=finp-7*tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 113 | sheets = 8; |
rschimpf78 | 7:ede305192e3b | 114 | |
rschimpf78 | 3:fd2ca631ab44 | 115 | } |
rschimpf78 | 7:ede305192e3b | 116 | if (dist<= finp-8*tp+er && dist>=finp-8*tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 117 | sheets = 9; |
rschimpf78 | 7:ede305192e3b | 118 | |
rschimpf78 | 3:fd2ca631ab44 | 119 | } |
rschimpf78 | 7:ede305192e3b | 120 | if (dist<= finp-9*tp+er && dist>=finp-9*tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 121 | sheets = 10; |
rschimpf78 | 7:ede305192e3b | 122 | |
rschimpf78 | 3:fd2ca631ab44 | 123 | } |
rschimpf78 | 7:ede305192e3b | 124 | if (dist<= finp-10*tp+er && dist>=finp-10*tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 125 | sheets = 11; |
rschimpf78 | 7:ede305192e3b | 126 | |
rschimpf78 | 3:fd2ca631ab44 | 127 | } |
rschimpf78 | 7:ede305192e3b | 128 | if (dist<= finp-11*tp+er && dist>=finp-11*tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 129 | sheets = 12; |
rschimpf78 | 7:ede305192e3b | 130 | |
rschimpf78 | 3:fd2ca631ab44 | 131 | } |
rschimpf78 | 7:ede305192e3b | 132 | if (dist<= finp-12*tp+er && dist>=finp-12*tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 133 | sheets = 13; |
rschimpf78 | 7:ede305192e3b | 134 | |
rschimpf78 | 3:fd2ca631ab44 | 135 | } |
rschimpf78 | 7:ede305192e3b | 136 | if (dist<= finp-13*tp+er && dist>=finp-13*tp-er){ |
rschimpf78 | 3:fd2ca631ab44 | 137 | sheets = 14; |
rschimpf78 | 7:ede305192e3b | 138 | |
rschimpf78 | 3:fd2ca631ab44 | 139 | } |
rschimpf78 | 7:ede305192e3b | 140 | if (dist<= finp-14*tp+er && dist>=finp-14*tp-er){ //102 to 112 |
rschimpf78 | 3:fd2ca631ab44 | 141 | sheets = 15; |
rschimpf78 | 7:ede305192e3b | 142 | |
rschimpf78 | 3:fd2ca631ab44 | 143 | } |
rschimpf78 | 7:ede305192e3b | 144 | if (dist<= 70 && dist>= 60){ //77 to 87 |
rschimpf78 | 7:ede305192e3b | 145 | sheets = 16; |
rschimpf78 | 3:fd2ca631ab44 | 146 | } |
rschimpf78 | 3:fd2ca631ab44 | 147 | } |
rschimpf78 | 3:fd2ca631ab44 | 148 | mot.stop(); |
rschimpf78 | 3:fd2ca631ab44 | 149 | en = 1; |
rschimpf78 | 3:fd2ca631ab44 | 150 | wait(1); |
rschimpf78 | 7:ede305192e3b | 151 | if (Duration2 > 55000){ |
rschimpf78 | 7:ede305192e3b | 152 | stop_error = 3; |
rschimpf78 | 7:ede305192e3b | 153 | } |
rschimpf78 | 3:fd2ca631ab44 | 154 | } |
rschimpf78 | 3:fd2ca631ab44 | 155 | |
rschimpf78 | 3:fd2ca631ab44 | 156 | void beam() |
rschimpf78 | 7:ede305192e3b | 157 | { |
rschimpf78 | 7:ede305192e3b | 158 | stage = 3; |
rschimpf78 | 3:fd2ca631ab44 | 159 | unsigned int path_dist; |
rschimpf78 | 7:ede305192e3b | 160 | int err = 12; |
rschimpf78 | 7:ede305192e3b | 161 | int sens_pos = 131; |
rschimpf78 | 3:fd2ca631ab44 | 162 | int sheet_th = 25; |
rschimpf78 | 3:fd2ca631ab44 | 163 | pathsensor.start(); |
rschimpf78 | 7:ede305192e3b | 164 | wait_ms(250); |
rschimpf78 | 3:fd2ca631ab44 | 165 | path_dist=pathsensor.get_dist_cm(); |
rschimpf78 | 7:ede305192e3b | 166 | pc.printf("\nBeam Path Sensor: %ld",path_dist); |
rschimpf78 | 7:ede305192e3b | 167 | |
rschimpf78 | 7:ede305192e3b | 168 | if (path_dist < 110){ |
rschimpf78 | 7:ede305192e3b | 169 | beam_distance = path_dist; |
rschimpf78 | 7:ede305192e3b | 170 | stop_error = 6; |
rschimpf78 | 7:ede305192e3b | 171 | } |
rschimpf78 | 7:ede305192e3b | 172 | |
rschimpf78 | 9:f72ddee31041 | 173 | else if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){ //119 143 |
rschimpf78 | 3:fd2ca631ab44 | 174 | path_sheets = 16; |
rschimpf78 | 3:fd2ca631ab44 | 175 | |
rschimpf78 | 3:fd2ca631ab44 | 176 | } |
rschimpf78 | 9:f72ddee31041 | 177 | else if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){ |
rschimpf78 | 3:fd2ca631ab44 | 178 | path_sheets = 15; |
rschimpf78 | 3:fd2ca631ab44 | 179 | |
rschimpf78 | 3:fd2ca631ab44 | 180 | } |
rschimpf78 | 9:f72ddee31041 | 181 | else if (path_dist >= sens_pos-err+2*sheet_th && path_dist <= sens_pos+err+2*sheet_th){ //179-191 |
rschimpf78 | 3:fd2ca631ab44 | 182 | path_sheets = 14; |
rschimpf78 | 3:fd2ca631ab44 | 183 | |
rschimpf78 | 3:fd2ca631ab44 | 184 | } |
rschimpf78 | 9:f72ddee31041 | 185 | else if (path_dist >= sens_pos-err+3*sheet_th && path_dist <= sens_pos+err+3*sheet_th){ //205-217 |
rschimpf78 | 3:fd2ca631ab44 | 186 | path_sheets = 13; |
rschimpf78 | 3:fd2ca631ab44 | 187 | |
rschimpf78 | 3:fd2ca631ab44 | 188 | } |
rschimpf78 | 9:f72ddee31041 | 189 | else if (path_dist >= sens_pos-err+4*sheet_th && path_dist <= sens_pos+err+4*sheet_th){ //231-243 |
rschimpf78 | 3:fd2ca631ab44 | 190 | path_sheets = 12; |
rschimpf78 | 3:fd2ca631ab44 | 191 | |
rschimpf78 | 3:fd2ca631ab44 | 192 | } |
rschimpf78 | 9:f72ddee31041 | 193 | else if (path_dist >= sens_pos-err+5*sheet_th && path_dist <= sens_pos+err+5*sheet_th){ //257-269 |
rschimpf78 | 3:fd2ca631ab44 | 194 | path_sheets = 11; |
rschimpf78 | 3:fd2ca631ab44 | 195 | |
rschimpf78 | 3:fd2ca631ab44 | 196 | } |
rschimpf78 | 9:f72ddee31041 | 197 | else if (path_dist >= sens_pos-err+6*sheet_th && path_dist <= sens_pos+err+6*sheet_th){ //283-295 |
rschimpf78 | 3:fd2ca631ab44 | 198 | path_sheets = 10; |
rschimpf78 | 3:fd2ca631ab44 | 199 | |
rschimpf78 | 3:fd2ca631ab44 | 200 | } |
rschimpf78 | 9:f72ddee31041 | 201 | else if (path_dist >= sens_pos-err+7*sheet_th && path_dist <= sens_pos+err+7*sheet_th){ //309-321 |
rschimpf78 | 3:fd2ca631ab44 | 202 | path_sheets = 9; |
rschimpf78 | 3:fd2ca631ab44 | 203 | |
rschimpf78 | 3:fd2ca631ab44 | 204 | } |
rschimpf78 | 9:f72ddee31041 | 205 | else if (path_dist >= sens_pos-err+8*sheet_th && path_dist <= sens_pos+err+8*sheet_th){ //323-333 |
rschimpf78 | 3:fd2ca631ab44 | 206 | path_sheets = 8; |
rschimpf78 | 3:fd2ca631ab44 | 207 | |
rschimpf78 | 3:fd2ca631ab44 | 208 | } |
rschimpf78 | 9:f72ddee31041 | 209 | else if (path_dist >= sens_pos-err+9*sheet_th && path_dist <= sens_pos+err+9*sheet_th){ //349-359 |
rschimpf78 | 3:fd2ca631ab44 | 210 | path_sheets = 7; |
rschimpf78 | 3:fd2ca631ab44 | 211 | |
rschimpf78 | 3:fd2ca631ab44 | 212 | } |
rschimpf78 | 9:f72ddee31041 | 213 | else if (path_dist >= sens_pos-err+10*sheet_th && path_dist <= sens_pos+err+10*sheet_th){ |
rschimpf78 | 3:fd2ca631ab44 | 214 | path_sheets = 6; |
rschimpf78 | 3:fd2ca631ab44 | 215 | |
rschimpf78 | 3:fd2ca631ab44 | 216 | } |
rschimpf78 | 9:f72ddee31041 | 217 | else if (path_dist >= sens_pos-err+11*sheet_th && path_dist <= sens_pos+err+11*sheet_th){ |
rschimpf78 | 3:fd2ca631ab44 | 218 | path_sheets = 5; |
rschimpf78 | 3:fd2ca631ab44 | 219 | |
rschimpf78 | 3:fd2ca631ab44 | 220 | } |
rschimpf78 | 9:f72ddee31041 | 221 | else if (path_dist >= sens_pos-err+12*sheet_th && path_dist <= sens_pos+err+12*sheet_th){ |
rschimpf78 | 3:fd2ca631ab44 | 222 | path_sheets = 4; |
rschimpf78 | 3:fd2ca631ab44 | 223 | |
rschimpf78 | 3:fd2ca631ab44 | 224 | } |
rschimpf78 | 9:f72ddee31041 | 225 | else if (path_dist >= sens_pos-err+13*sheet_th && path_dist <= sens_pos+err+13*sheet_th){ |
rschimpf78 | 3:fd2ca631ab44 | 226 | path_sheets = 3; |
rschimpf78 | 3:fd2ca631ab44 | 227 | |
rschimpf78 | 3:fd2ca631ab44 | 228 | } |
rschimpf78 | 9:f72ddee31041 | 229 | else if (path_dist >= sens_pos-err+14*sheet_th && path_dist <= sens_pos+err+14*sheet_th){ |
rschimpf78 | 3:fd2ca631ab44 | 230 | path_sheets = 2; |
rschimpf78 | 3:fd2ca631ab44 | 231 | |
rschimpf78 | 3:fd2ca631ab44 | 232 | } |
rschimpf78 | 9:f72ddee31041 | 233 | else if (path_dist >= sens_pos-10-err+15*sheet_th && path_dist <= sens_pos-10+err+15*sheet_th){ |
rschimpf78 | 3:fd2ca631ab44 | 234 | path_sheets = 1; |
rschimpf78 | 3:fd2ca631ab44 | 235 | |
rschimpf78 | 3:fd2ca631ab44 | 236 | } |
rschimpf78 | 9:f72ddee31041 | 237 | else if (path_dist > sens_pos-err+16*sheet_th){ |
rschimpf78 | 3:fd2ca631ab44 | 238 | path_sheets = 0; |
rschimpf78 | 7:ede305192e3b | 239 | } |
rschimpf78 | 9:f72ddee31041 | 240 | else { |
rschimpf78 | 9:f72ddee31041 | 241 | path_sheets = 17; |
rschimpf78 | 9:f72ddee31041 | 242 | } |
rschimpf78 | 3:fd2ca631ab44 | 243 | } |
rschimpf78 | 6:354264d1e4bb | 244 | |
rschimpf78 | 3:fd2ca631ab44 | 245 | void retract() |
rschimpf78 | 0:88834eed3de0 | 246 | { |
rschimpf78 | 7:ede305192e3b | 247 | stage = 4; |
rschimpf78 | 6:354264d1e4bb | 248 | millisStart(); |
rschimpf78 | 6:354264d1e4bb | 249 | long Duration = 0; |
rschimpf78 | 6:354264d1e4bb | 250 | long StartTime = millis(); |
rschimpf78 | 6:354264d1e4bb | 251 | |
rschimpf78 | 8:3f4d78017fd9 | 252 | while(homeSwitch == 1 && magnetSwitch == 1 && Duration <= 60000 ) |
rschimpf78 | 2:e8f8095464cf | 253 | { |
rschimpf78 | 0:88834eed3de0 | 254 | in1=0; |
rschimpf78 | 2:e8f8095464cf | 255 | in2=1; |
rschimpf78 | 6:354264d1e4bb | 256 | long CurrentTime = millis(); |
rschimpf78 | 6:354264d1e4bb | 257 | Duration = CurrentTime-StartTime; |
rschimpf78 | 3:fd2ca631ab44 | 258 | } |
rschimpf78 | 2:e8f8095464cf | 259 | in1=1; |
rschimpf78 | 2:e8f8095464cf | 260 | in2=1; |
rschimpf78 | 8:3f4d78017fd9 | 261 | if (Duration > 60000){ |
rschimpf78 | 7:ede305192e3b | 262 | stop_error = 5; |
rschimpf78 | 7:ede305192e3b | 263 | } |
rschimpf78 | 8:3f4d78017fd9 | 264 | else{ |
rschimpf78 | 8:3f4d78017fd9 | 265 | stop_error = 0; |
rschimpf78 | 8:3f4d78017fd9 | 266 | } |
rschimpf78 | 0:88834eed3de0 | 267 | } |
rschimpf78 | 0:88834eed3de0 | 268 | |
rschimpf78 | 3:fd2ca631ab44 | 269 | void send() |
rschimpf78 | 3:fd2ca631ab44 | 270 | { |
rschimpf78 | 3:fd2ca631ab44 | 271 | in1=1; |
rschimpf78 | 3:fd2ca631ab44 | 272 | in2=0; |
rschimpf78 | 8:3f4d78017fd9 | 273 | wait(58); |
rschimpf78 | 0:88834eed3de0 | 274 | in1=1; |
rschimpf78 | 2:e8f8095464cf | 275 | in2=1; |
rschimpf78 | 0:88834eed3de0 | 276 | } |
rschimpf78 | 7:ede305192e3b | 277 | |
rschimpf78 | 3:fd2ca631ab44 | 278 | |
rschimpf78 | 7:ede305192e3b | 279 | void logwriting() |
rschimpf78 | 3:fd2ca631ab44 | 280 | { |
rschimpf78 | 7:ede305192e3b | 281 | FILE *fp = fopen("/sd/mydir/sdtest.txt", "a"); |
rschimpf78 | 7:ede305192e3b | 282 | if(fp == NULL) { |
rschimpf78 | 7:ede305192e3b | 283 | pc.printf("\nError Writing to Log "); |
rschimpf78 | 7:ede305192e3b | 284 | } |
rschimpf78 | 7:ede305192e3b | 285 | pc.printf("\nLogging "); |
rschimpf78 | 7:ede305192e3b | 286 | pc.printf("\nStop Error: %ld ", stop_error); |
rschimpf78 | 7:ede305192e3b | 287 | |
rschimpf78 | 7:ede305192e3b | 288 | switch (stop_error){ |
rschimpf78 | 7:ede305192e3b | 289 | case 1: |
rschimpf78 | 7:ede305192e3b | 290 | fprintf(fp,"\nRequested: %ld ",plates); |
rschimpf78 | 7:ede305192e3b | 291 | fprintf(fp,"\nBroken Switch: %ld ", broken_switch); |
rschimpf78 | 7:ede305192e3b | 292 | fprintf(fp,"\nStop Error: %ld ", stop_error); |
rschimpf78 | 7:ede305192e3b | 293 | fclose(fp); |
rschimpf78 | 7:ede305192e3b | 294 | exit(1); |
rschimpf78 | 7:ede305192e3b | 295 | break; |
rschimpf78 | 7:ede305192e3b | 296 | case 2: |
rschimpf78 | 7:ede305192e3b | 297 | fprintf(fp,"\nRequested: %ld ",plates); |
rschimpf78 | 7:ede305192e3b | 298 | fprintf(fp,"\nSelector Sensor Distance : %ld ", error_distance); |
rschimpf78 | 7:ede305192e3b | 299 | fprintf(fp,"\nStop Error: %ld ", stop_error); |
rschimpf78 | 7:ede305192e3b | 300 | fclose(fp); |
rschimpf78 | 7:ede305192e3b | 301 | exit(1); |
rschimpf78 | 7:ede305192e3b | 302 | break; |
rschimpf78 | 7:ede305192e3b | 303 | case 3: |
rschimpf78 | 7:ede305192e3b | 304 | fprintf(fp,"\nRequested: %ld ",plates); |
rschimpf78 | 7:ede305192e3b | 305 | fprintf(fp,"\nSelector Timeout"); |
rschimpf78 | 7:ede305192e3b | 306 | fprintf(fp,"\nStop Error: %ld ", stop_error); |
rschimpf78 | 7:ede305192e3b | 307 | fclose(fp); |
rschimpf78 | 7:ede305192e3b | 308 | exit(1); |
rschimpf78 | 7:ede305192e3b | 309 | break; |
rschimpf78 | 7:ede305192e3b | 310 | case 4: |
rschimpf78 | 8:3f4d78017fd9 | 311 | fprintf(fp,"\nRequested: %ld ", plates); |
rschimpf78 | 7:ede305192e3b | 312 | fprintf(fp,"\nSelected: %ld ", sheets); |
rschimpf78 | 7:ede305192e3b | 313 | fprintf(fp,"\nDetected: %ld ", path_sheets); |
rschimpf78 | 7:ede305192e3b | 314 | fprintf(fp,"\nStop Error: %ld ", stop_error); |
rschimpf78 | 7:ede305192e3b | 315 | fclose(fp); |
rschimpf78 | 7:ede305192e3b | 316 | break; |
rschimpf78 | 7:ede305192e3b | 317 | case 5: |
rschimpf78 | 8:3f4d78017fd9 | 318 | fprintf(fp,"\nRequested: %ld ", plates); |
rschimpf78 | 7:ede305192e3b | 319 | fprintf(fp,"\nSelected: %ld ", sheets); |
rschimpf78 | 7:ede305192e3b | 320 | fprintf(fp,"\nDetected: %ld ", path_sheets); |
rschimpf78 | 7:ede305192e3b | 321 | fprintf(fp,"\nStop Error: %ld ", stop_error); |
rschimpf78 | 7:ede305192e3b | 322 | fclose(fp); |
rschimpf78 | 7:ede305192e3b | 323 | exit(1); |
rschimpf78 | 7:ede305192e3b | 324 | break; |
rschimpf78 | 7:ede305192e3b | 325 | case 6: |
rschimpf78 | 8:3f4d78017fd9 | 326 | fprintf(fp,"\nRequested: %ld ", plates); |
rschimpf78 | 7:ede305192e3b | 327 | fprintf(fp,"\nSelected: %ld ", sheets); |
rschimpf78 | 7:ede305192e3b | 328 | fprintf(fp,"\nBeam Sensor Distance: %ld ", beam_distance); |
rschimpf78 | 7:ede305192e3b | 329 | fprintf(fp,"\nStop Error: %ld ", stop_error); |
rschimpf78 | 7:ede305192e3b | 330 | fclose(fp); |
rschimpf78 | 7:ede305192e3b | 331 | exit(1); |
rschimpf78 | 7:ede305192e3b | 332 | break; |
rschimpf78 | 7:ede305192e3b | 333 | case 7: |
rschimpf78 | 8:3f4d78017fd9 | 334 | fprintf(fp,"\nRequested: %ld ", plates); |
rschimpf78 | 7:ede305192e3b | 335 | fprintf(fp,"\nNot at Start Position "); |
rschimpf78 | 7:ede305192e3b | 336 | fprintf(fp,"\nStop Error: %ld ", stop_error); |
rschimpf78 | 7:ede305192e3b | 337 | fclose(fp); |
rschimpf78 | 7:ede305192e3b | 338 | exit(1); |
rschimpf78 | 7:ede305192e3b | 339 | default: |
rschimpf78 | 7:ede305192e3b | 340 | pc.printf("\nstage: %ld ", stage); |
rschimpf78 | 7:ede305192e3b | 341 | if (stage == 1){ |
rschimpf78 | 7:ede305192e3b | 342 | pc.printf("\nRequested: %ld ", plates); |
rschimpf78 | 7:ede305192e3b | 343 | fprintf(fp,"\nRequested: %ld ",plates); |
rschimpf78 | 7:ede305192e3b | 344 | fclose(fp); |
rschimpf78 | 7:ede305192e3b | 345 | } |
rschimpf78 | 7:ede305192e3b | 346 | else if (stage == 2) { |
rschimpf78 | 7:ede305192e3b | 347 | pc.printf("\nSelected: %ld ", sheets); |
rschimpf78 | 7:ede305192e3b | 348 | fprintf(fp,"\nSelected: %ld ", sheets); |
rschimpf78 | 7:ede305192e3b | 349 | fclose(fp); |
rschimpf78 | 7:ede305192e3b | 350 | } |
rschimpf78 | 7:ede305192e3b | 351 | else if (stage == 3){ |
rschimpf78 | 7:ede305192e3b | 352 | pc.printf("\nDetected: %ld ", path_sheets); |
rschimpf78 | 7:ede305192e3b | 353 | fprintf(fp,"\nDetected: %ld ", path_sheets); |
rschimpf78 | 7:ede305192e3b | 354 | fclose(fp); |
rschimpf78 | 7:ede305192e3b | 355 | } |
rschimpf78 | 7:ede305192e3b | 356 | else{ |
rschimpf78 | 7:ede305192e3b | 357 | pc.printf("\nPass "); |
rschimpf78 | 7:ede305192e3b | 358 | fprintf(fp,"\nPass "); |
rschimpf78 | 7:ede305192e3b | 359 | fclose(fp); |
rschimpf78 | 7:ede305192e3b | 360 | } |
rschimpf78 | 7:ede305192e3b | 361 | break; |
rschimpf78 | 7:ede305192e3b | 362 | } |
rschimpf78 | 2:e8f8095464cf | 363 | } |
rschimpf78 | 0:88834eed3de0 | 364 | |
rschimpf78 | 0:88834eed3de0 | 365 | int main() |
rschimpf78 | 2:e8f8095464cf | 366 | { |
rschimpf78 | 8:3f4d78017fd9 | 367 | homeSwitch.mode(PullUp); |
rschimpf78 | 8:3f4d78017fd9 | 368 | wait(.01); |
rschimpf78 | 8:3f4d78017fd9 | 369 | magnetSwitch.mode(PullUp); |
rschimpf78 | 8:3f4d78017fd9 | 370 | wait (.01); |
rschimpf78 | 7:ede305192e3b | 371 | mkdir("/sd/mydir", 0777); |
rschimpf78 | 2:e8f8095464cf | 372 | in1=1; |
rschimpf78 | 2:e8f8095464cf | 373 | in2=1; |
rschimpf78 | 2:e8f8095464cf | 374 | en = 1; |
rschimpf78 | 2:e8f8095464cf | 375 | wait(0.1); |
rschimpf78 | 8:3f4d78017fd9 | 376 | |
rschimpf78 | 7:ede305192e3b | 377 | while(stop_error==0) |
rschimpf78 | 0:88834eed3de0 | 378 | { |
rschimpf78 | 4:63f0989c8d1e | 379 | plates = (rand() % 16) + 1; |
rschimpf78 | 7:ede305192e3b | 380 | wait(1); |
rschimpf78 | 7:ede305192e3b | 381 | switch_check(); //stage 1 |
rschimpf78 | 7:ede305192e3b | 382 | wait(1); |
rschimpf78 | 7:ede305192e3b | 383 | logwriting(); // plates requestd |
rschimpf78 | 7:ede305192e3b | 384 | wait(1); |
rschimpf78 | 7:ede305192e3b | 385 | plate_selection(); //stage 2 |
rschimpf78 | 7:ede305192e3b | 386 | wait(1); |
rschimpf78 | 7:ede305192e3b | 387 | logwriting(); //paltes selected |
rschimpf78 | 7:ede305192e3b | 388 | wait(1); |
rschimpf78 | 7:ede305192e3b | 389 | send(); |
rschimpf78 | 7:ede305192e3b | 390 | wait(1); |
rschimpf78 | 7:ede305192e3b | 391 | beam(); //stage 3 |
rschimpf78 | 7:ede305192e3b | 392 | wait(1); |
rschimpf78 | 7:ede305192e3b | 393 | logwriting(); //plates detected |
rschimpf78 | 7:ede305192e3b | 394 | wait(1); |
rschimpf78 | 7:ede305192e3b | 395 | retract(); //stage 4 |
rschimpf78 | 7:ede305192e3b | 396 | wait(1); |
rschimpf78 | 7:ede305192e3b | 397 | logwriting(); //nothing |
rschimpf78 | 7:ede305192e3b | 398 | } |
rschimpf78 | 7:ede305192e3b | 399 | pc.printf("\nError was detected. Stopping Program"); |
rschimpf78 | 7:ede305192e3b | 400 | } |
rschimpf78 | 7:ede305192e3b | 401 |