File content as of revision 0:fbceffb594b6:

/*********************************************************
*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
DigitalOut Lmotor(LEFT_MOTOR_PIN, 0);
DigitalOut Rmotor(RIGHT_MOTOR_PIN, 0);

// 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.6;

/* 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; //+ trim;
    Rmotor = 1; //- 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;  //set the right motor to full speed
    Lmotor = 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;  //set the left motor to full speed
    Rmotor = 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 0 if no object within 4m.
*
*WARNING: SPAGHETTI
*/
extern float getDistance_l(void)
{
    float time = 0;
    float distance_m = 0;
    
    trigger:
    //send trigger signal to ultrasonic
    trigger = 1;        //set trigger pin high
    wait_us(10);        //wait 10 us
    trigger = 0;        //set trigger pin low
    
    check_echo_high:
    if(echo_left == 1){          //echo pin goes high
        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();
                if(echo_left == 0){
                    goto trigger;   //when echo goes low again, reattempt measurement
                }
                else{
                    wait_ms(1);     //otherwise wait for low state
                }
            }
            else{
                goto check_echo_low;    //go to inner loop so that RenBED keeps checking echo pin for low state
            }
        }
    }
    else{
        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.
    
    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 0 if no object within 4m.
*/
float getDistance_r(void)
{
    float time = 0;
    float distance_m = 0;
    
    trigger:
    //send trigger signal to ultrasonic
    trigger = 1;        //set trigger pin high
    wait_us(10);        //wait 10 us
    trigger = 0;        //set trigger pin low
    
    check_echo_high:
    if(echo_right == 1){          //echo pin goes high
        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();
                if(echo_right == 0){
                    goto trigger;   //when echo goes low again, reattempt measurement
                }
                else{
                    wait_ms(1);     //otherwise wait for low state
                }
            }
            else{
                goto check_echo_low;    //go to inner loop so that RenBED keeps checking echo pin for low state
            }
        }
    }
    else{
        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.
    
    return distance_m;          //return the distance in metres                     
}

void resetSR04(void)
{
    SR04_nReset = 0;
    forward(1);
    timer.stop();
    timer.reset();
    SR04_nReset = 1;
}

#endif // BUGGY_FUNCTIONS_C