|
|
|
|
@ -5,7 +5,7 @@ Usage:
|
|
|
|
|
rc-pca9685 [steering|throttle] [-u USERNAME | --mqtt-username=USERNAME] [--mqtt-password=PASSWORD] \
|
|
|
|
|
[--mqtt-broker=HOSTNAME] [--mqtt-topic=TOPIC] [--mqtt-client-id=CLIENT_ID] [--i2c-bus=BUS] \
|
|
|
|
|
[--i2c-address=ADDRESS] [--pca9685-channel=CHANNEL] [--left-pulse=LEFT_PULSE] [--right-pulse=RIGHT_PULSE] \
|
|
|
|
|
[--max-pulse=MAX_PULSE] [--min-pulse=MIN_PULSE] [--zero-pulse=ZEO_PULSE] [--debug]
|
|
|
|
|
[--max-pulse=MAX_PULSE] [--min-pulse=MIN_PULSE] [--zero-pulse=ZEO_PULSE]
|
|
|
|
|
|
|
|
|
|
Options:
|
|
|
|
|
-h --help Show this screen.
|
|
|
|
|
@ -22,11 +22,9 @@ Options:
|
|
|
|
|
-M MAX_PULSE --max-pulse=MAX_PULSE Max Pulse for throttle
|
|
|
|
|
-m MIN_PULSE --min-pulse=MIN_PULSE Min Pulse for throttle
|
|
|
|
|
-z ZERO_PULE --zero-pulse=ZEO_PULSE Zero Pulse for throttle
|
|
|
|
|
-d --debug Debug logging
|
|
|
|
|
"""
|
|
|
|
|
import logging
|
|
|
|
|
import os
|
|
|
|
|
import signal, sys
|
|
|
|
|
|
|
|
|
|
from docopt import docopt
|
|
|
|
|
import paho.mqtt.client as mqtt
|
|
|
|
|
@ -43,22 +41,12 @@ logging.basicConfig(level=logging.INFO)
|
|
|
|
|
default_client_id = "robocar-pca9685-python"
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class SignalHandler:
|
|
|
|
|
def __init__(self, client: mqtt.Client):
|
|
|
|
|
self._client = client
|
|
|
|
|
|
|
|
|
|
def sigterm_handler(self, _signo, _stack_frame):
|
|
|
|
|
logger.info("exit on SIG_TERM")
|
|
|
|
|
self._client.disconnect()
|
|
|
|
|
sys.exit(0)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def init_mqtt_steering_client(steering_controller: actuator.PWMSteering, broker_host: str, user: str, password: str, client_id: str,
|
|
|
|
|
steering_topic: str) -> mqtt.Client:
|
|
|
|
|
logger.info("Start steering part")
|
|
|
|
|
client = mqtt.Client(client_id=client_id, clean_session=True, userdata=None, protocol=mqtt.MQTTv311)
|
|
|
|
|
|
|
|
|
|
def on_connect(client: mqtt.Client, userdata, flags, rc):
|
|
|
|
|
def on_connect(client, userdata, flags, rc):
|
|
|
|
|
logger.info("Register callback on topic %s", steering_topic)
|
|
|
|
|
# Subscribing in on_connect() means that if we lose the connection and
|
|
|
|
|
# reconnect then subscriptions will be renewed.
|
|
|
|
|
@ -68,9 +56,9 @@ def init_mqtt_steering_client(steering_controller: actuator.PWMSteering, broker_
|
|
|
|
|
try:
|
|
|
|
|
steering_msg = SteeringMessage()
|
|
|
|
|
steering_msg.ParseFromString(msg.payload)
|
|
|
|
|
steering_controller.run_threaded(steering_msg.steering)
|
|
|
|
|
steering_controller.run_threaded(steering_msg.Steering)
|
|
|
|
|
except:
|
|
|
|
|
logger.exception("unexpected error: unable to process steering, skip message")
|
|
|
|
|
logger.debug("unexpected error: unable to process steering, skip message")
|
|
|
|
|
|
|
|
|
|
client.username_pw_set(user, password)
|
|
|
|
|
client.on_connect = on_connect
|
|
|
|
|
@ -96,9 +84,9 @@ def init_mqtt_throttle_client(throttle_controller: actuator.PWMThrottle, broker_
|
|
|
|
|
try:
|
|
|
|
|
throttle_msg = ThrottleMessage()
|
|
|
|
|
throttle_msg.ParseFromString(msg.payload)
|
|
|
|
|
throttle_controller.run_threaded(throttle_msg.throttle)
|
|
|
|
|
throttle_controller.run_threaded(throttle_msg.Throttle)
|
|
|
|
|
except:
|
|
|
|
|
logger.exception("unexpected error: unable to process throttle, skip message")
|
|
|
|
|
logger.debug("unexpected error: unable to process throttle, skip message")
|
|
|
|
|
|
|
|
|
|
client.username_pw_set(user, password)
|
|
|
|
|
client.on_connect = on_connect
|
|
|
|
|
@ -123,31 +111,22 @@ def execute_from_command_line():
|
|
|
|
|
i2c_address = get_default_value(args["--i2c-address"], "I2C_ADDRESS", 40)
|
|
|
|
|
pca9685_channel = get_default_value(args["--pca9685-channel"], "PCA9685_CHANNEL", 0)
|
|
|
|
|
|
|
|
|
|
debug = bool(args["--debug"])
|
|
|
|
|
if debug:
|
|
|
|
|
logging.basicConfig(level=logging.DEBUG)
|
|
|
|
|
|
|
|
|
|
pin_id = "PCA9685.{bus}:{address}.{channel}".format(bus=i2c_bus, address=i2c_address,channel=pca9685_channel)
|
|
|
|
|
|
|
|
|
|
if args["steering"]:
|
|
|
|
|
steering_pin = donkeycar.parts.pins.pwm_pin_by_id(pin_id)
|
|
|
|
|
controller = actuator.PulseController(steering_pin)
|
|
|
|
|
steering = actuator.PWMSteering(controller=controller, left_pulse=int(args["--left-pulse"]), right_pulse=int(args["--right-pulse"]))
|
|
|
|
|
steering = actuator.PWMSteering(steering_pin, left_pulse=args["--left-pulse"], right_pulse=args["--right-pulse"])
|
|
|
|
|
client = init_mqtt_steering_client(steering_controller=steering, broker_host=broker_host, user=user,
|
|
|
|
|
password=password, client_id=client_id, steering_topic=topic)
|
|
|
|
|
elif args["throttle"]:
|
|
|
|
|
throttle_pin = donkeycar.parts.pins.pwm_pin_by_id(pin_id)
|
|
|
|
|
controller = actuator.PulseController(throttle_pin)
|
|
|
|
|
throttle = actuator.PWMThrottle(controller=controller, max_pulse=int(args["--max-pulse"]), min_pulse=int(args["--min-pulse"]),
|
|
|
|
|
zero_pulse=int(args["--zero-pulse"]))
|
|
|
|
|
throttle_pin = donkeycar.parts.pins.pwm_pin_by_id("PCA9685.0:40.7")
|
|
|
|
|
throttle = actuator.PWMThrottle(throttle_pin, max_pulse=args["--max-pulse"], min_pulse=args["--min-pulse"],
|
|
|
|
|
zero_pulse=args["--zero-pulse"])
|
|
|
|
|
client = init_mqtt_throttle_client(throttle_controller=throttle, broker_host=broker_host, user=user,
|
|
|
|
|
password=password, client_id=client_id, throttle_topic=topic)
|
|
|
|
|
else:
|
|
|
|
|
raise Exception('invalid mode selected, must be steering or throttle')
|
|
|
|
|
|
|
|
|
|
sh = SignalHandler(client)
|
|
|
|
|
signal.signal(signal.SIGTERM, sh.sigterm_handler)
|
|
|
|
|
|
|
|
|
|
client.loop_forever()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|