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 PwmOut Lmotor(LEFT_MOTOR_PIN);
00018 PwmOut Rmotor(RIGHT_MOTOR_PIN);
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.3;
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.0 + trim;
00046     Rmotor = 1.0 - 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.0;  //set the right motor to full speed
00055     Lmotor = 0.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.0;  //set the left motor to full speed
00064     Rmotor = 0.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 4.0 if no reflection is received.
00081 *
00082 *WARNING: SPAGHETTI
00083 *
00084 */
00085 extern float getDistance_l(void)
00086 {
00087     float time = 0;
00088     float distance_m = 0;
00089     
00090     trigger:
00091     timer.stop();
00092     timer.reset();
00093     //send trigger signal to ultrasonic
00094     trigger = 1;        //set trigger pin high
00095     wait_us(10);        //wait 10 us
00096     trigger = 0;        //set trigger pin low
00097     timer.start();
00098     
00099     check_echo_high:
00100     if(echo_left == 1){          //echo pin goes high
00101         timer.stop();
00102         timer.reset();
00103         timer.start();      //start the timer
00104         check_echo_low:
00105         if(echo_left == 0){      //echo pin goes low      
00106             timer.stop();   //stop the timer
00107             time = timer.read();    //read the timer value
00108             timer.reset();  //reset the timer
00109             goto echo_pulse_complete; //go to label echo_pulse_complete
00110         }
00111         else{
00112             if(timer.read_ms() > 30 ){   //if the timer reaches 30ms then echo is stuck high, so reset the SR04
00113                 
00114                 resetSR04();
00115                                                                        
00116                 distance_m = 4.0;
00117                 goto returnvalue;
00118                     
00119                 
00120             }
00121             else{
00122                 goto check_echo_low;    //go to inner loop so that RenBED keeps checking echo pin for low state
00123             }
00124         }
00125     }
00126     else{
00127         if(timer.read_ms() > 20){
00128             goto trigger;                   //resend trigger if module unresponsive
00129         }
00130         goto check_echo_high;              //go to loop so that RenBED keeps checking echo pin for high state
00131         
00132     }
00133     echo_pulse_complete: 
00134     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.
00135     
00136     if(distance_m < 0.02){
00137         goto trigger;
00138     }
00139     
00140     returnvalue:
00141     return distance_m;          //return the distance in metres                     
00142 }
00143 
00144 /*
00145 *Function: getDistance_r
00146 *
00147 *Gets a right distance reading from the ultrasonic sensor
00148 *
00149 *Returns: float(distance in metres). Returns 4.0 if no reflection is received.
00150 *
00151 *WARNING: SPAGHETTI
00152 *
00153 */
00154 float getDistance_r(void)
00155 {
00156     float time = 0;
00157     float distance_m = 0;
00158     
00159     trigger:
00160     timer.stop();
00161     timer.reset();
00162     //send trigger signal to ultrasonic
00163     trigger = 1;        //set trigger pin high
00164     wait_us(10);        //wait 10 us
00165     trigger = 0;        //set trigger pin low
00166     timer.start();      //start the timer
00167     
00168     check_echo_high:
00169     if(echo_right == 1){          //echo pin goes high
00170         timer.stop();
00171         timer.reset();
00172         timer.start();      //start the timer
00173         check_echo_low:
00174         if(echo_right == 0){      //echo pin goes low      
00175             timer.stop();   //stop the timer
00176             time = timer.read();    //read the timer value
00177             timer.reset();  //reset the timer
00178             goto echo_pulse_complete; //go to label echo_pulse_complete
00179         }
00180         else{
00181             if(timer.read_ms() > 30 ){   //if the timer reaches 30ms then echo is stuck high, so reset the SR04
00182                 
00183                 resetSR04();
00184                                                                            
00185                 distance_m = 4.0;
00186                 goto returnvalue;
00187             }
00188             else{
00189                 goto check_echo_low;    //go to inner loop so that RenBED keeps checking echo pin for low state
00190             }
00191         }
00192     }
00193     else{
00194         if(timer.read_ms() > 20){
00195             goto trigger;                   //resend trigger if module unresponsive
00196         }
00197         goto check_echo_high;              //go to loop so that RenBED keeps checking echo pin for high state
00198         
00199     }
00200     echo_pulse_complete: 
00201     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.
00202     
00203     if(distance_m < 0.02){
00204         goto trigger;
00205     }
00206     
00207     returnvalue:
00208     return distance_m;          //return the distance in metres                     
00209 }
00210 
00211 void resetSR04(void)
00212 {
00213     SR04_nReset = 0;        //turn off modules
00214     wait_ms(10);            //wait for a while    
00215     timer.stop();
00216     timer.reset();
00217     SR04_nReset = 1;        //turn modules back on
00218 }    
00219 
00220 #endif // BUGGY_FUNCTIONS_C