Uses 2 HC-SR04 ultrasonic modules to steer the RenBuggy away from obstacles. Renishaw Team page fork.
Dependencies: mbed
Fork of RenBuggy_Ultrasonic by
Diff: ultrasonic_buggy.cpp
- Revision:
- 1:80c2ef16a42f
- Parent:
- 0:fbceffb594b6
diff -r fbceffb594b6 -r 80c2ef16a42f ultrasonic_buggy.cpp --- a/ultrasonic_buggy.cpp Thu Jul 14 12:58:48 2016 +0000 +++ b/ultrasonic_buggy.cpp Mon Jul 18 10:00:29 2016 +0000 @@ -14,8 +14,8 @@ #include "ultrasonic_buggy.h" // PwmOut is used to control the motors -DigitalOut Lmotor(LEFT_MOTOR_PIN, 0); -DigitalOut Rmotor(RIGHT_MOTOR_PIN, 0); +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); @@ -24,10 +24,10 @@ 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; +/* 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 */ @@ -42,8 +42,8 @@ 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 + 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 } @@ -51,8 +51,8 @@ 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 + 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 } @@ -60,8 +60,8 @@ 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 + 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 } @@ -77,9 +77,10 @@ * *Gets a left distance reading from the ultrasonic sensor * -*Returns: float(distance in metres). Returns 0 if no object within 4m. +*Returns: float(distance in metres). Returns 4.0 if no reflection is received. * *WARNING: SPAGHETTI +* */ extern float getDistance_l(void) { @@ -87,13 +88,18 @@ 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 @@ -104,13 +110,13 @@ } 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 - } + + 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 @@ -118,11 +124,20 @@ } } 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 } @@ -131,7 +146,10 @@ * *Gets a right distance reading from the ultrasonic sensor * -*Returns: float(distance in metres). Returns 0 if no object within 4m. +*Returns: float(distance in metres). Returns 4.0 if no reflection is received. +* +*WARNING: SPAGHETTI +* */ float getDistance_r(void) { @@ -139,13 +157,18 @@ 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 @@ -156,13 +179,11 @@ } 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 - } + + 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 @@ -170,21 +191,30 @@ } } 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; - forward(1); + SR04_nReset = 0; //turn off modules + wait_ms(10); //wait for a while timer.stop(); timer.reset(); - SR04_nReset = 1; -} + SR04_nReset = 1; //turn modules back on +} #endif // BUGGY_FUNCTIONS_C \ No newline at end of file