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@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?
User | Revision | Line number | New 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 |