Renishaw / Mbed 2 deprecated RenBuggy_Ultrasonic

Uses 2 HC-SR04 ultrasonic modules to steer the RenBuggy away from obstacles. Renishaw Team page fork.

Dependencies:   mbed

Fork of RenBuggy_Ultrasonic by Ren Buggy

ultrasonic_buggy.cpp@1:80c2ef16a42f, 2016-07-18 (annotated)

Committer:
RenBuggy
Date:
Mon Jul 18 10:00:29 2016 +0000
Revision:
1:80c2ef16a42f
Parent:
0:fbceffb594b6 
First working revision, uses 2 sensors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RenBuggy 0:fbceffb594b6 1 /*********************************************************
RenBuggy 0:fbceffb594b6 2 *buggy_functions.cpp *
RenBuggy 0:fbceffb594b6 3 *Author: Elijah Orr & Dan Argust *
RenBuggy 0:fbceffb594b6 4 * *
RenBuggy 0:fbceffb594b6 5 *A library of functions that can be used to control the *
RenBuggy 0:fbceffb594b6 6 *RenBuggy. *
RenBuggy 0:fbceffb594b6 7 *********************************************************/
RenBuggy 0:fbceffb594b6 8
RenBuggy 0:fbceffb594b6 9 #ifndef BUGGY_FUNCTIONS_C
RenBuggy 0:fbceffb594b6 10 #define BUGGY_FUNCTIONS_C
RenBuggy 0:fbceffb594b6 11
RenBuggy 0:fbceffb594b6 12 /* necessary includes */
RenBuggy 0:fbceffb594b6 13 #include "mbed.h"
RenBuggy 0:fbceffb594b6 14 #include "ultrasonic_buggy.h"
RenBuggy 0:fbceffb594b6 15
RenBuggy 0:fbceffb594b6 16 // PwmOut is used to control the motors
RenBuggy 1:80c2ef16a42f 17 PwmOut Lmotor(LEFT_MOTOR_PIN);
RenBuggy 1:80c2ef16a42f 18 PwmOut Rmotor(RIGHT_MOTOR_PIN);
RenBuggy 0:fbceffb594b6 19
RenBuggy 0:fbceffb594b6 20 // set up digital in and out pins for the ultrasonic, as well as a timer for use in the ultrasonic functions
RenBuggy 0:fbceffb594b6 21 DigitalOut trigger(TRIGGER_PIN);
RenBuggy 0:fbceffb594b6 22 DigitalIn echo_left(LEFT_ECHO_PIN, PullNone);
RenBuggy 0:fbceffb594b6 23 DigitalIn echo_right(RIGHT_ECHO_PIN, PullNone);
RenBuggy 0:fbceffb594b6 24 Timer timer;
RenBuggy 0:fbceffb594b6 25 DigitalOut SR04_nReset(P0_7, 1);
RenBuggy 0:fbceffb594b6 26
RenBuggy 1:80c2ef16a42f 27 /* Trim is an offset that you can adjust to help the buggy drive straight
RenBuggy 1:80c2ef16a42f 28 Trim = -0.3 is a left trim
RenBuggy 1:80c2ef16a42f 29 Trim = 0.3 is a right trim */
RenBuggy 1:80c2ef16a42f 30 float trim = -0.3;
RenBuggy 0:fbceffb594b6 31
RenBuggy 0:fbceffb594b6 32 /* Functions (listed below) contain the code that runs the buggy.
RenBuggy 0:fbceffb594b6 33 These functions can be used in the main.cpp file */
RenBuggy 0:fbceffb594b6 34
RenBuggy 0:fbceffb594b6 35
RenBuggy 0:fbceffb594b6 36 extern void hold(float time) //waits for (time) seconds
RenBuggy 0:fbceffb594b6 37 {
RenBuggy 0:fbceffb594b6 38 for (float i = time;i>0;i-=0.01){ //For every hundreth of a second, display the time remaining
RenBuggy 0:fbceffb594b6 39 wait(0.01);
RenBuggy 0:fbceffb594b6 40 }
RenBuggy 0:fbceffb594b6 41 }
RenBuggy 0:fbceffb594b6 42
RenBuggy 0:fbceffb594b6 43 extern void forward(float time) //moves forward for (time) seconds
RenBuggy 0:fbceffb594b6 44 {
RenBuggy 1:80c2ef16a42f 45 Lmotor = 1.0 + trim;
RenBuggy 1:80c2ef16a42f 46 Rmotor = 1.0 - trim; //set the left and right motor to 1.0 (full speed) - the trim offset
RenBuggy 0:fbceffb594b6 47 hold(time); //wait for (time) seconds while the motors are on
RenBuggy 0:fbceffb594b6 48 stop(); //stops the motors
RenBuggy 0:fbceffb594b6 49 }
RenBuggy 0:fbceffb594b6 50
RenBuggy 0:fbceffb594b6 51
RenBuggy 0:fbceffb594b6 52 extern void left(float time) //moves left for (time) seconds
RenBuggy 0:fbceffb594b6 53 {
RenBuggy 1:80c2ef16a42f 54 Rmotor = 1.0; //set the right motor to full speed
RenBuggy 1:80c2ef16a42f 55 Lmotor = 0.0; //set the left motor to off
RenBuggy 0:fbceffb594b6 56 hold(time); //waits for (time) seconds
RenBuggy 0:fbceffb594b6 57 stop(); //stops the motors
RenBuggy 0:fbceffb594b6 58 }
RenBuggy 0:fbceffb594b6 59
RenBuggy 0:fbceffb594b6 60
RenBuggy 0:fbceffb594b6 61 extern void right(float time) //moves right for (time) seconds
RenBuggy 0:fbceffb594b6 62 {
RenBuggy 1:80c2ef16a42f 63 Lmotor = 1.0; //set the left motor to full speed
RenBuggy 1:80c2ef16a42f 64 Rmotor = 0.0; //set the right motor to off
RenBuggy 0:fbceffb594b6 65 hold(time); //waits for (time) seconds
RenBuggy 0:fbceffb594b6 66 stop(); //stops the motors
RenBuggy 0:fbceffb594b6 67 }
RenBuggy 0:fbceffb594b6 68
RenBuggy 0:fbceffb594b6 69 extern void stop() //stops the motors
RenBuggy 0:fbceffb594b6 70 {
RenBuggy 0:fbceffb594b6 71 Lmotor = Rmotor = 0; //set the speed of each motor to 0
RenBuggy 0:fbceffb594b6 72 }
RenBuggy 0:fbceffb594b6 73
RenBuggy 0:fbceffb594b6 74
RenBuggy 0:fbceffb594b6 75 /*
RenBuggy 0:fbceffb594b6 76 *Function: getDistance_l
RenBuggy 0:fbceffb594b6 77 *
RenBuggy 0:fbceffb594b6 78 *Gets a left distance reading from the ultrasonic sensor
RenBuggy 0:fbceffb594b6 79 *
RenBuggy 1:80c2ef16a42f 80 *Returns: float(distance in metres). Returns 4.0 if no reflection is received.
RenBuggy 0:fbceffb594b6 81 *
RenBuggy 0:fbceffb594b6 82 *WARNING: SPAGHETTI
RenBuggy 1:80c2ef16a42f 83 *
RenBuggy 0:fbceffb594b6 84 */
RenBuggy 0:fbceffb594b6 85 extern float getDistance_l(void)
RenBuggy 0:fbceffb594b6 86 {
RenBuggy 0:fbceffb594b6 87 float time = 0;
RenBuggy 0:fbceffb594b6 88 float distance_m = 0;
RenBuggy 0:fbceffb594b6 89
RenBuggy 0:fbceffb594b6 90 trigger:
RenBuggy 1:80c2ef16a42f 91 timer.stop();
RenBuggy 1:80c2ef16a42f 92 timer.reset();
RenBuggy 0:fbceffb594b6 93 //send trigger signal to ultrasonic
RenBuggy 0:fbceffb594b6 94 trigger = 1; //set trigger pin high
RenBuggy 0:fbceffb594b6 95 wait_us(10); //wait 10 us
RenBuggy 0:fbceffb594b6 96 trigger = 0; //set trigger pin low
RenBuggy 1:80c2ef16a42f 97 timer.start();
RenBuggy 0:fbceffb594b6 98
RenBuggy 0:fbceffb594b6 99 check_echo_high:
RenBuggy 0:fbceffb594b6 100 if(echo_left == 1){ //echo pin goes high
RenBuggy 1:80c2ef16a42f 101 timer.stop();
RenBuggy 1:80c2ef16a42f 102 timer.reset();
RenBuggy 0:fbceffb594b6 103 timer.start(); //start the timer
RenBuggy 0:fbceffb594b6 104 check_echo_low:
RenBuggy 0:fbceffb594b6 105 if(echo_left == 0){ //echo pin goes low
RenBuggy 0:fbceffb594b6 106 timer.stop(); //stop the timer
RenBuggy 0:fbceffb594b6 107 time = timer.read(); //read the timer value
RenBuggy 0:fbceffb594b6 108 timer.reset(); //reset the timer
RenBuggy 0:fbceffb594b6 109 goto echo_pulse_complete; //go to label echo_pulse_complete
RenBuggy 0:fbceffb594b6 110 }
RenBuggy 0:fbceffb594b6 111 else{
RenBuggy 0:fbceffb594b6 112 if(timer.read_ms() > 30 ){ //if the timer reaches 30ms then echo is stuck high, so reset the SR04
RenBuggy 1:80c2ef16a42f 113
RenBuggy 0:fbceffb594b6 114 resetSR04();
RenBuggy 1:80c2ef16a42f 115
RenBuggy 1:80c2ef16a42f 116 distance_m = 4.0;
RenBuggy 1:80c2ef16a42f 117 goto returnvalue;
RenBuggy 1:80c2ef16a42f 118
RenBuggy 1:80c2ef16a42f 119
RenBuggy 0:fbceffb594b6 120 }
RenBuggy 0:fbceffb594b6 121 else{
RenBuggy 0:fbceffb594b6 122 goto check_echo_low; //go to inner loop so that RenBED keeps checking echo pin for low state
RenBuggy 0:fbceffb594b6 123 }
RenBuggy 0:fbceffb594b6 124 }
RenBuggy 0:fbceffb594b6 125 }
RenBuggy 0:fbceffb594b6 126 else{
RenBuggy 1:80c2ef16a42f 127 if(timer.read_ms() > 20){
RenBuggy 1:80c2ef16a42f 128 goto trigger; //resend trigger if module unresponsive
RenBuggy 1:80c2ef16a42f 129 }
RenBuggy 0:fbceffb594b6 130 goto check_echo_high; //go to loop so that RenBED keeps checking echo pin for high state
RenBuggy 1:80c2ef16a42f 131
RenBuggy 0:fbceffb594b6 132 }
RenBuggy 0:fbceffb594b6 133 echo_pulse_complete:
RenBuggy 0:fbceffb594b6 134 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.
RenBuggy 0:fbceffb594b6 135
RenBuggy 1:80c2ef16a42f 136 if(distance_m < 0.02){
RenBuggy 1:80c2ef16a42f 137 goto trigger;
RenBuggy 1:80c2ef16a42f 138 }
RenBuggy 1:80c2ef16a42f 139
RenBuggy 1:80c2ef16a42f 140 returnvalue:
RenBuggy 0:fbceffb594b6 141 return distance_m; //return the distance in metres
RenBuggy 0:fbceffb594b6 142 }
RenBuggy 0:fbceffb594b6 143
RenBuggy 0:fbceffb594b6 144 /*
RenBuggy 0:fbceffb594b6 145 *Function: getDistance_r
RenBuggy 0:fbceffb594b6 146 *
RenBuggy 0:fbceffb594b6 147 *Gets a right distance reading from the ultrasonic sensor
RenBuggy 0:fbceffb594b6 148 *
RenBuggy 1:80c2ef16a42f 149 *Returns: float(distance in metres). Returns 4.0 if no reflection is received.
RenBuggy 1:80c2ef16a42f 150 *
RenBuggy 1:80c2ef16a42f 151 *WARNING: SPAGHETTI
RenBuggy 1:80c2ef16a42f 152 *
RenBuggy 0:fbceffb594b6 153 */
RenBuggy 0:fbceffb594b6 154 float getDistance_r(void)
RenBuggy 0:fbceffb594b6 155 {
RenBuggy 0:fbceffb594b6 156 float time = 0;
RenBuggy 0:fbceffb594b6 157 float distance_m = 0;
RenBuggy 0:fbceffb594b6 158
RenBuggy 0:fbceffb594b6 159 trigger:
RenBuggy 1:80c2ef16a42f 160 timer.stop();
RenBuggy 1:80c2ef16a42f 161 timer.reset();
RenBuggy 0:fbceffb594b6 162 //send trigger signal to ultrasonic
RenBuggy 0:fbceffb594b6 163 trigger = 1; //set trigger pin high
RenBuggy 0:fbceffb594b6 164 wait_us(10); //wait 10 us
RenBuggy 0:fbceffb594b6 165 trigger = 0; //set trigger pin low
RenBuggy 1:80c2ef16a42f 166 timer.start(); //start the timer
RenBuggy 0:fbceffb594b6 167
RenBuggy 0:fbceffb594b6 168 check_echo_high:
RenBuggy 0:fbceffb594b6 169 if(echo_right == 1){ //echo pin goes high
RenBuggy 1:80c2ef16a42f 170 timer.stop();
RenBuggy 1:80c2ef16a42f 171 timer.reset();
RenBuggy 0:fbceffb594b6 172 timer.start(); //start the timer
RenBuggy 0:fbceffb594b6 173 check_echo_low:
RenBuggy 0:fbceffb594b6 174 if(echo_right == 0){ //echo pin goes low
RenBuggy 0:fbceffb594b6 175 timer.stop(); //stop the timer
RenBuggy 0:fbceffb594b6 176 time = timer.read(); //read the timer value
RenBuggy 0:fbceffb594b6 177 timer.reset(); //reset the timer
RenBuggy 0:fbceffb594b6 178 goto echo_pulse_complete; //go to label echo_pulse_complete
RenBuggy 0:fbceffb594b6 179 }
RenBuggy 0:fbceffb594b6 180 else{
RenBuggy 0:fbceffb594b6 181 if(timer.read_ms() > 30 ){ //if the timer reaches 30ms then echo is stuck high, so reset the SR04
RenBuggy 1:80c2ef16a42f 182
RenBuggy 0:fbceffb594b6 183 resetSR04();
RenBuggy 1:80c2ef16a42f 184
RenBuggy 1:80c2ef16a42f 185 distance_m = 4.0;
RenBuggy 1:80c2ef16a42f 186 goto returnvalue;
RenBuggy 0:fbceffb594b6 187 }
RenBuggy 0:fbceffb594b6 188 else{
RenBuggy 0:fbceffb594b6 189 goto check_echo_low; //go to inner loop so that RenBED keeps checking echo pin for low state
RenBuggy 0:fbceffb594b6 190 }
RenBuggy 0:fbceffb594b6 191 }
RenBuggy 0:fbceffb594b6 192 }
RenBuggy 0:fbceffb594b6 193 else{
RenBuggy 1:80c2ef16a42f 194 if(timer.read_ms() > 20){
RenBuggy 1:80c2ef16a42f 195 goto trigger; //resend trigger if module unresponsive
RenBuggy 1:80c2ef16a42f 196 }
RenBuggy 0:fbceffb594b6 197 goto check_echo_high; //go to loop so that RenBED keeps checking echo pin for high state
RenBuggy 1:80c2ef16a42f 198
RenBuggy 0:fbceffb594b6 199 }
RenBuggy 0:fbceffb594b6 200 echo_pulse_complete:
RenBuggy 0:fbceffb594b6 201 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.
RenBuggy 0:fbceffb594b6 202
RenBuggy 1:80c2ef16a42f 203 if(distance_m < 0.02){
RenBuggy 1:80c2ef16a42f 204 goto trigger;
RenBuggy 1:80c2ef16a42f 205 }
RenBuggy 1:80c2ef16a42f 206
RenBuggy 1:80c2ef16a42f 207 returnvalue:
RenBuggy 0:fbceffb594b6 208 return distance_m; //return the distance in metres
RenBuggy 0:fbceffb594b6 209 }
RenBuggy 0:fbceffb594b6 210
RenBuggy 0:fbceffb594b6 211 void resetSR04(void)
RenBuggy 0:fbceffb594b6 212 {
RenBuggy 1:80c2ef16a42f 213 SR04_nReset = 0; //turn off modules
RenBuggy 1:80c2ef16a42f 214 wait_ms(10); //wait for a while
RenBuggy 0:fbceffb594b6 215 timer.stop();
RenBuggy 0:fbceffb594b6 216 timer.reset();
RenBuggy 1:80c2ef16a42f 217 SR04_nReset = 1; //turn modules back on
RenBuggy 1:80c2ef16a42f 218 }
RenBuggy 0:fbceffb594b6 219
RenBuggy 0:fbceffb594b6 220 #endif // BUGGY_FUNCTIONS_C