File content as of revision 3:fd2ca631ab44:

#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 = 448; //adjust if needed
    int lp = 64;  //adjust if needed
    int tp = 25;  
    
    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){          //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);
        }
        
        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){       // 75, 65 
            sheets = 16;
            pc.printf("\nSheets selected:%ld",sheets);
        }
    }
    mot.stop();
    en = 1;
    wait(1);
}

void beam()
{
    
    unsigned int path_dist;
    int err = 8;
    int sens_pos = 148; //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;
        
            if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){        //127,143 a:152          
               path_sheets = 16;
                
            }
            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;
                
            }
            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){ //468,478
               path_sheets = 3;
                
            }
            if (path_dist >= sens_pos-err+14*sheet_th && path_dist <= sens_pos+err+14*sheet_th){ //494,504
               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();
    }
}
void select_error_check()
{
    if (homeSwitch == 1) 
    {
        pc.printf("\nERROR: Not at Home Position |");
    } 
    else 
    {
        plate_selection();
    }
}
void retract_error_check()
{
    if (homeSwitch == 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");       
        }
    }
}