File content as of revision 1:80c2ef16a42f:

/*********************************************************
*buggy_functions.cpp                                     *
*Author: Elijah Orr & Dan Argust                         *
*                                                        *
*A library of functions that can be used to control the  *
*RenBuggy.                                               *
*********************************************************/

#ifndef BUGGY_FUNCTIONS_C
#define BUGGY_FUNCTIONS_C

/* necessary includes */
#include "mbed.h"
#include "ultrasonic_buggy.h"

// PwmOut is used to control the motors
PwmOut Lmotor(LEFT_MOTOR_PIN);
PwmOut Rmotor(RIGHT_MOTOR_PIN);

// set up digital in and out pins for the ultrasonic, as well as a timer for use in the ultrasonic functions
DigitalOut trigger(TRIGGER_PIN);
DigitalIn echo_left(LEFT_ECHO_PIN, PullNone);
DigitalIn echo_right(RIGHT_ECHO_PIN, PullNone);
Timer timer;
DigitalOut SR04_nReset(P0_7, 1);

/* Trim is an offset that you can adjust to help the buggy drive straight
Trim = -0.3 is a left trim
Trim =  0.3 is a right trim */
float trim = -0.3;

/* Functions (listed below) contain the code that runs the buggy.
These functions can be used in the main.cpp file */


extern void hold(float time) //waits for (time) seconds
{
    for (float i = time;i>0;i-=0.01){ //For every hundreth of a second, display the time remaining
        wait(0.01);
    }
}

extern void forward(float time) //moves forward for (time) seconds
{
    Lmotor = 1.0 + trim;
    Rmotor = 1.0 - trim; //set the left and right motor to 1.0 (full speed) - the trim offset
    hold(time); //wait for (time) seconds while the motors are on
    stop(); //stops the motors
}


extern void left(float time) //moves left for (time) seconds
{
    Rmotor = 1.0;  //set the right motor to full speed
    Lmotor = 0.0; //set the left motor to off
    hold(time); //waits for (time) seconds
    stop(); //stops the motors
}


extern void right(float time) //moves right for (time) seconds
{
    Lmotor = 1.0;  //set the left motor to full speed
    Rmotor = 0.0; //set the right motor to off
    hold(time); //waits for (time) seconds
    stop(); //stops the motors
}

extern void stop() //stops the motors
{
    Lmotor = Rmotor = 0; //set the speed of each motor to 0
}


/*
*Function: getDistance_l
*
*Gets a left distance reading from the ultrasonic sensor
*
*Returns: float(distance in metres). Returns 4.0 if no reflection is received.
*
*WARNING: SPAGHETTI
*
*/
extern float getDistance_l(void)
{
    float time = 0;
    float distance_m = 0;
    
    trigger:
    timer.stop();
    timer.reset();
    //send trigger signal to ultrasonic
    trigger = 1;        //set trigger pin high
    wait_us(10);        //wait 10 us
    trigger = 0;        //set trigger pin low
    timer.start();
    
    check_echo_high:
    if(echo_left == 1){          //echo pin goes high
        timer.stop();
        timer.reset();
        timer.start();      //start the timer
        check_echo_low:
        if(echo_left == 0){      //echo pin goes low      
            timer.stop();   //stop the timer
            time = timer.read();    //read the timer value
            timer.reset();  //reset the timer
            goto echo_pulse_complete; //go to label echo_pulse_complete
        }
        else{
            if(timer.read_ms() > 30 ){   //if the timer reaches 30ms then echo is stuck high, so reset the SR04
                
                resetSR04();
                                                                       
                distance_m = 4.0;
                goto returnvalue;
                    
                
            }
            else{
                goto check_echo_low;    //go to inner loop so that RenBED keeps checking echo pin for low state
            }
        }
    }
    else{
        if(timer.read_ms() > 20){
            goto trigger;                   //resend trigger if module unresponsive
        }
        goto check_echo_high;              //go to loop so that RenBED keeps checking echo pin for high state
        
    }
    echo_pulse_complete: 
    distance_m = (time * SPEED_OF_SOUND / 2);   //convert the time reading to a distance. Divided by 2 as the sound waves must travel and return.
    
    if(distance_m < 0.02){
        goto trigger;
    }
    
    returnvalue:
    return distance_m;          //return the distance in metres                     
}

/*
*Function: getDistance_r
*
*Gets a right distance reading from the ultrasonic sensor
*
*Returns: float(distance in metres). Returns 4.0 if no reflection is received.
*
*WARNING: SPAGHETTI
*
*/
float getDistance_r(void)
{
    float time = 0;
    float distance_m = 0;
    
    trigger:
    timer.stop();
    timer.reset();
    //send trigger signal to ultrasonic
    trigger = 1;        //set trigger pin high
    wait_us(10);        //wait 10 us
    trigger = 0;        //set trigger pin low
    timer.start();      //start the timer
    
    check_echo_high:
    if(echo_right == 1){          //echo pin goes high
        timer.stop();
        timer.reset();
        timer.start();      //start the timer
        check_echo_low:
        if(echo_right == 0){      //echo pin goes low      
            timer.stop();   //stop the timer
            time = timer.read();    //read the timer value
            timer.reset();  //reset the timer
            goto echo_pulse_complete; //go to label echo_pulse_complete
        }
        else{
            if(timer.read_ms() > 30 ){   //if the timer reaches 30ms then echo is stuck high, so reset the SR04
                
                resetSR04();
                                                                           
                distance_m = 4.0;
                goto returnvalue;
            }
            else{
                goto check_echo_low;    //go to inner loop so that RenBED keeps checking echo pin for low state
            }
        }
    }
    else{
        if(timer.read_ms() > 20){
            goto trigger;                   //resend trigger if module unresponsive
        }
        goto check_echo_high;              //go to loop so that RenBED keeps checking echo pin for high state
        
    }
    echo_pulse_complete: 
    distance_m = (time * SPEED_OF_SOUND / 2);   //convert the time reading to a distance. Divided by 2 as the sound waves must travel and return.
    
    if(distance_m < 0.02){
        goto trigger;
    }
    
    returnvalue:
    return distance_m;          //return the distance in metres                     
}

void resetSR04(void)
{
    SR04_nReset = 0;        //turn off modules
    wait_ms(10);            //wait for a while    
    timer.stop();
    timer.reset();
    SR04_nReset = 1;        //turn modules back on
}    

#endif // BUGGY_FUNCTIONS_C