Uses 2 HC-SR04 ultrasonic modules to steer the RenBuggy away from obstacles. Renishaw Team page fork.
Dependencies: mbed
Fork of RenBuggy_Ultrasonic by
ultrasonic_buggy.cpp
- Committer:
- RenBuggy
- Date:
- 2016-07-18
- Revision:
- 1:80c2ef16a42f
- Parent:
- 0:fbceffb594b6
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