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@0:fbceffb594b6, 2016-07-14 (annotated)

Committer:
RenBuggy
Date:
Thu Jul 14 12:58:48 2016 +0000
Revision:
0:fbceffb594b6
Child:
1:80c2ef16a42f
.

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 0:fbceffb594b6 17 DigitalOut Lmotor(LEFT_MOTOR_PIN, 0);
RenBuggy 0:fbceffb594b6 18 DigitalOut Rmotor(RIGHT_MOTOR_PIN, 0);
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 0:fbceffb594b6 27 //Trim is an offset that you can adjust to help the buggy drive straight
RenBuggy 0:fbceffb594b6 28 //Trim = -0.3 is a left trim
RenBuggy 0:fbceffb594b6 29 //Trim = 0.3 is a right trim
RenBuggy 0:fbceffb594b6 30 //float trim = -0.6;
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 0:fbceffb594b6 45 Lmotor = 1; //+ trim;
RenBuggy 0:fbceffb594b6 46 Rmotor = 1; //- 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 0:fbceffb594b6 54 Rmotor = 1; //set the right motor to full speed
RenBuggy 0:fbceffb594b6 55 Lmotor = 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 0:fbceffb594b6 63 Lmotor = 1; //set the left motor to full speed
RenBuggy 0:fbceffb594b6 64 Rmotor = 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 0:fbceffb594b6 80 *Returns: float(distance in metres). Returns 0 if no object within 4m.
RenBuggy 0:fbceffb594b6 81 *
RenBuggy 0:fbceffb594b6 82 *WARNING: SPAGHETTI
RenBuggy 0:fbceffb594b6 83 */
RenBuggy 0:fbceffb594b6 84 extern float getDistance_l(void)
RenBuggy 0:fbceffb594b6 85 {
RenBuggy 0:fbceffb594b6 86 float time = 0;
RenBuggy 0:fbceffb594b6 87 float distance_m = 0;
RenBuggy 0:fbceffb594b6 88
RenBuggy 0:fbceffb594b6 89 trigger:
RenBuggy 0:fbceffb594b6 90 //send trigger signal to ultrasonic
RenBuggy 0:fbceffb594b6 91 trigger = 1; //set trigger pin high
RenBuggy 0:fbceffb594b6 92 wait_us(10); //wait 10 us
RenBuggy 0:fbceffb594b6 93 trigger = 0; //set trigger pin low
RenBuggy 0:fbceffb594b6 94
RenBuggy 0:fbceffb594b6 95 check_echo_high:
RenBuggy 0:fbceffb594b6 96 if(echo_left == 1){ //echo pin goes high
RenBuggy 0:fbceffb594b6 97 timer.start(); //start the timer
RenBuggy 0:fbceffb594b6 98 check_echo_low:
RenBuggy 0:fbceffb594b6 99 if(echo_left == 0){ //echo pin goes low
RenBuggy 0:fbceffb594b6 100 timer.stop(); //stop the timer
RenBuggy 0:fbceffb594b6 101 time = timer.read(); //read the timer value
RenBuggy 0:fbceffb594b6 102 timer.reset(); //reset the timer
RenBuggy 0:fbceffb594b6 103 goto echo_pulse_complete; //go to label echo_pulse_complete
RenBuggy 0:fbceffb594b6 104 }
RenBuggy 0:fbceffb594b6 105 else{
RenBuggy 0:fbceffb594b6 106 if(timer.read_ms() > 30 ){ //if the timer reaches 30ms then echo is stuck high, so reset the SR04
RenBuggy 0:fbceffb594b6 107 resetSR04();
RenBuggy 0:fbceffb594b6 108 if(echo_left == 0){
RenBuggy 0:fbceffb594b6 109 goto trigger; //when echo goes low again, reattempt measurement
RenBuggy 0:fbceffb594b6 110 }
RenBuggy 0:fbceffb594b6 111 else{
RenBuggy 0:fbceffb594b6 112 wait_ms(1); //otherwise wait for low state
RenBuggy 0:fbceffb594b6 113 }
RenBuggy 0:fbceffb594b6 114 }
RenBuggy 0:fbceffb594b6 115 else{
RenBuggy 0:fbceffb594b6 116 goto check_echo_low; //go to inner loop so that RenBED keeps checking echo pin for low state
RenBuggy 0:fbceffb594b6 117 }
RenBuggy 0:fbceffb594b6 118 }
RenBuggy 0:fbceffb594b6 119 }
RenBuggy 0:fbceffb594b6 120 else{
RenBuggy 0:fbceffb594b6 121 goto check_echo_high; //go to loop so that RenBED keeps checking echo pin for high state
RenBuggy 0:fbceffb594b6 122 }
RenBuggy 0:fbceffb594b6 123 echo_pulse_complete:
RenBuggy 0:fbceffb594b6 124 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 125
RenBuggy 0:fbceffb594b6 126 return distance_m; //return the distance in metres
RenBuggy 0:fbceffb594b6 127 }
RenBuggy 0:fbceffb594b6 128
RenBuggy 0:fbceffb594b6 129 /*
RenBuggy 0:fbceffb594b6 130 *Function: getDistance_r
RenBuggy 0:fbceffb594b6 131 *
RenBuggy 0:fbceffb594b6 132 *Gets a right distance reading from the ultrasonic sensor
RenBuggy 0:fbceffb594b6 133 *
RenBuggy 0:fbceffb594b6 134 *Returns: float(distance in metres). Returns 0 if no object within 4m.
RenBuggy 0:fbceffb594b6 135 */
RenBuggy 0:fbceffb594b6 136 float getDistance_r(void)
RenBuggy 0:fbceffb594b6 137 {
RenBuggy 0:fbceffb594b6 138 float time = 0;
RenBuggy 0:fbceffb594b6 139 float distance_m = 0;
RenBuggy 0:fbceffb594b6 140
RenBuggy 0:fbceffb594b6 141 trigger:
RenBuggy 0:fbceffb594b6 142 //send trigger signal to ultrasonic
RenBuggy 0:fbceffb594b6 143 trigger = 1; //set trigger pin high
RenBuggy 0:fbceffb594b6 144 wait_us(10); //wait 10 us
RenBuggy 0:fbceffb594b6 145 trigger = 0; //set trigger pin low
RenBuggy 0:fbceffb594b6 146
RenBuggy 0:fbceffb594b6 147 check_echo_high:
RenBuggy 0:fbceffb594b6 148 if(echo_right == 1){ //echo pin goes high
RenBuggy 0:fbceffb594b6 149 timer.start(); //start the timer
RenBuggy 0:fbceffb594b6 150 check_echo_low:
RenBuggy 0:fbceffb594b6 151 if(echo_right == 0){ //echo pin goes low
RenBuggy 0:fbceffb594b6 152 timer.stop(); //stop the timer
RenBuggy 0:fbceffb594b6 153 time = timer.read(); //read the timer value
RenBuggy 0:fbceffb594b6 154 timer.reset(); //reset the timer
RenBuggy 0:fbceffb594b6 155 goto echo_pulse_complete; //go to label echo_pulse_complete
RenBuggy 0:fbceffb594b6 156 }
RenBuggy 0:fbceffb594b6 157 else{
RenBuggy 0:fbceffb594b6 158 if(timer.read_ms() > 30 ){ //if the timer reaches 30ms then echo is stuck high, so reset the SR04
RenBuggy 0:fbceffb594b6 159 resetSR04();
RenBuggy 0:fbceffb594b6 160 if(echo_right == 0){
RenBuggy 0:fbceffb594b6 161 goto trigger; //when echo goes low again, reattempt measurement
RenBuggy 0:fbceffb594b6 162 }
RenBuggy 0:fbceffb594b6 163 else{
RenBuggy 0:fbceffb594b6 164 wait_ms(1); //otherwise wait for low state
RenBuggy 0:fbceffb594b6 165 }
RenBuggy 0:fbceffb594b6 166 }
RenBuggy 0:fbceffb594b6 167 else{
RenBuggy 0:fbceffb594b6 168 goto check_echo_low; //go to inner loop so that RenBED keeps checking echo pin for low state
RenBuggy 0:fbceffb594b6 169 }
RenBuggy 0:fbceffb594b6 170 }
RenBuggy 0:fbceffb594b6 171 }
RenBuggy 0:fbceffb594b6 172 else{
RenBuggy 0:fbceffb594b6 173 goto check_echo_high; //go to loop so that RenBED keeps checking echo pin for high state
RenBuggy 0:fbceffb594b6 174 }
RenBuggy 0:fbceffb594b6 175 echo_pulse_complete:
RenBuggy 0:fbceffb594b6 176 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 177
RenBuggy 0:fbceffb594b6 178 return distance_m; //return the distance in metres
RenBuggy 0:fbceffb594b6 179 }
RenBuggy 0:fbceffb594b6 180
RenBuggy 0:fbceffb594b6 181 void resetSR04(void)
RenBuggy 0:fbceffb594b6 182 {
RenBuggy 0:fbceffb594b6 183 SR04_nReset = 0;
RenBuggy 0:fbceffb594b6 184 forward(1);
RenBuggy 0:fbceffb594b6 185 timer.stop();
RenBuggy 0:fbceffb594b6 186 timer.reset();
RenBuggy 0:fbceffb594b6 187 SR04_nReset = 1;
RenBuggy 0:fbceffb594b6 188 }
RenBuggy 0:fbceffb594b6 189
RenBuggy 0:fbceffb594b6 190 #endif // BUGGY_FUNCTIONS_C