diff --git a/control/MotorServer.py b/control/MotorServer.py index cd29eb6..68da24c 100755 --- a/control/MotorServer.py +++ b/control/MotorServer.py @@ -9,15 +9,15 @@ import grpc import control.motorService_pb2 as motorService_pb2 import control.motorService_pb2_grpc as motorService_pb2_grpc -from control.gpiozero.motor_session import Motor +from control.gpiozero.vehicle import Vehicle +from control.gpiozero.mockvehicle import MockVehicle from slam.slam_servicer import SlamServicer import slam.SlamController_pb2_grpc as SlamController_pb2_grpc class MotorServicer(motorService_pb2_grpc.CarControlServicer): - def __init__(self, motor, servo): - self.motor = motor - self.servo = servo + def __init__(self, vehicle): + self.vehicle = vehicle self._timer = None def SetThrottle(self, request, context): @@ -26,13 +26,12 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer): throttleFailed = False print('Setting throttle to: ' + str(request.throttle)) self.set_timeout(3) - throttleFailed = self.motor.set_throttle(request.throttle) + throttleFailed = self.vehicle.set_throttle(request.throttle) return motorService_pb2.ThrottleResponse(throttleSet=throttleFailed) def SetSteering(self, request, context): - # TODO: Fix this to use the motor object as well to check for bounds. print('Setting steering to: ' + str(request.steering)) - self.servo.value = request.steering + self.vehicle.steering = request.steering return motorService_pb2.SteeringResponse(steeringSet=True) def set_timeout(self, min_timeout): @@ -48,7 +47,7 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer): def timeout_elapsed(self): """Election or heartbeat timeout has elapsed.""" print("Node timeout elapsed") - self.motor.stop() + self.vehicle.stop() def start_server(self): server = grpc.server(futures.ThreadPoolExecutor(max_workers=8)) @@ -66,6 +65,7 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer): return SlamServicer('/dev/ttyUSB0') def create_credentials(self): + # Relativise this stuff. pvtKeyPath = '/home/pi/tls/device.key' pvtCertPath = '/home/pi/tls/device.crt' @@ -74,10 +74,8 @@ class MotorServicer(motorService_pb2_grpc.CarControlServicer): return grpc.ssl_server_credentials([[pvtKeyBytes, pvtCertBytes]]) - -motor = Motor() -servo = Servo(18) -servicer = MotorServicer(motor, servo) +vehicle = Vehicle() if __name__ == '__main__' else MockVehicle() +servicer = MotorServicer(vehicle) service_thread = Thread(target=servicer.start_server) service_thread.start() diff --git a/control/gpiozero/mockvehicle.py b/control/gpiozero/mockvehicle.py new file mode 100644 index 0000000..9b0c8ec --- /dev/null +++ b/control/gpiozero/mockvehicle.py @@ -0,0 +1,42 @@ + + +# 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 + + @property + def throttle(self): + return self._throttle + + @throttle.setter + def throttle(self, value): + self._throttle = value + + @property + def steering(self): + return self._steering + + @steering.setter + 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/control/gpiozero/vehicle.py b/control/gpiozero/vehicle.py new file mode 100644 index 0000000..9ae941b --- /dev/null +++ b/control/gpiozero/vehicle.py @@ -0,0 +1,83 @@ +from gpiozero import Servo, Device +from gpiozero.pins.pigpio import PiGPIOFactory +import subprocess + + +def _safely_set_servo_value(servo, value): + try: + if value < -1 or value > 1: + print("Not setting throttle, invalid value set.") + return False + servo.value = value + except TypeError: + print("throttle should be a number, preferably a float.") + return False + return True + +def _is_pin_valid(pin): + if isinstance(pin, int): + if pin < 2 or pin > 21: + print("Invalid GPIO pin") + return False + return True + else: + print("Value must be an int.") + return False + +# TODO: Allow a vector to be set to change the throttle/steering, for vehicles that don't use +# two servos for controls (e.g. drone, dog) +class Vehicle: + def __init__(self, motor_pin=19, servo_pin=18): + subprocess.call(['sudo', 'pigpiod']) + Device.pin_factory = PiGPIOFactory() + print('Using pin factory:') + print(Device.pin_factory) + self.motor_pin = motor_pin + self.steering_pin = servo_pin + self.initialise_motor() + + def initialise_motor(self): + self._motor_servo = Servo( + self._motor_pin, pin_factory=Device.pin_factory) + self._steering_servo = Servo(self._steering_pin, pin_factory=Device.pin_factory) + + @property + def throttle(self): + return self._motor_servo.value + + @throttle.setter + def throttle(self, value): + _safely_set_servo_value(self._motor_servo, value) + + @property + def steering(self): + return self._motor_servo.value + + @steering.setter + def steering(self, value): + _safely_set_servo_value(self._motor_servo, value) + + @property + def motor_pin(self): + return self._motor_pin + + @motor_pin.setter + def motor_pin(self, value): + # TODO: Reinitialise the servo when the pin changes, or discard this method + # (probably don't want to allow pin changes whilst the device is in use anyway) + self._motor_pin = value if _is_pin_valid(value) else self._motor_pin + + @property + def steering_pin(self): + return self._steering_pin + + @steering_pin.setter + def steering_pin(self, value): + self._steering_pin = value if _is_pin_valid(value) else self._steering_pin + + def stop(self): + self.throttle = 0 + self.steering = 0 + + def change_with_vector(self, vector): + pass