Diff: ultrasonic_buggy.cpp

Revision:

1:80c2ef16a42f

Parent:

0:fbceffb594b6

--- 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