Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix loss of steering in path_follow template #1033

Merged
merged 7 commits into from
Aug 9, 2022
Merged

Fix loss of steering in path_follow template #1033

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
4060025
Fix loss of steering in path_follow template
Ezward Aug 4, 2022
5fc5272
remove bad import of differential_steering
Ezward Aug 5, 2022
2c4c188
fix tests/test_actuator.py
Ezward Aug 5, 2022
34c4a95
added back in VESC after it was deleted during a rebase
Ezward Aug 6, 2022
b004be6
fix test failure on Mac/Ubuntu due to missing RPi.GPIO
Ezward Aug 6, 2022
bb7f0cf
version=4.3.21
Ezward Aug 6, 2022
90e8152
Merge branch 'main' into fix-loss-of-steering
DocGarbanzo Aug 8, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
181 changes: 75 additions & 106 deletions donkeycar/parts/actuator.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,18 @@
from typing import Tuple

import donkeycar as dk
from donkeycar import utils
from donkeycar.parts.kinematics import differential_steering
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could it be that you missed to check in that module - this seems to cause the break of 3 tests: test_actuator, test_template, test_web_socket.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for catching this. the import of differential_steering is not necessary; in the branch I am picking from it have been move to the kinematics parts, but not in this branch. I've removed the import.

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

Expand All @@ -18,26 +30,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)
#

Expand All @@ -57,7 +69,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
Expand Down Expand Up @@ -95,7 +107,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()
Expand Down Expand Up @@ -144,7 +157,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:
Expand All @@ -166,60 +181,6 @@ def set_pulse(self, pulse):
def run(self, pulse):
self.set_pulse(pulse)

class VESC:
Copy link
Contributor Author

@Ezward Ezward Aug 6, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I rebased against main. For some reason it shows me deleting the VESC class. My local copy of actuator has VESC in it. It won't let me add it or commit it (Everything is up to date). What is up with that? Any idea? If this get's merged is it going to delete VESC?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I just copied it back in from main to fix this issue.

'''
VESC Motor controler using pyvesc
This is used for most electric scateboards.

inputs: serial_port---- port used communicate with vesc. for linux should be something like /dev/ttyACM1
has_sensor=False------- default value from pyvesc
start_heartbeat=True----default value from pyvesc (I believe this sets up a heartbeat and kills speed if lost)
baudrate=115200--------- baudrate used for communication with VESC
timeout=0.05-------------time it will try before giving up on establishing connection

percent=.2--------------max percentage of the dutycycle that the motor will be set to
outputs: none

uses the pyvesc library to open communication with the VESC and sets the servo to the angle (0-1) and the duty_cycle(speed of the car) to the throttle (mapped so that percentage will be max/min speed)

Note that this depends on pyvesc, but using pip install pyvesc will create a pyvesc file that
can only set the speed, but not set the servo angle.

Instead please use:
pip install git+https://github.com/LiamBindle/PyVESC.git@master
to install the pyvesc library
'''
def __init__(self, serial_port, percent=.2, has_sensor=False, start_heartbeat=True, baudrate=115200, timeout=0.05, steering_scale = 1.0, steering_offset = 0.0 ):

try:
import pyvesc
except Exception as err:
print("\n\n\n\n", err, "\n")
print("please use the following command to import pyvesc so that you can also set")
print("the servo position:")
print("pip install git+https://github.com/LiamBindle/PyVESC.git@master")
print("\n\n\n")
time.sleep(1)
raise

assert percent <= 1 and percent >= -1,'\n\nOnly percentages are allowed for MAX_VESC_SPEED (we recommend a value of about .2) (negative values flip direction of motor)'
self.steering_scale = steering_scale
self.steering_offset = steering_offset
self.percent = percent

try:
self.v = pyvesc.VESC(serial_port, has_sensor, start_heartbeat, baudrate, timeout)
except Exception as err:
print("\n\n\n\n", err)
print("\n\nto fix permission denied errors, try running the following command:")
print("sudo chmod a+rw {}".format(serial_port), "\n\n\n\n")
time.sleep(1)
raise

def run(self, angle, throttle):
self.v.set_servo((angle * self.steering_scale) + self.steering_offset)
self.v.set_duty_cycle(throttle*self.percent)


@deprecated("Deprecated in favor or PulseController. This will be removed in a future release")
class PiGPIO_PWM():
Expand All @@ -238,7 +199,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):
Expand Down Expand Up @@ -293,6 +254,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)
Expand Down Expand Up @@ -346,6 +308,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)
Expand All @@ -368,15 +331,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")
Expand Down Expand Up @@ -414,7 +377,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.")
Expand Down Expand Up @@ -477,7 +440,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")
Expand Down Expand Up @@ -734,9 +697,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.

Expand All @@ -748,18 +711,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
Expand All @@ -774,12 +737,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)
Expand Down Expand Up @@ -817,18 +784,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:
Expand All @@ -848,28 +819,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.
"""
Expand All @@ -891,11 +862,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)

Expand All @@ -915,19 +887,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=GPIO.BCM, freq = 50, min=5.0, max=7.8):
DocGarbanzo marked this conversation as resolved.
Show resolved Hide resolved
self.pin = pin
GPIO.setmode(GPIO.BOARD)
GPIO.setmode(pin_scheme)
GPIO.setup(self.pin, GPIO.OUT)

self.pwm = GPIO.PWM(self.pin, freq)
Expand All @@ -936,7 +907,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.
Expand All @@ -948,13 +918,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")
Expand Down Expand Up @@ -999,7 +968,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.
#
Expand Down
Loading