Merge pull request #68 from Open-STEM/rangefinder-fix

fix rangefinder returning zeros

Author: AnselChang

XRPLib/differential_drive.py

@@ -278,7 +278,7 @@ def turn(self, turn_degrees: float, max_effort: float = 0.5, timeout: float = No
                 turn_error = turn_degrees - ((right_delta-left_delta)/2)*360/(self.track_width*math.pi)
 
             # Pass the turn error to the main controller to get a turn speed
-            turn_speed = main_controller.update(turn_error, True)
+            turn_speed = main_controller.update(turn_error)
 
             # exit if timeout or tolerance reached
             if main_controller.is_done() or time_out.is_done():

XRPLib/rangefinder.py

@@ -61,7 +61,7 @@ def distance(self) -> float:
         """
         Get the distance in centimeters by measuring the echo pulse time
         """
-        if time.ticks_diff(time.ticks_us(), self.last_echo_time) < self.cache_time_us and not self.cms == 65535:
+        if time.ticks_diff(time.ticks_us(), self.last_echo_time) < self.cache_time_us and not (self.cms == 65535 or self.cms == 0):
             return self.cms
 
         try:

mainprogram.py

@@ -1,21 +1,3 @@
 
 
 from XRPLib.encoded_motor import EncodedMotor
-import time
-from XRPLib.differential_drive import DifferentialDrive
-
-differentialDrive = DifferentialDrive.get_default_differential_drive()
-
-def turn_test(angle, effort):
-    for i in range(2):
-        differentialDrive.turn(angle, effort)
-        differentialDrive.turn(-angle, effort)
-    differentialDrive.stop()
-
-
-turn_test(30, 0.5)
-turn_test(180, 0.5)
-turn_test(30, 0.75)
-turn_test(180, 0.75)
-turn_test(30, 1)
-turn_test(180, 1)