File content as of revision 10:47aede089c8d:

#include "mbed.h"
#include "Stepper.h"
#include "string"
#include "hcsr04.h"
#include "millis.h"

Serial pc(USBTX, USBRX);
HCSR04 heightsensor(PTD2,PTD3);
HCSR04 pathsensor(PTC2,PTA2); //(trig,echo)
DigitalOut in1(PTC12);
DigitalOut in2(D7);
DigitalIn magnetSwitch(D10);
DigitalIn homeSwitch(D9);
Stepper mot(D3,D4);  
DigitalOut en(D2); 

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);

    if (command==1){
        
        int plates = 0;
        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);
                  }
        }
        
        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;
            }
        pc.printf("\nRequested Plates:%ld",plates); 
        platedist = (468-24*plates);  
        millisStart();
        long durationSM = 0;
        long starttimeSM = millis();  
         
        SENSORREAD:
        heightsensor.start();
        wait_ms(250); 
        dist = heightsensor.get_dist_cm(); 
       
        
        while ((dist < platedist-2 && durationSM < 60000 || dist > platedist+2 && durationSM < 60000)){
             
             
              heightsensor.start();
              long currenttimeSM = millis();
              durationSM = currenttimeSM-starttimeSM;
              en = 0;
              mot.setSpeed(500);
              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);
                }
              if(dist>platedist+2){ 
                mot.rotate(0);
                wait(0.5);
               }
              mot.stop();
        }
        
        pc.printf("\nSelector Height:%ld", dist);
        pc.printf("\nSelector Motor Duration:%ld", durationSM);
        mot.stop(); 
        en = 1;
        wait(1);
        
        if (dist >= 480 || dist <= 40){
            pc.printf("\nSelector Sensor Error");
            goto LOOPY;
            }
        else if (durationSM >= 60000){
            pc.printf("\nSelector Motor Time Out");
            exit(1);
            }
        else{
            in1=1;
            in2=0;
            wait(56);
            in1=1;
            in2=1;
            pc.printf("\nSending Finished");
            }          
     }
  
    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 (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;
                
            }
            else if (pathdist >= sens_pos-detectionTOL+7*sheet_th && pathdist <= sens_pos+detectionTOL+7*sheet_th){ //309-321
               path_sheets = 9;
                
            }
            else if (pathdist >= sens_pos-detectionTOL+8*sheet_th && pathdist <= sens_pos+detectionTOL+8*sheet_th){ //323-333
               path_sheets = 8;
                
            }
            else if (pathdist >= sens_pos-detectionTOL+9*sheet_th && pathdist <= sens_pos+detectionTOL+9*sheet_th){ //349-359
               path_sheets = 7;
                
            }
            else if (pathdist >= sens_pos-detectionTOL+10*sheet_th && pathdist <= sens_pos+detectionTOL+10*sheet_th){
               path_sheets = 6;
                
            }
            else if (pathdist >= sens_pos-detectionTOL+11*sheet_th && pathdist <= sens_pos+detectionTOL+11*sheet_th){
               path_sheets = 5;
                
            }
            else if (pathdist >= sens_pos-detectionTOL+12*sheet_th && pathdist <= sens_pos+detectionTOL+12*sheet_th){
               path_sheets = 4;
                
            }
            else if (pathdist >= sens_pos-detectionTOL+13*sheet_th && pathdist <= sens_pos+detectionTOL+13*sheet_th){ //468,478
               path_sheets = 3;
                
            }
            else if (pathdist >= sens_pos-detectionTOL+14*sheet_th && pathdist <= sens_pos+detectionTOL+14*sheet_th){ //494,504
               path_sheets = 2;
                
            }
            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;
                
            }
            else if (pathdist > sens_pos-detectionTOL+16*sheet_th){  //greater than 534
               path_sheets = 0;
              } 
            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 (command==3){
        
        millisStart();
        long Duration = 0;
        long StartTime = millis();
    
        while(magnetSwitch == 1 && homeSwitch == 1 && Duration <= 58000 ){
             in1=0;
             in2=1;
             long CurrentTime = millis();
             Duration = CurrentTime-StartTime;
        }
        
        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");
            wait(1);
            }
     }
     
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");
    }
}
}