File content as of revision 8:3f4d78017fd9:

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

SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd"); // mosi, miso, sclk, cs, name
Serial pc(USBTX, USBRX);
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+)
DigitalOut en(D2); 

volatile int plates = 0;
volatile unsigned int sheets=0;
volatile unsigned int path_sheets=0;
volatile int stop_error=0;
volatile int error_distance;
volatile int beam_distance;
volatile int broken_switch;
volatile int stage = 1;
 
void switch_check(){
    stage = 1;
    pc.printf("\nSwitch Check ");
        if (homeSwitch == 1 && magnetSwitch == 1){
            stop_error = 1;
            broken_switch = 2; //both switches broken
            }
        else if (homeSwitch == 0){
            stop_error = 1;
            broken_switch = 1; //magnet switch isn't working
            }
        else{
            broken_switch = 0;
            }
}

void plate_selection(){
    stage = 2;
    unsigned int dist;
    int er = 5;
    int finp = 447;   //sensor height initial
    int tp = 25; 
    wait(1);
    in1=1;
    in2=0;
    wait_ms(900); //stair fix
    in1=1;
    in2=1;
    wait(1);
    millisStart();
    long Duration2 = 0;
    long StartTime2 = millis();
    
while (plates!=sheets && Duration2 <= 55000) {
        
        heightsensor.start();
        wait_ms(250);
        dist=heightsensor.get_dist_cm(); 
        pc.printf("\nmm:%ld",dist);
        long CurrentTime2 = millis();
        Duration2 = CurrentTime2-StartTime2;
        
        if ( dist < 30 || dist > 500){ 
            error_distance = dist;
            pc.printf("\n Distance Error: %ld", error_distance);
            stop_error = 2;
            break;
            } 
           // pc.printf("\nin while loop");
        if (plates<sheets){
            en = 0;
            mot.setSpeed(500);
            mot.rotate(1);
            }
        if (plates>sheets){
            en = 0;
            mot.setSpeed(500);
            mot.rotate(0);
            }
        if (dist<=finp+er && dist>=finp-er){                 
            sheets = 1;
            }
        if (dist<= finp-tp+er && dist>=finp-tp-er){    
            sheets = 2;
            }
        if (dist<= finp-2*tp+er && dist>=finp-2*tp-er){     
            sheets = 3;
        }
        if (dist<= finp-3*tp+er && dist>=finp-3*tp-er){         
            sheets = 4;
        }
        if (dist<= finp-4*tp+er && dist>=finp-4*tp-er){         
            sheets = 5;
             
             }
         if (dist<= finp-5*tp+er && dist>=finp-5*tp-er){       
            sheets = 6;
             
        }
         if (dist<= finp-6*tp+er && dist>=finp-6*tp-er){     
            sheets = 7;
             
        }
         if (dist<= finp-7*tp+er && dist>=finp-7*tp-er){         
            sheets = 8;
             
        }
         if (dist<= finp-8*tp+er && dist>=finp-8*tp-er){       
            sheets = 9;
             
        }
         if (dist<= finp-9*tp+er && dist>=finp-9*tp-er){         
            sheets = 10;
             
        }
         if (dist<= finp-10*tp+er && dist>=finp-10*tp-er){         
            sheets = 11;
             
        }
         if (dist<= finp-11*tp+er && dist>=finp-11*tp-er){        
            sheets = 12;
             
        }
         if (dist<= finp-12*tp+er && dist>=finp-12*tp-er){         
            sheets = 13;
             
        }
         if (dist<= finp-13*tp+er && dist>=finp-13*tp-er){        
            sheets = 14;
             
        }
         if (dist<= finp-14*tp+er && dist>=finp-14*tp-er){     //102 to 112 
            sheets = 15;
             
        }
         if (dist<= 70 && dist>= 60){        //77 to 87
            sheets = 16;    
        }
    }
    mot.stop();
    en = 1;
    wait(1);
    if (Duration2 > 55000){
    stop_error = 3;    
    }
}

void beam()
{ 
    stage = 3;
    unsigned int path_dist;
    int err = 12;
    int sens_pos = 131; 
    int sheet_th = 25; 
    pathsensor.start();
    wait_ms(250);
    path_dist=pathsensor.get_dist_cm();
    pc.printf("\nBeam Path Sensor: %ld",path_dist);
           
            if (path_dist < 110){
               beam_distance = path_dist;
               stop_error = 6;          
            }

            if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){     //119  143      
               path_sheets = 16;
                
            }
            if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){
               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){ 
               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-10-err+15*sheet_th && path_dist <= sens_pos-10+err+15*sheet_th){
               path_sheets = 1;
                
            }
            if (path_dist > sens_pos-err+16*sheet_th){  
               path_sheets = 0;
            }   
}

void retract()
{
    stage = 4;
    millisStart();
    long Duration = 0;
    long StartTime = millis();
    
    while(homeSwitch == 1 && magnetSwitch == 1 && Duration <= 60000 )
    {
    in1=0;
    in2=1;
    long CurrentTime = millis();
    Duration = CurrentTime-StartTime;
    }
    in1=1;
    in2=1;
    if (Duration > 60000){
    stop_error = 5; 
    }
    else{
    stop_error = 0;
    }
}

void send()                 
{ 
    in1=1;
    in2=0;
    wait(58);
    in1=1;
    in2=1;
}


void logwriting()
{
   FILE *fp = fopen("/sd/mydir/sdtest.txt", "a");
    if(fp == NULL) {
        pc.printf("\nError Writing to Log ");
        }    
    pc.printf("\nLogging ");
    pc.printf("\nStop Error: %ld ", stop_error);
    
    switch (stop_error){
        case 1:
        fprintf(fp,"\nRequested: %ld ",plates);
        fprintf(fp,"\nBroken Switch: %ld ", broken_switch);
        fprintf(fp,"\nStop Error: %ld ", stop_error);
        fclose(fp);
        exit(1);
        break;
        case 2:
        fprintf(fp,"\nRequested: %ld ",plates);
        fprintf(fp,"\nSelector Sensor Distance : %ld ", error_distance);
        fprintf(fp,"\nStop Error: %ld ", stop_error);
        fclose(fp);
        exit(1);
        break;
        case 3:
        fprintf(fp,"\nRequested: %ld ",plates);
        fprintf(fp,"\nSelector Timeout");
        fprintf(fp,"\nStop Error: %ld ", stop_error);
        fclose(fp);
        exit(1);
        break;
        case 4:
        fprintf(fp,"\nRequested: %ld ", plates);
        fprintf(fp,"\nSelected: %ld ", sheets);
        fprintf(fp,"\nDetected: %ld ", path_sheets);
        fprintf(fp,"\nStop Error: %ld ", stop_error);
        fclose(fp);
        break;
        case 5:
        fprintf(fp,"\nRequested: %ld ", plates);
        fprintf(fp,"\nSelected: %ld ", sheets);
        fprintf(fp,"\nDetected: %ld ", path_sheets);
        fprintf(fp,"\nStop Error: %ld ", stop_error);
        fclose(fp);
        exit(1);
        break;
        case 6:
        fprintf(fp,"\nRequested: %ld ", plates);
        fprintf(fp,"\nSelected: %ld ", sheets);
        fprintf(fp,"\nBeam Sensor Distance: %ld ", beam_distance);
        fprintf(fp,"\nStop Error: %ld ", stop_error);
        fclose(fp);
        exit(1);
        break;
        case 7:
        fprintf(fp,"\nRequested: %ld ", plates);
        fprintf(fp,"\nNot at Start Position ");
        fprintf(fp,"\nStop Error: %ld ", stop_error);
        fclose(fp);
        exit(1);
        default: 
        pc.printf("\nstage: %ld ", stage);
            if (stage == 1){
                pc.printf("\nRequested: %ld ", plates);
                fprintf(fp,"\nRequested: %ld ",plates);
                fclose(fp);
                }
            else if (stage == 2) {
                pc.printf("\nSelected: %ld ", sheets);
                fprintf(fp,"\nSelected: %ld ", sheets);
                fclose(fp);
                }
            else if (stage == 3){
                pc.printf("\nDetected: %ld ", path_sheets);
                fprintf(fp,"\nDetected: %ld ", path_sheets);
                fclose(fp);
                }
            else{
                pc.printf("\nPass ");
                fprintf(fp,"\nPass ");
                fclose(fp);
                }     
        break;
  }
}

int main()
{   
    homeSwitch.mode(PullUp);
    wait(.01);
    magnetSwitch.mode(PullUp);
    wait (.01);
    mkdir("/sd/mydir", 0777);
    in1=1;
    in2=1;
    en = 1;
    wait(0.1);
   
    while(stop_error==0) 
    {
            plates = (rand() % 16) + 1;
            wait(1);
            switch_check();     //stage 1
            wait(1);
            logwriting();       // plates requestd
            wait(1);
            plate_selection();  //stage 2
            wait(1);
            logwriting();       //paltes selected
            wait(1);
            send();
            wait(1);
            beam();             //stage 3
            wait(1);
            logwriting();       //plates detected
            wait(1);
            retract();          //stage 4
            wait(1);
            logwriting();       //nothing     
    }   
    pc.printf("\nError was detected. Stopping Program");
}