From c97df7bbaa2ea2cd177b8101b58bc7d8a76b8720 Mon Sep 17 00:00:00 2001 From: Piv <18462828+Piv200@users.noreply.github.com> Date: Sat, 12 Sep 2020 15:49:01 +0930 Subject: [PATCH] Add Esp32 (serial) vehicle to pycar --- .vscode/launch.json | 3 +- esp32/README.md | 8 +++- .../src/car/control/gpio/abstract_vehicle.py | 26 ++++++++++++ pycar/src/car/control/gpio/factory.py | 15 +++++-- pycar/src/car/control/gpio/mockvehicle.py | 27 +++--------- .../gpio/recording_vehicle_decorator.py | 21 ++-------- pycar/src/car/control/gpio/serial_vehicle.py | 42 +++++++++++++++++++ pycar/src/car/control/gpio/vehicle.py | 3 +- 8 files changed, 99 insertions(+), 46 deletions(-) create mode 100644 pycar/src/car/control/gpio/abstract_vehicle.py create mode 100644 pycar/src/car/control/gpio/serial_vehicle.py diff --git a/.vscode/launch.json b/.vscode/launch.json index 3704543..eaf57ba 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -10,7 +10,8 @@ "request": "launch", "module": "car", "env": { - "CAR_VEHICLE": "CAR_MOCK", + "CAR_VEHICLE": "VEHICLE_MOCK", + // "CAR_VEHICLE": "VEHICLE_SERIAL", // "CAR_LIDAR": "/dev/tty.usbserial-0001", "CAR_LIDAR": "LIDAR_MOCK" } diff --git a/esp32/README.md b/esp32/README.md index a206c6b..2f54f1b 100644 --- a/esp32/README.md +++ b/esp32/README.md @@ -10,6 +10,8 @@ The protocol specification is as follows, assuming an array of bytes: | 0 | 0 if Calibrating a servo. Higher values indicates channel number to set duty cycle on. | | 1 | If byte 0 = 0: number of servos to calibrate. Else, the new duty cycle value for the channel specified in byte 0. | +When setting the duty cycle, the current min angle is 0, and the max angle is 255, which allows the entire byte to be used for setting the duty cycle. + The table below describes the byte format for calibrating a servo. This array of bytes can be repeated for the given number of servos in byte 1: | Byte Number | Value Description | @@ -19,4 +21,8 @@ The table below describes the byte format for calibrating a servo. This array of The min/max pulse widths are hardcoded to 1000us/2000us respectively. -At the end of each loop, the entire array will be read, to flush any data that may cause issues later. \ No newline at end of file +At the end of each loop, the entire array will be read, to flush any data that may cause issues later. + +Upcoming (TODO): +- Use bit shift to allow 12 bits to be used for the duty cycle range, as there can only be a max of 16 channels anyway (4 bits). +- Consider protobuf or msgpack for serialisation format, for more advanced use cases, and more maintainable communication formats (currently changing the message format will require changes to the entire protocol) \ No newline at end of file diff --git a/pycar/src/car/control/gpio/abstract_vehicle.py b/pycar/src/car/control/gpio/abstract_vehicle.py new file mode 100644 index 0000000..8028d85 --- /dev/null +++ b/pycar/src/car/control/gpio/abstract_vehicle.py @@ -0,0 +1,26 @@ +from abc import ABC, abstractmethod, abstractproperty + +class AbstractVehicle(ABC): + + @abstractmethod + @property + def throttle(self): + pass + + @abstractmethod + @throttle.setter + def throttle(self): + pass + + @abstractmethod + @property + def steering(self): + pass + + @abstractmethod + @steering.setter + def throttle(self): + pass + + def stop(self): + pass \ No newline at end of file diff --git a/pycar/src/car/control/gpio/factory.py b/pycar/src/car/control/gpio/factory.py index 505e2aa..df9a795 100644 --- a/pycar/src/car/control/gpio/factory.py +++ b/pycar/src/car/control/gpio/factory.py @@ -1,18 +1,25 @@ +from car.control.gpio.abstract_vehicle import AbstractVehicle +from car.control.gpio.serial_vehicle import SerialVehicle from .mockvehicle import MockVehicle import os -def get_vehicle(motor_pin=19, steering_pin=18): +# TODO: Remove need for motor/steering pin, instead retrieve from env variable. +# TODO: Dependency injectino in python? +def get_vehicle(motor_pin=19, steering_pin=18) -> AbstractVehicle: ENV_CAR = None if 'CAR_VEHICLE' not in os.environ else os.environ['CAR_VEHICLE'] - if ENV_CAR == "CAR_2D": + if ENV_CAR == "VEHICLE_2D": try: from .vehicle import Vehicle return Vehicle(motor_pin, steering_pin) except ImportError: print( 'Could not import CAR_2D vehicle. Have you installed the GPIOZERO package?') - elif ENV_CAR == "CAR_MOCK": - return MockVehicle(motor_pin, steering_pin) + elif ENV_CAR == "VEHICLE_MOCK": + return MockVehicle() + elif ENV_CAR == "VEHICLE_SERIAL": + # TODO: Pins in environment variables. + return SerialVehicle() else: print('No valid vehicle found. Have you set the CAR_VEHICLE environment variable?') return None diff --git a/pycar/src/car/control/gpio/mockvehicle.py b/pycar/src/car/control/gpio/mockvehicle.py index b5a08d3..79d37dd 100644 --- a/pycar/src/car/control/gpio/mockvehicle.py +++ b/pycar/src/car/control/gpio/mockvehicle.py @@ -1,10 +1,11 @@ -# A dummy vehicle class to use when -class MockVehicle: - def __init__(self, motor_pin=19, servo_pin=18): - self.motor_pin = motor_pin - self.steering_pin = servo_pin +# A dummy vehicle class to use when testing/not connected to a real device. +from car.control.gpio.abstract_vehicle import AbstractVehicle + + +class MockVehicle(AbstractVehicle): + def __init__(self): print('Using Mock Vehicle') @property @@ -23,21 +24,5 @@ class MockVehicle: def steering(self, value): self._steering = value - @property - def motor_pin(self): - return self._motor_pin - - @motor_pin.setter - def motor_pin(self, value): - self._motor_pin = value - - @property - def steering_pin(self): - return self._steering_pin - - @steering_pin.setter - def steering_pin(self, value): - self._steering_pin = value - def stop(self): self.throttle = 0 diff --git a/pycar/src/car/control/gpio/recording_vehicle_decorator.py b/pycar/src/car/control/gpio/recording_vehicle_decorator.py index 1b839f6..1dc3784 100644 --- a/pycar/src/car/control/gpio/recording_vehicle_decorator.py +++ b/pycar/src/car/control/gpio/recording_vehicle_decorator.py @@ -1,11 +1,12 @@ import datetime +from .abstract_vehicle import AbstractVehicle -class VehicleRecordingDecorator: +class VehicleRecordingDecorator(AbstractVehicle): def __init__(self, vehicle): """ A decorator for a vehicle object to record the changes in steering/throttle. - This will be recorded to memory, and will save to the given file when save is called. + This will be recorded to memory, and will save to the given file when save_data is called. Parameters ---------- @@ -66,21 +67,5 @@ class VehicleRecordingDecorator: 's,' + str(value) + ',' + datetime.datetime.now().isoformat(sep=' ', timespec='seconds')) self._vehicle.steering = value - @property - def motor_pin(self): - return self._vehicle.motor_pin - - @motor_pin.setter - def motor_pin(self, value): - self._vehicle.motor_pin = value - - @property - def steering_pin(self): - return self._vehicle.steering_pin - - @steering_pin.setter - def steering_pin(self, value): - self._vehicle.steering_pin = value - def stop(self): self.throttle = 0 diff --git a/pycar/src/car/control/gpio/serial_vehicle.py b/pycar/src/car/control/gpio/serial_vehicle.py new file mode 100644 index 0000000..48dad07 --- /dev/null +++ b/pycar/src/car/control/gpio/serial_vehicle.py @@ -0,0 +1,42 @@ +from .abstract_vehicle import AbstractVehicle +from serial import Serial + +STEERING_CHANNEL = 1 +THROTTLE_CHANNEL = 2 + + +class SerialVehicle(AbstractVehicle): + + def __init__(self, serial_port='/dev/ttyUSB0', steering_pin=12, throttle_pin=14): + self.serial_port = Serial(port=serial_port, baudrate=115200) + + # Initialise the channels and pins on esp32. + self._init_esp32_pwm(steering_pin, throttle_pin) + self.throttle = 0 + self.steering = 0 + + @property + def throttle(self) -> float: + return self.throttle + + @throttle.setter + def throttle(self, new_throttle: float): + self.throttle = new_throttle + self._set_servo_value(THROTTLE_CHANNEL, new_throttle) + + @property + def steering(self) -> float: + return self.steering + + @steering.setter + def steering(self, new_steering: float): + self.steering = new_steering + self._set_servo_value(STEERING_CHANNEL, new_steering) + + def _set_servo_value(self, channel, value): + # Scale the value to a byte, as 0-255 is the angle range for the esp32 servo. + self.serial_port.write(bytes[channel, (value + 1) / 2 * 255]) + + def _init_esp32_pwm(self, steering_pin, throttle_pin): + self.serial_port.write(bytes([0, 2, STEERING_CHANNEL, + steering_pin, THROTTLE_CHANNEL, throttle_pin])) diff --git a/pycar/src/car/control/gpio/vehicle.py b/pycar/src/car/control/gpio/vehicle.py index 00d3de4..56fe093 100644 --- a/pycar/src/car/control/gpio/vehicle.py +++ b/pycar/src/car/control/gpio/vehicle.py @@ -1,3 +1,4 @@ +from .abstract_vehicle import AbstractVehicle from gpiozero import Servo, Device from gpiozero.pins.pigpio import PiGPIOFactory import subprocess @@ -29,7 +30,7 @@ def _is_pin_valid(pin): # two servos for controls (e.g. drone, dog) -class Vehicle: +class Vehicle(AbstractVehicle): def __init__(self, motor_pin=19, servo_pin=18): subprocess.call(['pigpiod']) Device.pin_factory = PiGPIOFactory()