Diff: main.cpp

Revision:

9:5263aacbda53

Parent:

8:a7b443aee89b

Child:

10:47aede089c8d

--- a/main.cpp	Fri Jan 18 21:07:57 2019 +0000
+++ b/main.cpp	Mon Jan 21 19:54:15 2019 +0000
@@ -3,360 +3,312 @@
 #include "string"
 #include "hcsr04.h"
 #include "millis.h"
-//#include "SDFileSystem.h"
 
 Serial pc(USBTX, USBRX);
+HCSR04 heightsensor(PTD2,PTD3);
 HCSR04 pathsensor(PTC2,PTA2); //(trig,echo)
-HCSR04 heightsensor(PTD2,PTD3);
 DigitalOut in1(PTC12);
 DigitalOut in2(D7);
-
-//DigitalIn homeSwitch(D9);
 DigitalIn magnetSwitch(D10);
-Stepper mot(D3,D4);  //(D3 to PUL+, D4 to DIR+)
+DigitalIn homeSwitch(D9);
+Stepper mot(D3,D4);  
 DigitalOut en(D2); 
 
-volatile int plates=0;
-volatile unsigned int sheets=0;
-volatile unsigned int path_sheets=0;
+int main(){
+    LOOPY:
+     en = 1;
+    in1 = 1;
+    in2 = 1;
+    magnetSwitch.mode(PullUp);
+    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);
 
-void plate_selection()
-{
-    unsigned int dist;
-    int er = 5;
-    int fp = 447; //adjust if needed
-    int lp = 64;  //adjust if needed
-    int tp = 25;  
-    wait(1);
-    in1=1;
-    in2=0;
-    wait_ms(850);
-    in1=1;
-    in2=1;
-    wait(1);
-    while (plates!=sheets) {
-        heightsensor.start();
-        wait_ms(500);
-        dist=heightsensor.get_dist_cm(); 
-        pc.printf("\nmm:%ld",dist);
+    if (command==1){
         
-        if (dist<=lp-10 || dist >= fp+10){
-        mot.stop();
-        en = 1;
-        sheets = plates;
-        pc.printf("\nERROR: Selector is outside of plate range\nRestart/Reset Program");
-        }
-        else if (plates<sheets){
-            en = 0;
-            mot.setSpeed(600);
-            mot.rotate(1);
-        }
-        else{
-            en = 0;
-            mot.setSpeed(600);
-            mot.rotate(0);
-        }
+        int plates = 0;
+        unsigned int dist;
+        int platedist;
         
-        if (dist<=fp+er && dist>=fp-er){          //450,440        
-            sheets = 1;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-        
-        if (dist<= fp-tp+er && dist>=fp-tp-er){   //424,414   
-            sheets = 2;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-        
-        if (dist<= fp-2*tp+er && dist>=fp-2*tp-er){     
-            sheets = 3;
-            pc.printf("\nSheets selected:%ld",sheets);
+        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);
+                  }
         }
         
-        if (dist<= fp-3*tp+er && dist>=fp-3*tp-er){         
-            sheets = 4;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-         if (dist<= fp-4*tp+er && dist>=fp-4*tp-er){         
-            sheets = 5;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-         if (dist<= fp-5*tp+er && dist>=fp-5*tp-er){       
-            sheets = 6;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-         if (dist<= fp-6*tp+er && dist>=fp-6*tp-er){     
-            sheets = 7;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-         if (dist<= fp-7*tp+er && dist>=fp-7*tp-er){         
-            sheets = 8;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-         if (dist<= fp-8*tp+er && dist>=fp-8*tp-er){       
-            sheets = 9;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-         if (dist<= fp-9*tp+er && dist>=fp-9*tp-er){         
-            sheets = 10;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-         if (dist<= fp-10*tp+er && dist>=fp-10*tp-er){         
-            sheets = 11;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-         if (dist<= fp-11*tp+er && dist>=fp-11*tp-er){        
-            sheets = 12;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-         if (dist<= fp-12*tp+er && dist>=fp-12*tp-er){         
-            sheets = 13;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-         if (dist<= fp-13*tp+er && dist>=fp-13*tp-er){        
-            sheets = 14;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-         if (dist<= fp-14*tp+er && dist>=fp-14*tp-er){      
-            sheets = 15;
-            pc.printf("\nSheets selected:%ld",sheets);
+        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();
         }
-         if (dist<= fp-15*tp+er && dist>=fp-15*tp-er){       // 75, 65 
-            sheets = 16;
-            pc.printf("\nSheets selected:%ld",sheets);
-        }
-    }
-    mot.stop();
-    en = 1;
-    wait(1);
-}
-
-void beam()
-{
-    
-    unsigned int path_dist;
-    int err = 12;
-    int sens_pos = 131; //adjust if needed //535 0 plates
-    int sheet_th = 25; 
-    pathsensor.start();
-    wait_ms(500);
-    path_dist=pathsensor.get_dist_cm();
-    pc.printf("\nBeam Path Sensor : %ld",path_dist);
-    path_sheets = 17;
+        
+        mot.stop(); 
+        en = 1;
+        wait(1);
         
-            if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){        //110,130          
-               path_sheets = 16;
-                
+        if (dist >=480){
+            pc.printf("\nSelector Distance out of Range:%ld",dist);
+            goto LOOPY;
             }
-            if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){ //152,168 a:
-               path_sheets = 15;
-                
-            }
-            if (path_dist >= sens_pos-err+2*sheet_th && path_dist <= sens_pos+err+2*sheet_th){ //179-191
-               path_sheets = 14;
-                
+        else if (durationSM >= 56000){
+            pc.printf("\nSelector Motor Time Out");
+            goto LOOPY;
             }
-            if (path_dist >= sens_pos-err+3*sheet_th && path_dist <= sens_pos+err+3*sheet_th){ //205-217
-               path_sheets = 13;
-                 
-            }
-            if (path_dist >= sens_pos-err+4*sheet_th && path_dist <= sens_pos+err+4*sheet_th){ //231-243
-               path_sheets = 12;
+        else{
+            pc.printf("\nPlates Selected: %ld", plates);
+            in1=1;
+            in2=0;
+            wait(58);
+            in1=1;
+            in2=1;
+            }         
+    }
+  
+    else if (command==2){
+            
+            unsigned int pathdist;
+            int path_sheets;
+            int detectionTOL = 12;
+            int sens_pos = 131;          
+            int sheet_th = 25; 
+            pathsensor.start();
+            wait_ms(500);
+            pathdist=pathsensor.get_dist_cm();
                 
-            }
-            if (path_dist >= sens_pos-err+5*sheet_th && path_dist <= sens_pos+err+5*sheet_th){ //257-269
-               path_sheets = 11;
-                
-            }
-            if (path_dist >= sens_pos-err+6*sheet_th && path_dist <= sens_pos+err+6*sheet_th){ //283-295
+            if (pathdist >= sens_pos-detectionTOL && pathdist <= sens_pos+detectionTOL){        //110,130          
+                path_sheets = 16; 
+                }
+            else if (pathdist >= sens_pos-detectionTOL+sheet_th && pathdist <= sens_pos+detectionTOL+sheet_th){ //152,168 a:
+                path_sheets = 15;   
+                }
+            else if (pathdist >= sens_pos-detectionTOL+2*sheet_th && pathdist <= sens_pos+detectionTOL+2*sheet_th){ //179-191
+                path_sheets = 14;
+                }
+            else if (pathdist >= sens_pos-detectionTOL+3*sheet_th && pathdist <= sens_pos+detectionTOL+3*sheet_th){ //205-217
+                path_sheets = 13;                 
+                }
+            else if (pathdist >= sens_pos-detectionTOL+4*sheet_th && pathdist <= sens_pos+detectionTOL+4*sheet_th){ //231-243
+                path_sheets = 12;
+                }
+            else if (pathdist >= sens_pos-detectionTOL+5*sheet_th && pathdist <= sens_pos+detectionTOL+5*sheet_th){ //257-269
+                path_sheets = 11;
+                }
+            else if (pathdist >= sens_pos-detectionTOL+6*sheet_th && pathdist <= sens_pos+detectionTOL+6*sheet_th){ //283-295
                path_sheets = 10;
                 
             }
-            if (path_dist >= sens_pos-err+7*sheet_th && path_dist <= sens_pos+err+7*sheet_th){ //309-321
+            else if (pathdist >= sens_pos-detectionTOL+7*sheet_th && pathdist <= sens_pos+detectionTOL+7*sheet_th){ //309-321
                path_sheets = 9;
                 
             }
-            if (path_dist >= sens_pos-err+8*sheet_th && path_dist <= sens_pos+err+8*sheet_th){ //323-333
+            else if (pathdist >= sens_pos-detectionTOL+8*sheet_th && pathdist <= sens_pos+detectionTOL+8*sheet_th){ //323-333
                path_sheets = 8;
                 
             }
-            if (path_dist >= sens_pos-err+9*sheet_th && path_dist <= sens_pos+err+9*sheet_th){ //349-359
+            else if (pathdist >= sens_pos-detectionTOL+9*sheet_th && pathdist <= sens_pos+detectionTOL+9*sheet_th){ //349-359
                path_sheets = 7;
                 
             }
-            if (path_dist >= sens_pos-err+10*sheet_th && path_dist <= sens_pos+err+10*sheet_th){
+            else if (pathdist >= sens_pos-detectionTOL+10*sheet_th && pathdist <= sens_pos+detectionTOL+10*sheet_th){
                path_sheets = 6;
                 
             }
-            if (path_dist >= sens_pos-err+11*sheet_th && path_dist <= sens_pos+err+11*sheet_th){
+            else if (pathdist >= sens_pos-detectionTOL+11*sheet_th && pathdist <= sens_pos+detectionTOL+11*sheet_th){
                path_sheets = 5;
                 
             }
-            if (path_dist >= sens_pos-err+12*sheet_th && path_dist <= sens_pos+err+12*sheet_th){
+            else if (pathdist >= sens_pos-detectionTOL+12*sheet_th && pathdist <= sens_pos+detectionTOL+12*sheet_th){
                path_sheets = 4;
                 
             }
-            if (path_dist >= sens_pos-err+13*sheet_th && path_dist <= sens_pos+err+13*sheet_th){ //468,478
+            else if (pathdist >= sens_pos-detectionTOL+13*sheet_th && pathdist <= sens_pos+detectionTOL+13*sheet_th){ //468,478
                path_sheets = 3;
                 
             }
-            if (path_dist >= sens_pos-err+14*sheet_th && path_dist <= sens_pos+err+14*sheet_th){ //494,504
+            else if (pathdist >= sens_pos-detectionTOL+14*sheet_th && pathdist <= sens_pos+detectionTOL+14*sheet_th){ //494,504
                path_sheets = 2;
                 
             }
-            if (path_dist >= sens_pos-10-err+15*sheet_th && path_dist <= sens_pos-10+err+15*sheet_th){//greater than 509 less  517
+            else if (pathdist >= sens_pos-10-detectionTOL+15*sheet_th && pathdist <= sens_pos-10+detectionTOL+15*sheet_th){//greater than 509 less  517
                path_sheets = 1;
                 
             }
-            if (path_dist > sens_pos-err+16*sheet_th){  //greater than 534
+            else if (pathdist > sens_pos-detectionTOL+16*sheet_th){  //greater than 534
                path_sheets = 0;
-            }
-}
-
-void platedetect()
-{ 
-    if(magnetSwitch==0 && path_sheets==0) 
-    {
-    pc.printf("\nAll plates are in the start position");
-    }
-    else if (magnetSwitch==1 && path_sheets==0)
-    {
-    pc.printf("\nError-Plates are not fully extended");    
-    }
-    else 
-    {
-        if (plates==path_sheets)
-        {
-        pc.printf("\nPlates in beam path:%ld",path_sheets);
+              } 
+            else {
+                 pc.printf("\nBeam Path Detector distance out of range : %ld", pathdist);
+                 wait(2);
+                 goto LOOPY;
+                 }
+            pc.printf("\nPlates Detected in Beam Path: %ld", path_sheets);
+            wait(2);
         }
-        else if (plates != path_sheets && path_sheets <= 16)
-        {
-        pc.printf("\nError: %ld",path_sheets);
-        pc.printf(" Plates in beam path, but user sent %ld",plates);
-        pc.printf(" Plates");
-        }
-        else{
-             pc.printf("\nERROR: Additional objects detected in beam path");
-            }
-    }    
-}
-void retract()
-{
-    millisStart();
-    long Duration = 0;
-    long StartTime = millis();
+        
+else if (command==3){
+        
+        millisStart();
+        long Duration = 0;
+        long StartTime = millis();
     
-    while(magnetSwitch == 1 && Duration <= 60000 )
-    {
-    in1=0;
-    in2=1;
-    long CurrentTime = millis();
-    Duration = CurrentTime-StartTime;
-    pc.printf("Duration = %d\r\n", Duration); 
+        while(magnetSwitch == 1 && homeSwitch == 1 && Duration <= 58000 ){
+             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;
+            }
+        else if (homeSwitch == 0){
+             pc.printf("\nMagnet Switch Broken");
+             goto LOOPY;
+            }
+        else{
+            pc.printf("\nPlate Retraction Complete");
+            }
+     }
+     
+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");
+            }
     }
-    in1=1;
-    in2=1;
-}
-
-void send()                 
-{ 
-    in1=1;
-    in2=0;
-    wait(58);
-    in1=1;
-    in2=1;
-}
-
-void internalpullups()
-{
-    magnetSwitch.mode(PullUp);
-    wait (.01);
-    //homeSwitch.mode(PullUp);
-    //wait (0.01);
-}
-
-void send_error_check()
-{
-    if (plates <= 0 || plates >= 17) 
-    {
-        pc.printf("\nERROR: Invalid Range |");
-    } 
-    else if(magnetSwitch == 1)
-    {
-        pc.printf("\nERROR: Not at Home Position |");
-    }
-    else 
-    {
-        plate_selection();
-        send();
+else {
+      pc.printf("\nInvalid Command");
     }
 }
-void select_error_check()
-{
-    if (magnetSwitch == 1) 
-    {
-        pc.printf("\nERROR: Not at Home Position |");
-    } 
-    else 
-    {
-        plate_selection();
-    }
-}
-void retract_error_check()
-{
-    if (magnetSwitch == 0)
-    {
-        pc.printf("\nERROR: Already at Home Position |");   
-    }
-    else
-    {
-        retract();
-    }
-}
-
-int main()
-{   
-    in1=1;
-    in2=1;
-    en = 1;
-    wait(0.1);
-    internalpullups();
-    wait(0.1);  
-    char command[15] = {0};
-    pc.printf("\nCommands:\n\n\ts [N]\t**moves N PMMA sheets in beam path\n\tr\t\t**removes all PMMA from beam path\n\tc [N]\t**raises bar to \n\tb\t\t**reports number of plates in beam path\n\n");
-    
-    while(1) 
-    {
-        pc.printf("\nRequests:  ");
-        pc.scanf("%s",&command);
-    
-        if (strcmp (command, "s") == 0) 
-        {   
-            pc.scanf(" %d",&plates);
-            pc.printf("\nresponse recieved |");
-            send_error_check();
-            beam();
-            platedetect();
-        }
-        else if (strcmp (command, "r") == 0)
-        {   
-            pc.printf("\nresponse recieved |");
-            retract_error_check();
-        }
-        else if (strcmp (command, "c") == 0)
-        {   
-            pc.scanf(" %d",&plates);
-            pc.printf("\nresponse recieved |");
-            select_error_check();
-        }
-         else if (strcmp (command, "b") == 0)
-        {   
-            pc.printf("\nresponse recieved |");
-            beam();
-            platedetect();
-        }
-        else
-        {
-            pc.printf("\nERROR: Invalid Command");       
-        }
-    }
 }
\ No newline at end of file