Vam GE / Mbed 2 deprecated VAM_MANUAL_2_DO_NOT_EDIT

VAM

Dependencies:   Stepper mbed HCSR04 millis

main.cpp@10:47aede089c8d, 2019-01-21 (annotated)

Committer:
vamgehealthcare11
Date:
Mon Jan 21 23:37:54 2019 +0000
Revision:
10:47aede089c8d
Parent:
9:5263aacbda53 
update

Who changed what in which revision?

UserRevisionLine numberNew 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 5:e958d61e0564 5 #include "millis.h"
rschimpf78 6:6ca8a0ef7d4a 6
rschimpf78 3:fd2ca631ab44 7 Serial pc(USBTX, USBRX);
vamgehealthcare11 9:5263aacbda53 8 HCSR04 heightsensor(PTD2,PTD3);
rschimpf78 6:6ca8a0ef7d4a 9 HCSR04 pathsensor(PTC2,PTA2); //(trig,echo)
rschimpf78 3:fd2ca631ab44 10 DigitalOut in1(PTC12);
rschimpf78 3:fd2ca631ab44 11 DigitalOut in2(D7);
rschimpf78 6:6ca8a0ef7d4a 12 DigitalIn magnetSwitch(D10);
vamgehealthcare11 9:5263aacbda53 13 DigitalIn homeSwitch(D9);
vamgehealthcare11 9:5263aacbda53 14 Stepper mot(D3,D4);
rschimpf78 2:e8f8095464cf 15 DigitalOut en(D2);
rschimpf78 0:88834eed3de0 16
vamgehealthcare11 9:5263aacbda53 17 int main(){
vamgehealthcare11 10:47aede089c8d 18
vamgehealthcare11 9:5263aacbda53 19 LOOPY:
vamgehealthcare11 9:5263aacbda53 20 en = 1;
vamgehealthcare11 9:5263aacbda53 21 in1 = 1;
vamgehealthcare11 9:5263aacbda53 22 in2 = 1;
vamgehealthcare11 9:5263aacbda53 23 magnetSwitch.mode(PullUp);
vamgehealthcare11 9:5263aacbda53 24 wait (.01);
vamgehealthcare11 9:5263aacbda53 25 homeSwitch.mode(PullUp);
vamgehealthcare11 9:5263aacbda53 26 wait (0.01);
vamgehealthcare11 9:5263aacbda53 27 int command;
vamgehealthcare11 9:5263aacbda53 28
vamgehealthcare11 9:5263aacbda53 29 while(1){
vamgehealthcare11 9:5263aacbda53 30
vamgehealthcare11 9:5263aacbda53 31 pc.printf("\nCommand:\n\n\t1\tSend Plates\n\t2\tDetect Active Plates\n\t3\tRetract All Active Plates\n\t4\tSelect Plates\n\n");
vamgehealthcare11 9:5263aacbda53 32 pc.printf("\nRequest: ");
vamgehealthcare11 9:5263aacbda53 33 pc.scanf("%d",&command);
rschimpf78 0:88834eed3de0 34
vamgehealthcare11 9:5263aacbda53 35 if (command==1){
rschimpf78 3:fd2ca631ab44 36
vamgehealthcare11 9:5263aacbda53 37 int plates = 0;
vamgehealthcare11 9:5263aacbda53 38 unsigned int dist;
vamgehealthcare11 9:5263aacbda53 39 int platedist;
rschimpf78 3:fd2ca631ab44 40
vamgehealthcare11 9:5263aacbda53 41 while(plates<=0 || plates>16){
vamgehealthcare11 9:5263aacbda53 42 pc.printf("\nEnter Number of Plates to Send 0-16: ");
vamgehealthcare11 9:5263aacbda53 43 pc.scanf("%d",&plates);
vamgehealthcare11 9:5263aacbda53 44
vamgehealthcare11 9:5263aacbda53 45 if (plates > 16 || plates < 0 ){
vamgehealthcare11 9:5263aacbda53 46 pc.printf("\nInvalid Entry");
vamgehealthcare11 9:5263aacbda53 47 }
vamgehealthcare11 9:5263aacbda53 48 else{
vamgehealthcare11 9:5263aacbda53 49 pc.printf("\nPlates Requested: %ld", plates);
vamgehealthcare11 9:5263aacbda53 50 }
rschimpf78 3:fd2ca631ab44 51 }
rschimpf78 3:fd2ca631ab44 52
vamgehealthcare11 9:5263aacbda53 53 if (magnetSwitch == 1){
vamgehealthcare11 9:5263aacbda53 54 pc.printf("\nNot in Home Position");
vamgehealthcare11 9:5263aacbda53 55 goto LOOPY;
vamgehealthcare11 9:5263aacbda53 56 }
vamgehealthcare11 9:5263aacbda53 57 else{
vamgehealthcare11 9:5263aacbda53 58 wait(1);
vamgehealthcare11 9:5263aacbda53 59 in1=1;
vamgehealthcare11 9:5263aacbda53 60 in2=0;
vamgehealthcare11 9:5263aacbda53 61 wait_ms(900);
vamgehealthcare11 9:5263aacbda53 62 in1=1;
vamgehealthcare11 9:5263aacbda53 63 in2=1;
vamgehealthcare11 9:5263aacbda53 64 }
vamgehealthcare11 10:47aede089c8d 65 pc.printf("\nRequested Plates:%ld",plates);
vamgehealthcare11 10:47aede089c8d 66 platedist = (468-24*plates);
vamgehealthcare11 9:5263aacbda53 67 millisStart();
vamgehealthcare11 9:5263aacbda53 68 long durationSM = 0;
vamgehealthcare11 9:5263aacbda53 69 long starttimeSM = millis();
vamgehealthcare11 10:47aede089c8d 70
vamgehealthcare11 10:47aede089c8d 71 SENSORREAD:
vamgehealthcare11 10:47aede089c8d 72 heightsensor.start();
vamgehealthcare11 10:47aede089c8d 73 wait_ms(250);
vamgehealthcare11 10:47aede089c8d 74 dist = heightsensor.get_dist_cm();
vamgehealthcare11 10:47aede089c8d 75
vamgehealthcare11 9:5263aacbda53 76
vamgehealthcare11 10:47aede089c8d 77 while ((dist < platedist-2 && durationSM < 60000 || dist > platedist+2 && durationSM < 60000)){
vamgehealthcare11 10:47aede089c8d 78
vamgehealthcare11 9:5263aacbda53 79
vamgehealthcare11 9:5263aacbda53 80 heightsensor.start();
vamgehealthcare11 9:5263aacbda53 81 long currenttimeSM = millis();
vamgehealthcare11 9:5263aacbda53 82 durationSM = currenttimeSM-starttimeSM;
vamgehealthcare11 9:5263aacbda53 83 en = 0;
vamgehealthcare11 9:5263aacbda53 84 mot.setSpeed(500);
vamgehealthcare11 10:47aede089c8d 85 dist = heightsensor.get_dist_cm();
vamgehealthcare11 10:47aede089c8d 86
vamgehealthcare11 10:47aede089c8d 87 if(dist >= 480 || dist <= 40 ){
vamgehealthcare11 10:47aede089c8d 88 pc.printf("\nBad Read at :%ld", durationSM);
vamgehealthcare11 10:47aede089c8d 89 goto SENSORREAD;
vamgehealthcare11 10:47aede089c8d 90 }
vamgehealthcare11 10:47aede089c8d 91
vamgehealthcare11 10:47aede089c8d 92 if(dist<platedist-2){ //397 403
vamgehealthcare11 9:5263aacbda53 93 mot.rotate(1);
vamgehealthcare11 9:5263aacbda53 94 wait(0.5);
vamgehealthcare11 9:5263aacbda53 95 }
vamgehealthcare11 9:5263aacbda53 96 if(dist>platedist+2){
vamgehealthcare11 9:5263aacbda53 97 mot.rotate(0);
vamgehealthcare11 9:5263aacbda53 98 wait(0.5);
vamgehealthcare11 9:5263aacbda53 99 }
vamgehealthcare11 9:5263aacbda53 100 mot.stop();
rschimpf78 3:fd2ca631ab44 101 }
vamgehealthcare11 9:5263aacbda53 102
vamgehealthcare11 10:47aede089c8d 103 pc.printf("\nSelector Height:%ld", dist);
vamgehealthcare11 10:47aede089c8d 104 pc.printf("\nSelector Motor Duration:%ld", durationSM);
vamgehealthcare11 9:5263aacbda53 105 mot.stop();
vamgehealthcare11 9:5263aacbda53 106 en = 1;
vamgehealthcare11 9:5263aacbda53 107 wait(1);
rschimpf78 3:fd2ca631ab44 108
vamgehealthcare11 10:47aede089c8d 109 if (dist >= 480 || dist <= 40){
vamgehealthcare11 10:47aede089c8d 110 pc.printf("\nSelector Sensor Error");
vamgehealthcare11 9:5263aacbda53 111 goto LOOPY;
rschimpf78 3:fd2ca631ab44 112 }
vamgehealthcare11 10:47aede089c8d 113 else if (durationSM >= 60000){
vamgehealthcare11 9:5263aacbda53 114 pc.printf("\nSelector Motor Time Out");
vamgehealthcare11 10:47aede089c8d 115 exit(1);
rschimpf78 3:fd2ca631ab44 116 }
vamgehealthcare11 9:5263aacbda53 117 else{
vamgehealthcare11 9:5263aacbda53 118 in1=1;
vamgehealthcare11 9:5263aacbda53 119 in2=0;
vamgehealthcare11 10:47aede089c8d 120 wait(56);
vamgehealthcare11 9:5263aacbda53 121 in1=1;
vamgehealthcare11 9:5263aacbda53 122 in2=1;
vamgehealthcare11 10:47aede089c8d 123 pc.printf("\nSending Finished");
vamgehealthcare11 10:47aede089c8d 124 }
vamgehealthcare11 10:47aede089c8d 125 }
vamgehealthcare11 9:5263aacbda53 126
vamgehealthcare11 9:5263aacbda53 127 else if (command==2){
vamgehealthcare11 9:5263aacbda53 128
vamgehealthcare11 9:5263aacbda53 129 unsigned int pathdist;
vamgehealthcare11 9:5263aacbda53 130 int path_sheets;
vamgehealthcare11 9:5263aacbda53 131 int detectionTOL = 12;
vamgehealthcare11 9:5263aacbda53 132 int sens_pos = 131;
vamgehealthcare11 9:5263aacbda53 133 int sheet_th = 25;
vamgehealthcare11 9:5263aacbda53 134 pathsensor.start();
vamgehealthcare11 9:5263aacbda53 135 wait_ms(500);
vamgehealthcare11 9:5263aacbda53 136 pathdist=pathsensor.get_dist_cm();
rschimpf78 3:fd2ca631ab44 137
vamgehealthcare11 9:5263aacbda53 138 if (pathdist >= sens_pos-detectionTOL && pathdist <= sens_pos+detectionTOL){ //110,130
vamgehealthcare11 9:5263aacbda53 139 path_sheets = 16;
vamgehealthcare11 9:5263aacbda53 140 }
vamgehealthcare11 9:5263aacbda53 141 else if (pathdist >= sens_pos-detectionTOL+sheet_th && pathdist <= sens_pos+detectionTOL+sheet_th){ //152,168 a:
vamgehealthcare11 9:5263aacbda53 142 path_sheets = 15;
vamgehealthcare11 9:5263aacbda53 143 }
vamgehealthcare11 9:5263aacbda53 144 else if (pathdist >= sens_pos-detectionTOL+2*sheet_th && pathdist <= sens_pos+detectionTOL+2*sheet_th){ //179-191
vamgehealthcare11 9:5263aacbda53 145 path_sheets = 14;
vamgehealthcare11 9:5263aacbda53 146 }
vamgehealthcare11 9:5263aacbda53 147 else if (pathdist >= sens_pos-detectionTOL+3*sheet_th && pathdist <= sens_pos+detectionTOL+3*sheet_th){ //205-217
vamgehealthcare11 9:5263aacbda53 148 path_sheets = 13;
vamgehealthcare11 9:5263aacbda53 149 }
vamgehealthcare11 9:5263aacbda53 150 else if (pathdist >= sens_pos-detectionTOL+4*sheet_th && pathdist <= sens_pos+detectionTOL+4*sheet_th){ //231-243
vamgehealthcare11 9:5263aacbda53 151 path_sheets = 12;
vamgehealthcare11 9:5263aacbda53 152 }
vamgehealthcare11 9:5263aacbda53 153 else if (pathdist >= sens_pos-detectionTOL+5*sheet_th && pathdist <= sens_pos+detectionTOL+5*sheet_th){ //257-269
vamgehealthcare11 9:5263aacbda53 154 path_sheets = 11;
vamgehealthcare11 9:5263aacbda53 155 }
vamgehealthcare11 9:5263aacbda53 156 else if (pathdist >= sens_pos-detectionTOL+6*sheet_th && pathdist <= sens_pos+detectionTOL+6*sheet_th){ //283-295
rschimpf78 3:fd2ca631ab44 157 path_sheets = 10;
rschimpf78 3:fd2ca631ab44 158
rschimpf78 3:fd2ca631ab44 159 }
vamgehealthcare11 9:5263aacbda53 160 else if (pathdist >= sens_pos-detectionTOL+7*sheet_th && pathdist <= sens_pos+detectionTOL+7*sheet_th){ //309-321
rschimpf78 3:fd2ca631ab44 161 path_sheets = 9;
rschimpf78 3:fd2ca631ab44 162
rschimpf78 3:fd2ca631ab44 163 }
vamgehealthcare11 9:5263aacbda53 164 else if (pathdist >= sens_pos-detectionTOL+8*sheet_th && pathdist <= sens_pos+detectionTOL+8*sheet_th){ //323-333
rschimpf78 3:fd2ca631ab44 165 path_sheets = 8;
rschimpf78 3:fd2ca631ab44 166
rschimpf78 3:fd2ca631ab44 167 }
vamgehealthcare11 9:5263aacbda53 168 else if (pathdist >= sens_pos-detectionTOL+9*sheet_th && pathdist <= sens_pos+detectionTOL+9*sheet_th){ //349-359
rschimpf78 3:fd2ca631ab44 169 path_sheets = 7;
rschimpf78 3:fd2ca631ab44 170
rschimpf78 3:fd2ca631ab44 171 }
vamgehealthcare11 9:5263aacbda53 172 else if (pathdist >= sens_pos-detectionTOL+10*sheet_th && pathdist <= sens_pos+detectionTOL+10*sheet_th){
rschimpf78 3:fd2ca631ab44 173 path_sheets = 6;
rschimpf78 3:fd2ca631ab44 174
rschimpf78 3:fd2ca631ab44 175 }
vamgehealthcare11 9:5263aacbda53 176 else if (pathdist >= sens_pos-detectionTOL+11*sheet_th && pathdist <= sens_pos+detectionTOL+11*sheet_th){
rschimpf78 3:fd2ca631ab44 177 path_sheets = 5;
rschimpf78 3:fd2ca631ab44 178
rschimpf78 3:fd2ca631ab44 179 }
vamgehealthcare11 9:5263aacbda53 180 else if (pathdist >= sens_pos-detectionTOL+12*sheet_th && pathdist <= sens_pos+detectionTOL+12*sheet_th){
rschimpf78 3:fd2ca631ab44 181 path_sheets = 4;
rschimpf78 3:fd2ca631ab44 182
rschimpf78 3:fd2ca631ab44 183 }
vamgehealthcare11 9:5263aacbda53 184 else if (pathdist >= sens_pos-detectionTOL+13*sheet_th && pathdist <= sens_pos+detectionTOL+13*sheet_th){ //468,478
rschimpf78 3:fd2ca631ab44 185 path_sheets = 3;
rschimpf78 3:fd2ca631ab44 186
rschimpf78 3:fd2ca631ab44 187 }
vamgehealthcare11 9:5263aacbda53 188 else if (pathdist >= sens_pos-detectionTOL+14*sheet_th && pathdist <= sens_pos+detectionTOL+14*sheet_th){ //494,504
rschimpf78 3:fd2ca631ab44 189 path_sheets = 2;
rschimpf78 3:fd2ca631ab44 190
rschimpf78 3:fd2ca631ab44 191 }
vamgehealthcare11 9:5263aacbda53 192 else if (pathdist >= sens_pos-10-detectionTOL+15*sheet_th && pathdist <= sens_pos-10+detectionTOL+15*sheet_th){//greater than 509 less 517
rschimpf78 3:fd2ca631ab44 193 path_sheets = 1;
rschimpf78 3:fd2ca631ab44 194
rschimpf78 3:fd2ca631ab44 195 }
vamgehealthcare11 9:5263aacbda53 196 else if (pathdist > sens_pos-detectionTOL+16*sheet_th){ //greater than 534
rschimpf78 3:fd2ca631ab44 197 path_sheets = 0;
vamgehealthcare11 9:5263aacbda53 198 }
vamgehealthcare11 9:5263aacbda53 199 else {
vamgehealthcare11 9:5263aacbda53 200 pc.printf("\nBeam Path Detector distance out of range : %ld", pathdist);
vamgehealthcare11 9:5263aacbda53 201 wait(2);
vamgehealthcare11 9:5263aacbda53 202 goto LOOPY;
vamgehealthcare11 9:5263aacbda53 203 }
vamgehealthcare11 9:5263aacbda53 204 pc.printf("\nPlates Detected in Beam Path: %ld", path_sheets);
vamgehealthcare11 9:5263aacbda53 205 wait(2);
rschimpf78 3:fd2ca631ab44 206 }
vamgehealthcare11 9:5263aacbda53 207
vamgehealthcare11 9:5263aacbda53 208 else if (command==3){
vamgehealthcare11 9:5263aacbda53 209
vamgehealthcare11 9:5263aacbda53 210 millisStart();
vamgehealthcare11 9:5263aacbda53 211 long Duration = 0;
vamgehealthcare11 9:5263aacbda53 212 long StartTime = millis();
rschimpf78 5:e958d61e0564 213
vamgehealthcare11 9:5263aacbda53 214 while(magnetSwitch == 1 && homeSwitch == 1 && Duration <= 58000 ){
vamgehealthcare11 9:5263aacbda53 215 in1=0;
vamgehealthcare11 9:5263aacbda53 216 in2=1;
vamgehealthcare11 9:5263aacbda53 217 long CurrentTime = millis();
vamgehealthcare11 9:5263aacbda53 218 Duration = CurrentTime-StartTime;
vamgehealthcare11 9:5263aacbda53 219 }
vamgehealthcare11 9:5263aacbda53 220
vamgehealthcare11 9:5263aacbda53 221 in1=1;
vamgehealthcare11 9:5263aacbda53 222 in2=1;
vamgehealthcare11 9:5263aacbda53 223
vamgehealthcare11 9:5263aacbda53 224 if (Duration > 58000){
vamgehealthcare11 9:5263aacbda53 225 pc.printf("\nRetraction Time Out Error");
vamgehealthcare11 9:5263aacbda53 226 goto LOOPY;
vamgehealthcare11 9:5263aacbda53 227 }
vamgehealthcare11 9:5263aacbda53 228 else if (homeSwitch == 0){
vamgehealthcare11 9:5263aacbda53 229 pc.printf("\nMagnet Switch Broken");
vamgehealthcare11 9:5263aacbda53 230 goto LOOPY;
vamgehealthcare11 9:5263aacbda53 231 }
vamgehealthcare11 9:5263aacbda53 232 else{
vamgehealthcare11 9:5263aacbda53 233 pc.printf("\nPlate Retraction Complete");
vamgehealthcare11 10:47aede089c8d 234 wait(1);
vamgehealthcare11 9:5263aacbda53 235 }
vamgehealthcare11 9:5263aacbda53 236 }
vamgehealthcare11 9:5263aacbda53 237
vamgehealthcare11 9:5263aacbda53 238 else if (command==4){
vamgehealthcare11 9:5263aacbda53 239
vamgehealthcare11 9:5263aacbda53 240 int plates = 0;
vamgehealthcare11 9:5263aacbda53 241 unsigned int dist;
vamgehealthcare11 9:5263aacbda53 242 int platedist;
vamgehealthcare11 9:5263aacbda53 243
vamgehealthcare11 9:5263aacbda53 244 while(plates<=0 || plates>16){
vamgehealthcare11 9:5263aacbda53 245
vamgehealthcare11 9:5263aacbda53 246 pc.printf("\nEnter Number of Plates to Select 0-16: ");
vamgehealthcare11 9:5263aacbda53 247 pc.scanf("%d",&plates);
vamgehealthcare11 9:5263aacbda53 248
vamgehealthcare11 9:5263aacbda53 249 if (plates > 16 || plates < 0 ){
vamgehealthcare11 9:5263aacbda53 250 pc.printf("\nInvalid Entry");
vamgehealthcare11 9:5263aacbda53 251 }
vamgehealthcare11 9:5263aacbda53 252 else{
vamgehealthcare11 9:5263aacbda53 253 pc.printf("\nPlates Requested: %ld", plates);
vamgehealthcare11 9:5263aacbda53 254 }
vamgehealthcare11 9:5263aacbda53 255 }
vamgehealthcare11 9:5263aacbda53 256
vamgehealthcare11 9:5263aacbda53 257 if (magnetSwitch == 1){
vamgehealthcare11 9:5263aacbda53 258 pc.printf("\nNot in Home Position");
vamgehealthcare11 9:5263aacbda53 259 goto LOOPY;
vamgehealthcare11 9:5263aacbda53 260 }
vamgehealthcare11 9:5263aacbda53 261 else{
vamgehealthcare11 9:5263aacbda53 262 wait(1);
vamgehealthcare11 9:5263aacbda53 263 in1=1;
vamgehealthcare11 9:5263aacbda53 264 in2=0;
vamgehealthcare11 9:5263aacbda53 265 wait_ms(900);
vamgehealthcare11 9:5263aacbda53 266 in1=1;
vamgehealthcare11 9:5263aacbda53 267 in2=1;
vamgehealthcare11 9:5263aacbda53 268 }
vamgehealthcare11 9:5263aacbda53 269
vamgehealthcare11 9:5263aacbda53 270 platedist = (468-24*plates);
vamgehealthcare11 9:5263aacbda53 271 wait(1);
vamgehealthcare11 9:5263aacbda53 272 heightsensor.start();
vamgehealthcare11 9:5263aacbda53 273 wait_ms(500);
vamgehealthcare11 9:5263aacbda53 274 dist = heightsensor.get_dist_cm();
vamgehealthcare11 9:5263aacbda53 275 millisStart();
vamgehealthcare11 9:5263aacbda53 276 long durationSM = 0;
vamgehealthcare11 9:5263aacbda53 277 long starttimeSM = millis();
vamgehealthcare11 9:5263aacbda53 278 pc.printf("\nPlate Distance:%ld",platedist);
vamgehealthcare11 9:5263aacbda53 279 pc.printf("\nSelector Height:%ld",dist);
vamgehealthcare11 9:5263aacbda53 280
vamgehealthcare11 9:5263aacbda53 281 while(dist < platedist-2 || dist > platedist+2 && dist < 480 && durationSM <56000){
vamgehealthcare11 9:5263aacbda53 282
vamgehealthcare11 9:5263aacbda53 283 heightsensor.start();
vamgehealthcare11 9:5263aacbda53 284 dist = heightsensor.get_dist_cm();
vamgehealthcare11 9:5263aacbda53 285 long currenttimeSM = millis();
vamgehealthcare11 9:5263aacbda53 286 durationSM = currenttimeSM-starttimeSM;
vamgehealthcare11 9:5263aacbda53 287 pc.printf("\nSelector Height:%ld",dist);
vamgehealthcare11 9:5263aacbda53 288 en = 0;
vamgehealthcare11 9:5263aacbda53 289 mot.setSpeed(500);
vamgehealthcare11 9:5263aacbda53 290
vamgehealthcare11 9:5263aacbda53 291 if(dist<platedist-2){
vamgehealthcare11 9:5263aacbda53 292 mot.rotate(1);
vamgehealthcare11 9:5263aacbda53 293 wait(0.5);
vamgehealthcare11 9:5263aacbda53 294 }
vamgehealthcare11 9:5263aacbda53 295 if(dist>platedist+2){
vamgehealthcare11 9:5263aacbda53 296 mot.rotate(0);
vamgehealthcare11 9:5263aacbda53 297 wait(0.5);
vamgehealthcare11 9:5263aacbda53 298 }
vamgehealthcare11 9:5263aacbda53 299 mot.stop();
vamgehealthcare11 9:5263aacbda53 300 }
vamgehealthcare11 9:5263aacbda53 301
vamgehealthcare11 9:5263aacbda53 302 mot.stop();
vamgehealthcare11 9:5263aacbda53 303 en = 1;
vamgehealthcare11 9:5263aacbda53 304 wait(1);
vamgehealthcare11 9:5263aacbda53 305
vamgehealthcare11 9:5263aacbda53 306 if (dist >=480){
vamgehealthcare11 9:5263aacbda53 307 pc.printf("\nSelector Distance out of Range:%ld",dist);
vamgehealthcare11 9:5263aacbda53 308 goto LOOPY;
vamgehealthcare11 9:5263aacbda53 309 }
vamgehealthcare11 9:5263aacbda53 310 else if (durationSM >= 56000){
vamgehealthcare11 9:5263aacbda53 311 pc.printf("\nSelector Motor Time Out");
vamgehealthcare11 9:5263aacbda53 312 goto LOOPY;
vamgehealthcare11 9:5263aacbda53 313 }
vamgehealthcare11 9:5263aacbda53 314 else{
vamgehealthcare11 9:5263aacbda53 315 pc.printf("\nSelection Finished");
vamgehealthcare11 9:5263aacbda53 316 }
rschimpf78 3:fd2ca631ab44 317 }
vamgehealthcare11 9:5263aacbda53 318 else {
vamgehealthcare11 9:5263aacbda53 319 pc.printf("\nInvalid Command");
rschimpf78 3:fd2ca631ab44 320 }
rschimpf78 3:fd2ca631ab44 321 }
rschimpf78 2:e8f8095464cf 322 }