Fix loss of steering in path_follow template

donkeycar/parts/actuator.py

@@ -10,6 +10,17 @@
 from typing import Tuple
 
 import donkeycar as dk
+from donkeycar import utils
+from donkeycar.utils import clamp
+
+logger = logging.getLogger(__name__)
+
+try:
+    import RPi.GPIO as GPIO
+except ImportError as e:
+    logger.warn(f"RPi.GPIO was not imported. {e}")
+    globals()["GPIO"] = None
+
 from donkeycar.parts.pins import OutputPin, PwmPin, PinState
 from donkeycar.utilities.deprecated import deprecated
 
@@ -18,26 +29,26 @@
 
 #
 # pwm/duty-cycle/pulse
-# - Standard RC servo pulses range from 1 millisecond (full reverse) 
+# - Standard RC servo pulses range from 1 millisecond (full reverse)
 #   to 2 milliseconds (full forward) with 1.5 milliseconds being neutral (stopped).
 # - These pulses are typically send at 50 hertz (every 20 milliseconds).
-# - This means that, using the standard 50hz frequency, a 1 ms pulse 
+# - This means that, using the standard 50hz frequency, a 1 ms pulse
 #   represents a 5% duty cycle and a 2 ms pulse represents a 10% duty cycle.
-# - The important part is the length of the pulse; 
-#   it must be in the range of 1 ms to 2ms.  
-# - So this means that if a different frequency is used, then the duty cycle 
+# - The important part is the length of the pulse;
+#   it must be in the range of 1 ms to 2ms.
+# - So this means that if a different frequency is used, then the duty cycle
 #   must be adjusted in order to get the 1ms to 2ms pulse.
 # - For instance, if a 60hz frequency is used, then a 1 ms pulse requires
 #   a duty cycle of 0.05 * 60 / 50 = 0.06 (6%) duty cycle
-# - We default the frequency of our PCA9685 to 60 hz, so pulses in 
+# - We default the frequency of our PCA9685 to 60 hz, so pulses in
 #   config are generally based on 60hz frequency and 12 bit values.
 #   We use 12 bit values because the PCA9685 has 12 bit resolution.
 #   So a 1 ms pulse is 0.06 * 4096 ~= 246, a neutral pulse of 0.09 duty cycle
 #   is 0.09 * 4096 ~= 367 and full forward pulse of 0.12 duty cycles
 #   is 0.12 * 4096 ~= 492
 # - These are generalizations that are useful for understanding the underlying
-#   api call arguments.  The final choice of duty-cycle/pulse length depends 
-#   on your hardware and perhaps your strategy (you may not want to go too fast, 
+#   api call arguments.  The final choice of duty-cycle/pulse length depends
+#   on your hardware and perhaps your strategy (you may not want to go too fast,
 #   and so you may choose is low max throttle pwm)
 #
 
@@ -57,7 +68,7 @@ def duty_cycle(pulse_ms:float, frequency_hz:float) -> float:
 
 def pulse_ms(pulse_bits:int) -> float:
     """
-    Calculate pulse width in milliseconds given a 
+    Calculate pulse width in milliseconds given a
     12bit pulse (as a PCA9685 would use).
     Donkeycar throttle and steering PWM values are
     based on PCA9685 12bit pulse values, where
@@ -95,7 +106,8 @@ def set_pulse(self, pulse:int) -> None:
         :param pulse:int 12bit integer (0..4095)
         """
         if pulse < 0 or pulse > 4095:
-            raise ValueError("pulse must be in range 0 to 4095")
+            logging.error("pulse must be in range 0 to 4095")
+            pulse = clamp(pulse, 0, 4095)
 
         if not self.started:
             self.pwm_pin.start()
@@ -144,7 +156,9 @@ def set_low(self):
 
     def set_duty_cycle(self, duty_cycle):
         if duty_cycle < 0 or duty_cycle > 1:
-            raise ValueError("duty_cycle must be in range 0 to 1")
+            logging.error("duty_cycle must be in range 0 to 1")
+            duty_cycle = clamp(duty_cycle, 0, 1)
+
         if duty_cycle == 1:
             self.set_high()
         elif duty_cycle == 0:
@@ -166,6 +180,7 @@ def set_pulse(self, pulse):
     def run(self, pulse):
         self.set_pulse(pulse)
 
+
 class VESC:
     ''' 
     VESC Motor controler using pyvesc
@@ -238,7 +253,7 @@ class PiGPIO_PWM():
     # If you use a control circuit that inverts the steering signal, set inverted to True
     # Default multipler for pulses from config etc is 100
     #
-    #   
+    #
     '''
 
     def __init__(self, pin, pgio=None, freq=75, inverted=False):
@@ -293,6 +308,7 @@ def update(self):
 
     def run_threaded(self, angle):
         # map absolute angle to angle that vehicle can implement.
+        angle = utils.clamp(angle, self.LEFT_ANGLE, self.RIGHT_ANGLE)
         self.pulse = dk.utils.map_range(angle,
                                         self.LEFT_ANGLE, self.RIGHT_ANGLE,
                                         self.left_pulse, self.right_pulse)
@@ -346,6 +362,7 @@ def update(self):
             self.controller.set_pulse(self.pulse)
 
     def run_threaded(self, throttle):
+        throttle = utils.clamp(throttle, self.MIN_THROTTLE, self.MAX_THROTTLE)
         if throttle > 0:
             self.pulse = dk.utils.map_range(throttle, 0, self.MAX_THROTTLE,
                                             self.zero_pulse, self.max_pulse)
@@ -368,15 +385,15 @@ def shutdown(self):
 # why don't we just use that code?
 # - this is not used in any templates
 # - this is not documented in docs or elsewhere
-# - this seems redundant; if it emultates a PCA9685 then we should be able to use that code. 
-# - there is an intention to implement a Firmata driver in pins.py.  
-#   Teensy can run Firmata, so that is a way to get Teensy support. 
+# - this seems redundant; if it emultates a PCA9685 then we should be able to use that code.
+# - there is an intention to implement a Firmata driver in pins.py.
+#   Teensy can run Firmata, so that is a way to get Teensy support.
 #   See https://www.pjrc.com/teensy/td_libs_Firmata.html
 #
 @deprecated("JHat is unsupported/undocumented in the framework.  It will be removed in a future release.")
 class JHat:
     ''' 
-    PWM motor controller using Teensy emulating PCA9685. 
+    PWM motor controller using Teensy emulating PCA9685.
     '''
     def __init__(self, channel, address=0x40, frequency=60, busnum=None):
         logger.info("Firing up the Hat")
@@ -414,7 +431,7 @@ def run(self, pulse):
 # - it is not documented in docs or elsewhere
 # This appears to be a way to read RC receiving input.  As such
 # it would more appropriately be integrated into controllers.py.
-# If that can be addressed then we would keep this, 
+# If that can be addressed then we would keep this,
 # otherwise this should be removed.
 #
 @deprecated("JHatReader is unsupported/undocumented in the framework.  It may be removed in a future release.")
@@ -477,7 +494,7 @@ def shutdown(self):
 # - it is not integrated into any templates
 # - It is not documented in the docs or otherwise
 # A path forward would be to add a drive train option
-# DRIVE_TRAIN_TYPE = "DC_TWO_WHEEL_ADAFRUIT" 
+# DRIVE_TRAIN_TYPE = "DC_TWO_WHEEL_ADAFRUIT"
 # and integrate this into complete.py
 #
 @deprecated("This appears to be unsupported/undocumented in the framework. This may be removed in a future release")
@@ -734,9 +751,9 @@ def shutdown(self):
 
 class L298N_HBridge_3pin(object):
     """
-    Motor controlled with an L298N hbridge, 
+    Motor controlled with an L298N hbridge,
     chosen with configuration DRIVETRAIN_TYPE=DC_TWO_WHEEL_L298N
-    Uses two OutputPins to select direction and 
+    Uses two OutputPins to select direction and
     a PwmPin to control the power to the motor.
     See pins.py for pin provider implementations.
 
@@ -748,18 +765,18 @@ class L298N_HBridge_3pin(object):
 
     def __init__(self, pin_forward:OutputPin, pin_backward:OutputPin, pwm_pin:PwmPin, zero_throttle:float=0, max_duty=0.9):
         """
-        :param pin_forward:OutputPin when HIGH the motor will turn clockwise 
+        :param pin_forward:OutputPin when HIGH the motor will turn clockwise
                         using the output of the pwm_pin as a duty_cycle
         :param pin_backward:OutputPin when HIGH the motor will turn counter-clockwise
                             using the output of the pwm_pin as a duty_cycle
-        :param pwm_pin:PwmPin takes a duty cycle in the range of 0 to 1, 
+        :param pwm_pin:PwmPin takes a duty cycle in the range of 0 to 1,
                     where 0 is fully off and 1 is fully on.
         :param zero_throttle: values at or below zero_throttle are treated as zero.
         :param max_duty: the maximum duty cycle that will be send to the motors
 
         NOTE: if pin_forward and pin_backward are both LOW, then the motor is
             'detached' and will glide to a stop.
-            if pin_forward and pin_backward are both HIGH, then the motor 
+            if pin_forward and pin_backward are both HIGH, then the motor
             will be forcibly stopped (can be used for braking)
         """
         self.pin_forward = pin_forward
@@ -774,12 +791,16 @@ def __init__(self, pin_forward:OutputPin, pin_backward:OutputPin, pwm_pin:PwmPin
 
     def run(self, throttle:float) -> None:
         """
-        Update the speed of the motor 
+        Update the speed of the motor
         :param throttle:float throttle value in range -1 to 1,
                         where 1 is full forward and -1 is full backwards.
         """
+        if throttle is None:
+            logger.warn("TwoWheelSteeringThrottle throttle is None")
+            return
         if throttle > 1 or throttle < -1:
-            raise ValueError( "Speed must be between 1(forward) and -1(reverse)")
+            logger.warn( f"TwoWheelSteeringThrottle throttle is {throttle}, but it must be between 1(forward) and -1(reverse)")
+            throttle = clamp(throttle, -1, 1)
 
         self.speed = throttle
         self.throttle = dk.utils.map_range_float(throttle, -1, 1, -self.max_duty, self.max_duty)
@@ -817,18 +838,22 @@ def run(self, throttle:float, steering:float) -> Tuple[float, float]:
         :return: tuple of left motor and right motor throttle values in range -1 to 1
                  where 1 is full forward and -1 is full backwards.
         """
+        if throttle is None:
+            logger.warn("TwoWheelSteeringThrottle throttle is None")
+            return
+        if steering is None:
+            logger.warn("TwoWheelSteeringThrottle steering is None")
+            return
         if throttle > 1 or throttle < -1:
-            logger.warn( f"throttle is {throttle}, but it must be between 1(forward) and -1(reverse)")
+            logger.warn( f"TwoWheelSteeringThrottle throttle is {throttle}, but it must be between 1(forward) and -1(reverse)")
+            throttle = clamp(throttle, -1, 1)
         if steering > 1 or steering < -1:
-            logger.warn( f"steering is {steering}, but it must be between 1(right) and -1(left)")
-
-        from donkeycar.utils import clamp
-        throttle = clamp(throttle, -1, 1)
-        steering = clamp(steering, -1, 1)
+            logger.warn( f"TwoWheelSteeringThrottle steering is {steering}, but it must be between 1(right) and -1(left)")
+            steering = clamp(steering, -1, 1)
 
         left_motor_speed = throttle
         right_motor_speed = throttle
- 
+
         if steering < 0:
             left_motor_speed *= (1.0 - (-steering))
         elif steering > 0:
@@ -848,28 +873,28 @@ class L298N_HBridge_2pin(object):
     See pins.py for pin provider implementations.
 
     See https://www.instructables.com/Tutorial-for-Dual-Channel-DC-Motor-Driver-Board-PW/
-    for how an L298N mini-hbridge modules is wired.  
-    This driver can also be used for an L9110S/HG7881 motor driver.  See 
+    for how an L298N mini-hbridge modules is wired.
+    This driver can also be used for an L9110S/HG7881 motor driver.  See
     https://electropeak.com/learn/interfacing-l9110s-dual-channel-h-bridge-motor-driver-module-with-arduino/
     for how an L9110S motor driver module is wired.
     """
 
     def __init__(self, pin_forward:PwmPin, pin_backward:PwmPin, zero_throttle:float=0, max_duty = 0.9):
         """
-        pin_forward:PwmPin Takes a duty cycle in the range of 0 to 1, 
+        pin_forward:PwmPin Takes a duty cycle in the range of 0 to 1,
                         where 0 is fully off and 1 is fully on.
-                        When the duty_cycle > 0 the motor will turn clockwise 
+                        When the duty_cycle > 0 the motor will turn clockwise
                         proportial to the duty_cycle
-        pin_backward:PwmPin Takes a duty cycle in the range of 0 to 1, 
+        pin_backward:PwmPin Takes a duty cycle in the range of 0 to 1,
                             where 0 is fully off and 1 is fully on.
-                            When the duty_cycle > 0 the motor will turn counter-clockwise 
+                            When the duty_cycle > 0 the motor will turn counter-clockwise
                             proportial to the duty_cycle
         zero_throttle: values at or below zero_throttle are treated as zero.
         max_duty: the maximum duty cycle that will be send to the motors
 
-        NOTE: if pin_forward and pin_backward are both at duty_cycle == 0, 
+        NOTE: if pin_forward and pin_backward are both at duty_cycle == 0,
             then the motor is 'detached' and will glide to a stop.
-            if pin_forward and pin_backward are both at duty_cycle == 1, 
+            if pin_forward and pin_backward are both at duty_cycle == 1,
             then the motor will be forcibly stopped (can be used for braking)
         max_duty is from 0 to 1 (fully off to fully on). I've read 0.9 is a good max.
         """
@@ -891,11 +916,12 @@ def run(self, throttle:float) -> None:
                         where 1 is full forward and -1 is full backwards.
         """
         if throttle is None:
+            logger.warn("TwoWheelSteeringThrottle throttle is None")
             return
-
         if throttle > 1 or throttle < -1:
-            raise ValueError( "Throttle must be between 1(forward) and -1(reverse)")
-
+            logger.warn( f"TwoWheelSteeringThrottle throttle is {throttle}, but it must be between 1(forward) and -1(reverse)")
+            throttle = clamp(throttle, -1, 1)
+
         self.speed = throttle
         self.throttle = dk.utils.map_range_float(throttle, -1, 1, -self.max_duty, self.max_duty)
 
@@ -915,19 +941,18 @@ def shutdown(self):
 
 
 #
-# This is being replaced by pins.py and PulseController.  
-# GPIO pins can be configured using RPi.GPIO or PIGPIO, 
+# This is being replaced by pins.py and PulseController.
+# GPIO pins can be configured using RPi.GPIO or PIGPIO,
 # so this is redundant
 #
 @deprecated("This will be removed in a future release in favor of PulseController")
 class RPi_GPIO_Servo(object):
     '''
     Servo controlled from the gpio pins on Rpi
     '''
-    def __init__(self, pin, freq = 50, min=5.0, max=7.8):
-        import RPi.GPIO as GPIO
+    def __init__(self, pin, pin_scheme=None, freq = 50, min=5.0, max=7.8):
         self.pin = pin
-        GPIO.setmode(GPIO.BOARD)
+        GPIO.setmode(GPIO.BCM if pin_scheme is None else pin_scheme)
         GPIO.setup(self.pin, GPIO.OUT)
 
         self.pwm = GPIO.PWM(self.pin, freq)
@@ -936,7 +961,6 @@ def __init__(self, pin, freq = 50, min=5.0, max=7.8):
         self.max = max
 
     def run(self, pulse):
-        import RPi.GPIO as GPIO
         '''
         Update the speed of the motor where 1 is full forward and
         -1 is full backwards.
@@ -948,13 +972,12 @@ def run(self, pulse):
 
 
     def shutdown(self):
-        import RPi.GPIO as GPIO
         self.pwm.stop()
         GPIO.cleanup()
 
 
 #
-# This is being replaced by pins.py.  GPIO pins can be 
+# This is being replaced by pins.py.  GPIO pins can be
 # configured using RPi.GPIO or PIGPIO, so ServoBlaster is redundant
 #
 @deprecated("This will be removed in a future release in favor of PulseController")
@@ -999,7 +1022,7 @@ def shutdown(self):
 #
 # TODO: integrate ArduinoFirmata support into pin providers, then we can remove all of this code and use PulseController
 #
-# Arduino/Microcontroller PWM support. 
+# Arduino/Microcontroller PWM support.
 # Firmata is a specification for configuring general purpose microcontrollers remotey.
 # The implementatino for Arduino is used here.
 #

donkeycar/tests/test_actuator.py

@@ -11,4 +11,4 @@ def test_PCA9685():
 @pytest.mark.skipif(on_pi() == False, reason='Not on RPi')
 def test_PWMSteering():
     c = PCA9685(0)
-    s = PWMSteering(c)
+    s = PWMSteering(c, 300, 440)

donkeycar/utils.py

@@ -249,6 +249,9 @@ def zip_dir(dir_path, zip_path):
 
 
 def clamp(n, min, max):
+    if min > max:
+        return clamp(n, max, min)
+
     if n < min:
         return min
     if n > max:
@@ -372,10 +375,36 @@ def throttle(input_value):
 OTHER
 '''
 
+def is_number_type(i):
+    return type(i) == int or type(i) == float;
+
+
+def sign(x):
+    if x > 0:
+        return 1
+    if x < 0:
+        return -1
+    return 0
+
+
+def compare_to(
+    value:float,      # IN : value to compare
+    toValue:float,    # IN : value to compare with tolerance
+    tolerance:float): # IN : non-negative tolerance
+                      # RET: 1 if value > toValue + tolerance
+                      #      -1 if value < toValue - tolerance
+                      #      otherwise zero
+    if (toValue - value) > tolerance:
+        return -1
+    if (value - toValue) > tolerance:
+        return 1
+    return 0
+
 
 def map_frange(x, X_min, X_max, Y_min, Y_max):
     '''
     Linear mapping between two ranges of values
+    map from x range to y range
     '''
     X_range = X_max - X_min
     Y_range = Y_max - Y_min