Diff: ultrasonic_buggy.cpp

Revision:

0:fbceffb594b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ultrasonic_buggy.cpp	Thu Jul 14 12:58:48 2016 +0000
@@ -0,0 +1,190 @@
+/*********************************************************
+*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
+DigitalOut Lmotor(LEFT_MOTOR_PIN, 0);
+DigitalOut Rmotor(RIGHT_MOTOR_PIN, 0);
+
+// 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.6;
+
+/* 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; //+ trim;
+    Rmotor = 1; //- 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;  //set the right motor to full speed
+    Lmotor = 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;  //set the left motor to full speed
+    Rmotor = 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 0 if no object within 4m.
+*
+*WARNING: SPAGHETTI
+*/
+extern float getDistance_l(void)
+{
+    float time = 0;
+    float distance_m = 0;
+    
+    trigger:
+    //send trigger signal to ultrasonic
+    trigger = 1;        //set trigger pin high
+    wait_us(10);        //wait 10 us
+    trigger = 0;        //set trigger pin low
+    
+    check_echo_high:
+    if(echo_left == 1){          //echo pin goes high
+        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();
+                if(echo_left == 0){
+                    goto trigger;   //when echo goes low again, reattempt measurement
+                }
+                else{
+                    wait_ms(1);     //otherwise wait for low state
+                }
+            }
+            else{
+                goto check_echo_low;    //go to inner loop so that RenBED keeps checking echo pin for low state
+            }
+        }
+    }
+    else{
+        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.
+    
+    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 0 if no object within 4m.
+*/
+float getDistance_r(void)
+{
+    float time = 0;
+    float distance_m = 0;
+    
+    trigger:
+    //send trigger signal to ultrasonic
+    trigger = 1;        //set trigger pin high
+    wait_us(10);        //wait 10 us
+    trigger = 0;        //set trigger pin low
+    
+    check_echo_high:
+    if(echo_right == 1){          //echo pin goes high
+        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();
+                if(echo_right == 0){
+                    goto trigger;   //when echo goes low again, reattempt measurement
+                }
+                else{
+                    wait_ms(1);     //otherwise wait for low state
+                }
+            }
+            else{
+                goto check_echo_low;    //go to inner loop so that RenBED keeps checking echo pin for low state
+            }
+        }
+    }
+    else{
+        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.
+    
+    return distance_m;          //return the distance in metres                     
+}
+
+void resetSR04(void)
+{
+    SR04_nReset = 0;
+    forward(1);
+    timer.stop();
+    timer.reset();
+    SR04_nReset = 1;
+}
+
+#endif // BUGGY_FUNCTIONS_C
\ No newline at end of file