File content as of revision 0:5848e605bae5:

#include "mbed.h"
#include "Stepper.h"
#include "string"
#include "hcsr04.h"
Serial pc(USBTX, USBRX);
HCSR04 heightsensor(PTA2,PTC2); //(trig,echo)
HCSR04 pathsensor(PTD3,PTD2);
DigitalOut in1(PTC12);
DigitalOut in2(D7);
DigitalIn homeSwitch(D9);
Stepper mot(D3,D4);  //(D3 to PUL+, D4 to DIR+)
DigitalOut en(D2); 

volatile int plates=0;
volatile unsigned int sheets=0;
volatile unsigned int path_sheets=0;

void plate_selection()
{
    unsigned int dist;
    int er = 5;
    int fp = 470; //adjust if needed
    int lp = 75;  //adjust if needed
    int tp = 26;  
    
    while (plates!=sheets) {
        heightsensor.start();
        wait_ms(500);
        dist=heightsensor.get_dist_cm(); 
        pc.printf("\nmm:%ld",dist);
        
        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);
        }
        
        if (dist<=fp+er && dist>=fp-er){                  
            sheets = 1;
            pc.printf("\nSheets selected:%ld",sheets);
        }
        
        if (dist<= fp-tp-er && dist>=fp-tp-er){       
            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);
        }
        
        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 (dist<= fp-15*tp-er && dist>=fp-15*tp-er){         
            sheets = 16;
            pc.printf("\nSheets selected:%ld",sheets);
        }
    }
    mot.stop();
    en = 1;
    wait(1);
}

void beam()
{
    
    unsigned int path_dist;
    int err = 5;
    int sens_pos = 133; //adjust if needed
    int sheet_th = 26; 
    pathsensor.start();
    wait_ms(500);
    path_dist=pathsensor.get_dist_cm();
    pc.printf("\nmm:%ld",path_dist);
    path_sheets = 17;
        
            if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){                   //127-139
               path_sheets = 16;
                
            }
            if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){ // 153-165 
               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;
                
            }
            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;
                
            }
            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
               path_sheets = 10;
                
            }
            if (path_dist >= sens_pos-err+7*sheet_th && path_dist <= sens_pos+err+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
               path_sheets = 8;
                
            }
            if (path_dist >= sens_pos-err+9*sheet_th && path_dist <= sens_pos+err+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){
               path_sheets = 6;
                
            }
            if (path_dist >= sens_pos-err+11*sheet_th && path_dist <= sens_pos+err+11*sheet_th){
               path_sheets = 5;
                
            }
            if (path_dist >= sens_pos-err+12*sheet_th && path_dist <= sens_pos+err+12*sheet_th){
               path_sheets = 4;
                
            }
            if (path_dist >= sens_pos-err+13*sheet_th && path_dist <= sens_pos+err+13*sheet_th){ //465-477
               path_sheets = 3;
                
            }
            if (path_dist >= sens_pos-err+14*sheet_th && path_dist <= sens_pos+err+14*sheet_th){
               path_sheets = 2;
                
            }
            if (path_dist >= sens_pos-err+15*sheet_th && path_dist <= sens_pos+err+15*sheet_th){
               path_sheets = 1;
                
            }
            if (path_dist > sens_pos-err+16*sheet_th){
               path_sheets = 0;
            }
}
void platedetect()
{ 
    if(homeSwitch==0 && path_sheets==0) 
    {
    pc.printf("\nAll plates are in the start position");
    }
    else if (homeSwitch==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 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()
{
    while(homeSwitch == 1)
    {
    in1=0;
    in2=1;
    }
    in1=1;
    in2=1;
}

void send()                 
{ 
    in1=1;
    in2=0;
    wait(17);
    in1=1;
    in2=1;
}
void internalpullups()
{
    homeSwitch.mode(PullUp);
    wait (.01);
}

void send_error_check()
{
    if (plates <= 0 || plates >= 17) 
    {
        pc.printf("\nERROR: Invalid Range |");
    } 
    else if(homeSwitch == 1)
    {
        pc.printf("\nERROR: Not at Home Position |");
    }
    else 
    {
        plate_selection();
        send();
    }
}

int main() {
  
   while(1){
        plates = (rand() % 16) + 1;
        pc.printf("\nSending: ",plates);
        send_error_check();
        beam();
        platedetect();
        wait(2);
        retract();
       }
   
   
}