220 lines
7.4 KiB
Python
220 lines
7.4 KiB
Python
"""
|
|
actuators.py
|
|
Classes to control the motors and servos. These classes
|
|
are wrapped in a mixer class before being used in the drive loop.
|
|
"""
|
|
|
|
import logging
|
|
import time
|
|
|
|
import donkeycar as dk
|
|
from donkeycar.parts.pins import PwmPin, PinState
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
|
|
#
|
|
# pwm/duty-cycle/pulse
|
|
# - 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
|
|
# 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
|
|
# 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
|
|
# 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,
|
|
# and so you may choose is low max throttle pwm)
|
|
#
|
|
|
|
def duty_cycle(pulse_ms: float, frequency_hz: float) -> float:
|
|
"""
|
|
Calculate the duty cycle, 0 to 1, of a pulse given
|
|
the frequency and the pulse length
|
|
|
|
:param pulse_ms:float the desired pulse length in milliseconds
|
|
:param frequency_hz:float the pwm frequency in hertz
|
|
:return:float duty cycle in range 0 to 1
|
|
"""
|
|
ms_per_cycle = 1000 / frequency_hz
|
|
duty = pulse_ms / ms_per_cycle
|
|
return duty
|
|
|
|
|
|
def pulse_ms(pulse_bits: int) -> float:
|
|
"""
|
|
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
|
|
0 is zero duty cycle and 4095 is 100% duty cycle.
|
|
|
|
:param pulse_bits:int 12bit integer in range 0 to 4095
|
|
:return:float pulse length in milliseconds
|
|
"""
|
|
if pulse_bits < 0 or pulse_bits > 4095:
|
|
raise ValueError("pulse_bits must be in range 0 to 4095 (12bit integer)")
|
|
return pulse_bits / 4095
|
|
|
|
|
|
class PulseController:
|
|
"""
|
|
Controller that provides a servo PWM pulse using the given PwmPin
|
|
See pins.py for pin provider implementations.
|
|
"""
|
|
|
|
def __init__(self, pwm_pin: PwmPin, pwm_scale: float = 1.0, pwm_inverted: bool = False) -> None:
|
|
"""
|
|
:param pwm_pin:PwnPin pin that will emit the pulse.
|
|
:param pwm_scale:float scaling the 12 bit pulse value to compensate
|
|
for non-standard pwm frequencies.
|
|
:param pwm_inverted:bool True to invert the duty cycle
|
|
"""
|
|
self.pwm_pin = pwm_pin
|
|
self.scale = pwm_scale
|
|
self.inverted = pwm_inverted
|
|
self.started = pwm_pin.state() != PinState.NOT_STARTED
|
|
|
|
def set_pulse(self, pulse: int) -> None:
|
|
"""
|
|
Set the length of the pulse using a 12 bit integer (0..4095)
|
|
:param pulse:int 12bit integer (0..4095)
|
|
"""
|
|
if pulse < 0 or pulse > 4095:
|
|
raise ValueError("pulse must be in range 0 to 4095")
|
|
|
|
if not self.started:
|
|
self.pwm_pin.start()
|
|
self.started = True
|
|
if self.inverted:
|
|
pulse = 4095 - pulse
|
|
self.pwm_pin.duty_cycle(int(pulse * self.scale) / 4095)
|
|
|
|
def run(self, pulse: int) -> None:
|
|
"""
|
|
Set the length of the pulse using a 12 bit integer (0..4095)
|
|
:param pulse:int 12bit integer (0..4095)
|
|
"""
|
|
self.set_pulse(pulse)
|
|
|
|
|
|
class PWMSteering:
|
|
"""
|
|
Wrapper over a PWM pulse controller to convert angles to PWM pulses.
|
|
"""
|
|
LEFT_ANGLE = -1
|
|
RIGHT_ANGLE = 1
|
|
|
|
def __init__(self, controller, left_pulse, right_pulse):
|
|
|
|
if controller is None:
|
|
raise ValueError("PWMSteering requires a set_pulse controller to be passed")
|
|
set_pulse = getattr(controller, "set_pulse", None)
|
|
if set_pulse is None or not callable(set_pulse):
|
|
raise ValueError("controller must have a set_pulse method")
|
|
|
|
self.controller = controller
|
|
self.left_pulse = left_pulse
|
|
self.right_pulse = right_pulse
|
|
self.pulse = dk.utils.map_range(0, self.LEFT_ANGLE, self.RIGHT_ANGLE,
|
|
self.left_pulse, self.right_pulse)
|
|
self.running = True
|
|
logger.info('PWM Steering created')
|
|
|
|
def update(self):
|
|
while self.running:
|
|
self.controller.set_pulse(self.pulse)
|
|
|
|
def run_threaded(self, angle):
|
|
# map absolute angle to angle that vehicle can implement.
|
|
self.pulse = dk.utils.map_range(angle,
|
|
self.LEFT_ANGLE, self.RIGHT_ANGLE,
|
|
self.left_pulse, self.right_pulse)
|
|
|
|
def run(self, angle):
|
|
self.run_threaded(angle)
|
|
self.controller.set_pulse(self.pulse)
|
|
|
|
def shutdown(self):
|
|
# set steering straight
|
|
self.pulse = 0
|
|
time.sleep(0.3)
|
|
self.running = False
|
|
|
|
|
|
class PWMThrottle:
|
|
"""
|
|
Wrapper over a PWM pulse controller to convert -1 to 1 throttle
|
|
values to PWM pulses.
|
|
"""
|
|
MIN_THROTTLE = -1
|
|
MAX_THROTTLE = 1
|
|
|
|
def __init__(self, controller, max_pulse, min_pulse, zero_pulse):
|
|
|
|
if controller is None:
|
|
raise ValueError("PWMThrottle requires a set_pulse controller to be passed")
|
|
set_pulse = getattr(controller, "set_pulse", None)
|
|
if set_pulse is None or not callable(set_pulse):
|
|
raise ValueError("controller must have a set_pulse method")
|
|
|
|
self.controller = controller
|
|
self.max_pulse = max_pulse
|
|
self.min_pulse = min_pulse
|
|
self.zero_pulse = zero_pulse
|
|
self.pulse = zero_pulse
|
|
|
|
# send zero pulse to calibrate ESC
|
|
logger.info("Init ESC")
|
|
self.controller.set_pulse(self.max_pulse)
|
|
time.sleep(0.01)
|
|
self.controller.set_pulse(self.min_pulse)
|
|
time.sleep(0.01)
|
|
self.controller.set_pulse(self.zero_pulse)
|
|
time.sleep(1)
|
|
self.running = True
|
|
logger.info('PWM Throttle created')
|
|
|
|
def update(self):
|
|
while self.running:
|
|
self.controller.set_pulse(self.pulse)
|
|
|
|
def run_threaded(self, throttle):
|
|
if throttle > 0:
|
|
self.pulse = dk.utils.map_range(throttle, 0, self.MAX_THROTTLE,
|
|
self.zero_pulse, self.max_pulse)
|
|
else:
|
|
self.pulse = dk.utils.map_range(throttle, self.MIN_THROTTLE, 0,
|
|
self.min_pulse, self.zero_pulse)
|
|
|
|
def run(self, throttle):
|
|
self.run_threaded(throttle)
|
|
self.controller.set_pulse(self.pulse)
|
|
|
|
def shutdown(self):
|
|
# stop vehicle
|
|
self.run(0)
|
|
self.running = False
|
|
|
|
|
|
class MockController(object):
|
|
def __init__(self):
|
|
pass
|
|
|
|
def run(self, pulse):
|
|
pass
|
|
|
|
def shutdown(self):
|
|
pass
|