Diff: main.cpp

Revision:

10:4c3a8f835bef

Parent:

9:5263aacbda53

--- a/main.cpp	Mon Jan 21 19:54:15 2019 +0000
+++ b/main.cpp	Mon Jan 21 23:38:28 2019 +0000
@@ -15,7 +15,8 @@
 DigitalOut en(D2); 
 
 int main(){
-    LOOPY:
+    
+   
      en = 1;
     in1 = 1;
     in2 = 1;
@@ -23,13 +24,12 @@
     wait (.01);
     homeSwitch.mode(PullUp);
     wait (0.01);
-    int command;
     
 while(1){
-      
-        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");
-        pc.printf("\nRequest: ");
-        pc.scanf("%d",&command);
+     
+     LOOPY:
+     
+for ( int command = 1; command < 4; command++){          
 
     if (command==1){
         
@@ -37,21 +37,11 @@
         unsigned int dist;
         int platedist;
         
-        while(plates<=0 || plates>16){
-              pc.printf("\nEnter Number of Plates to Send 0-16: "); 
-              pc.scanf("%d",&plates);
-            
-              if (plates > 16 || plates < 0 ){
-                  pc.printf("\nInvalid Entry"); 
-                  } 
-              else{
-                  pc.printf("\nPlates Requested: %ld", plates);
-                  }
-        }
+        plates = (rand() % 16) + 1;
         
         if (magnetSwitch == 1){
             pc.printf("\nNot in Home Position"); 
-            goto LOOPY;
+            exit(1);
             }
         else{
             wait(1);
@@ -61,29 +51,34 @@
             in1=1;
             in2=1;
             }
-      
-        platedist = (468-24*plates);
-        wait(1);   
-        heightsensor.start();
-        wait_ms(500); 
-        dist = heightsensor.get_dist_cm(); 
+        pc.printf("\nRequested Plates:%ld",plates);
+        platedist = (468-24*plates);  
         millisStart();
         long durationSM = 0;
         long starttimeSM = millis();  
-        pc.printf("\nPlate Distance:%ld",platedist); 
-        pc.printf("\nSelector Height:%ld",dist);
         
-        while(dist < platedist-2 || dist > platedist+2 && dist < 480 && durationSM <56000){
+        SENSORREAD:
+        heightsensor.start();
+        wait_ms(250); 
+        dist = heightsensor.get_dist_cm(); 
+         
+        
+        while ((dist < platedist-2 && durationSM < 60000 || dist > platedist+2 && durationSM < 60000)){
+             
              
               heightsensor.start();
-              dist = heightsensor.get_dist_cm(); 
               long currenttimeSM = millis();
               durationSM = currenttimeSM-starttimeSM;
-              pc.printf("\nSelector Height:%ld",dist); 
               en = 0;
               mot.setSpeed(500);
-           
-              if(dist<platedist-2){ 
+              dist = heightsensor.get_dist_cm(); 
+              
+              if(dist >= 480 || dist <= 40 ){
+                 pc.printf("\nBad Read at :%ld", durationSM); 
+                 goto SENSORREAD;
+              }
+              
+              if(dist<platedist-2){ //397 403
                  mot.rotate(1);
                  wait(0.5);
                 }
@@ -94,25 +89,27 @@
               mot.stop();
         }
         
+        pc.printf("\nSelector Height:%ld", dist);
+        pc.printf("\nSelector Motor Duration:%ld", durationSM);
         mot.stop(); 
         en = 1;
         wait(1);
         
-        if (dist >=480){
-            pc.printf("\nSelector Distance out of Range:%ld",dist);
-            goto LOOPY;
+        if (dist >= 480 || dist <= 40){
+            pc.printf("\nSelector Distance out of Range");
+            exit(1);
             }
-        else if (durationSM >= 56000){
+        else if (durationSM >= 60000){
             pc.printf("\nSelector Motor Time Out");
-            goto LOOPY;
+            exit(1);
             }
         else{
-            pc.printf("\nPlates Selected: %ld", plates);
             in1=1;
             in2=0;
-            wait(58);
+            wait(56);
             in1=1;
             in2=1;
+            pc.printf("\nSending Finished");
             }         
     }
   
@@ -191,9 +188,9 @@
             else {
                  pc.printf("\nBeam Path Detector distance out of range : %ld", pathdist);
                  wait(2);
-                 goto LOOPY;
+                 path_sheets = 111;
                  }
-            pc.printf("\nPlates Detected in Beam Path: %ld", path_sheets);
+            pc.printf("\nDetected Plates: %ld", path_sheets);
             wait(2);
         }
         
@@ -203,112 +200,33 @@
         long Duration = 0;
         long StartTime = millis();
     
-        while(magnetSwitch == 1 && homeSwitch == 1 && Duration <= 58000 ){
+        while(magnetSwitch == 1 && homeSwitch == 1 && Duration <= 52000 ){
              in1=0;
              in2=1;
              long CurrentTime = millis();
              Duration = CurrentTime-StartTime;
-             pc.printf("\nDuration: %d\r\n", Duration); 
         }
         
         in1=1;
         in2=1;
         
-        if (Duration > 58000){
-             pc.printf("\nRetraction Time Out Error");
-             goto LOOPY;
+        if (Duration > 52000){
+             pc.printf("\nRetraction Time Out Error: %d\r\n", Duration);
+             exit(1);
             }
         else if (homeSwitch == 0){
              pc.printf("\nMagnet Switch Broken");
-             goto LOOPY;
+             exit(1);
             }
         else{
-            pc.printf("\nPlate Retraction Complete");
+            pc.printf("\nRetraction Complete");
+            goto LOOPY;
             }
      }
      
-else if (command==4){
-     
-        int plates = 0;
-        unsigned int dist;
-        int platedist;
-        
-        while(plates<=0 || plates>16){
-              
-              pc.printf("\nEnter Number of Plates to Select 0-16: "); 
-              pc.scanf("%d",&plates);
-            
-              if (plates > 16 || plates < 0 ){
-                  pc.printf("\nInvalid Entry"); 
-                  } 
-              else{
-                  pc.printf("\nPlates Requested: %ld", plates);
-                  }
-        }
-        
-        if (magnetSwitch == 1){
-            pc.printf("\nNot in Home Position"); 
-            goto LOOPY;
-            }
-        else{
-            wait(1);
-            in1=1;
-            in2=0;
-            wait_ms(900);
-            in1=1;
-            in2=1;
-            }
-      
-        platedist = (468-24*plates);
-        wait(1);   
-        heightsensor.start();
-        wait_ms(500); 
-        dist = heightsensor.get_dist_cm(); 
-        millisStart();
-        long durationSM = 0;
-        long starttimeSM = millis();  
-        pc.printf("\nPlate Distance:%ld",platedist); 
-        pc.printf("\nSelector Height:%ld",dist);
-        
-        while(dist < platedist-2 || dist > platedist+2 && dist < 480 && durationSM <56000){
-             
-              heightsensor.start();
-              dist = heightsensor.get_dist_cm(); 
-              long currenttimeSM = millis();
-              durationSM = currenttimeSM-starttimeSM;
-              pc.printf("\nSelector Height:%ld",dist); 
-              en = 0;
-              mot.setSpeed(500);
-           
-              if(dist<platedist-2){ 
-                 mot.rotate(1);
-                 wait(0.5);
-                }
-              if(dist>platedist+2){ 
-                mot.rotate(0);
-                wait(0.5);
-               }
-              mot.stop();
-        }
-        
-        mot.stop(); 
-        en = 1;
-        wait(1);
-        
-        if (dist >=480){
-            pc.printf("\nSelector Distance out of Range:%ld",dist);
-            goto LOOPY;
-            }
-        else if (durationSM >= 56000){
-            pc.printf("\nSelector Motor Time Out");
-            goto LOOPY;
-            }
-        else{
-            pc.printf("\nSelection Finished");
-            }
-    }
 else {
       pc.printf("\nInvalid Command");
     }
 }
-}
\ No newline at end of file
+}
+}