ultrasonic_buggy.cpp

00001 /*********************************************************
00002 *buggy_functions.cpp                                     *
00003 *Author: Elijah Orr & Dan Argust                         *
00004 *                                                        *
00005 *A library of functions that can be used to control the  *
00006 *RenBuggy.                                               *
00007 *********************************************************/
00008 
00009 #ifndef BUGGY_FUNCTIONS_C
00010 #define BUGGY_FUNCTIONS_C
00011 
00012 /* necessary includes */
00013 #include "mbed.h"
00014 #include "ultrasonic_buggy.h"
00015 
00016 // PwmOut is used to control the motors
00017 DigitalOut Lmotor(LEFT_MOTOR_PIN, 0);
00018 DigitalOut Rmotor(RIGHT_MOTOR_PIN, 0);
00019 
00020 // set up digital in and out pins for the ultrasonic, as well as a timer for use in the ultrasonic functions
00021 DigitalOut trigger(TRIGGER_PIN);
00022 DigitalIn echo_left(LEFT_ECHO_PIN, PullNone);
00023 DigitalIn echo_right(RIGHT_ECHO_PIN, PullNone);
00024 Timer timer;
00025 DigitalOut SR04_nReset(P0_7, 1);
00026 
00027 //Trim is an offset that you can adjust to help the buggy drive straight
00028 //Trim = -0.3 is a left trim
00029 //Trim =  0.3 is a right trim
00030 //float trim = -0.6;
00031 
00032 /* Functions (listed below) contain the code that runs the buggy.
00033 These functions can be used in the main.cpp file */
00034 
00035 
00036 extern void hold(float time) //waits for (time) seconds
00037 {
00038     for (float i = time;i>0;i-=0.01){ //For every hundreth of a second, display the time remaining
00039         wait(0.01);
00040     }
00041 }
00042 
00043 extern void forward(float time) //moves forward for (time) seconds
00044 {
00045     Lmotor = 1; //+ trim;
00046     Rmotor = 1; //- trim; //set the left and right motor to 1.0 (full speed) - the trim offset
00047     hold(time); //wait for (time) seconds while the motors are on
00048     stop(); //stops the motors
00049 }
00050 
00051 
00052 extern void left(float time) //moves left for (time) seconds
00053 {
00054     Rmotor = 1;  //set the right motor to full speed
00055     Lmotor = 0; //set the left motor to off
00056     hold(time); //waits for (time) seconds
00057     stop(); //stops the motors
00058 }
00059 
00060 
00061 extern void right(float time) //moves right for (time) seconds
00062 {
00063     Lmotor = 1;  //set the left motor to full speed
00064     Rmotor = 0; //set the right motor to off
00065     hold(time); //waits for (time) seconds
00066     stop(); //stops the motors
00067 }
00068 
00069 extern void stop() //stops the motors
00070 {
00071     Lmotor = Rmotor = 0; //set the speed of each motor to 0
00072 }
00073 
00074 
00075 /*
00076 *Function: getDistance_l
00077 *
00078 *Gets a left distance reading from the ultrasonic sensor
00079 *
00080 *Returns: float(distance in metres). Returns 0 if no object within 4m.
00081 *
00082 *WARNING: SPAGHETTI
00083 */
00084 extern float getDistance_l(void)
00085 {
00086     float time = 0;
00087     float distance_m = 0;
00088     
00089     trigger:
00090     //send trigger signal to ultrasonic
00091     trigger = 1;        //set trigger pin high
00092     wait_us(10);        //wait 10 us
00093     trigger = 0;        //set trigger pin low
00094     
00095     check_echo_high:
00096     if(echo_left == 1){          //echo pin goes high
00097         timer.start();      //start the timer
00098         check_echo_low:
00099         if(echo_left == 0){      //echo pin goes low      
00100             timer.stop();   //stop the timer
00101             time = timer.read();    //read the timer value
00102             timer.reset();  //reset the timer
00103             goto echo_pulse_complete; //go to label echo_pulse_complete
00104         }
00105         else{
00106             if(timer.read_ms() > 30 ){   //if the timer reaches 30ms then echo is stuck high, so reset the SR04
00107                 resetSR04();
00108                 if(echo_left == 0){
00109                     goto trigger;   //when echo goes low again, reattempt measurement
00110                 }
00111                 else{
00112                     wait_ms(1);     //otherwise wait for low state
00113                 }
00114             }
00115             else{
00116                 goto check_echo_low;    //go to inner loop so that RenBED keeps checking echo pin for low state
00117             }
00118         }
00119     }
00120     else{
00121         goto check_echo_high;              //go to loop so that RenBED keeps checking echo pin for high state
00122     }
00123     echo_pulse_complete: 
00124     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.
00125     
00126     return distance_m;          //return the distance in metres                     
00127 }
00128 
00129 /*
00130 *Function: getDistance_r
00131 *
00132 *Gets a right distance reading from the ultrasonic sensor
00133 *
00134 *Returns: float(distance in metres). Returns 0 if no object within 4m.
00135 */
00136 float getDistance_r(void)
00137 {
00138     float time = 0;
00139     float distance_m = 0;
00140     
00141     trigger:
00142     //send trigger signal to ultrasonic
00143     trigger = 1;        //set trigger pin high
00144     wait_us(10);        //wait 10 us
00145     trigger = 0;        //set trigger pin low
00146     
00147     check_echo_high:
00148     if(echo_right == 1){          //echo pin goes high
00149         timer.start();      //start the timer
00150         check_echo_low:
00151         if(echo_right == 0){      //echo pin goes low      
00152             timer.stop();   //stop the timer
00153             time = timer.read();    //read the timer value
00154             timer.reset();  //reset the timer
00155             goto echo_pulse_complete; //go to label echo_pulse_complete
00156         }
00157         else{
00158             if(timer.read_ms() > 30 ){   //if the timer reaches 30ms then echo is stuck high, so reset the SR04
00159                 resetSR04();
00160                 if(echo_right == 0){
00161                     goto trigger;   //when echo goes low again, reattempt measurement
00162                 }
00163                 else{
00164                     wait_ms(1);     //otherwise wait for low state
00165                 }
00166             }
00167             else{
00168                 goto check_echo_low;    //go to inner loop so that RenBED keeps checking echo pin for low state
00169             }
00170         }
00171     }
00172     else{
00173         goto check_echo_high;              //go to loop so that RenBED keeps checking echo pin for high state
00174     }
00175     echo_pulse_complete: 
00176     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.
00177     
00178     return distance_m;          //return the distance in metres                     
00179 }
00180 
00181 void resetSR04(void)
00182 {
00183     SR04_nReset = 0;
00184     forward(1);
00185     timer.stop();
00186     timer.reset();
00187     SR04_nReset = 1;
00188 }
00189 
00190 #endif // BUGGY_FUNCTIONS_C