Diff: main.cpp

Revision:

10:47aede089c8d

Parent:

9:5263aacbda53

--- a/main.cpp	Mon Jan 21 19:54:15 2019 +0000
+++ b/main.cpp	Mon Jan 21 23:37:54 2019 +0000
@@ -15,6 +15,7 @@
 DigitalOut en(D2); 
 
 int main(){
+    
     LOOPY:
      en = 1;
     in1 = 1;
@@ -61,29 +62,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);
+         
+        SENSORREAD:
+        heightsensor.start();
+        wait_ms(250); 
+        dist = heightsensor.get_dist_cm(); 
+       
         
-        while(dist < platedist-2 || dist > platedist+2 && dist < 480 && durationSM <56000){
+        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,27 +100,29 @@
               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);
+        if (dist >= 480 || dist <= 40){
+            pc.printf("\nSelector Sensor Error");
             goto LOOPY;
             }
-        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");
+            }          
+     }
   
     else if (command==2){
             
@@ -208,7 +216,6 @@
              in2=1;
              long CurrentTime = millis();
              Duration = CurrentTime-StartTime;
-             pc.printf("\nDuration: %d\r\n", Duration); 
         }
         
         in1=1;
@@ -224,6 +231,7 @@
             }
         else{
             pc.printf("\nPlate Retraction Complete");
+            wait(1);
             }
      }