Add support for mock vehicle

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):
+    def __init__(self, vehicle):
-        self.motor = motor
+        self.vehicle = vehicle
-        self.servo = servo
         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]])
 
-
+vehicle = Vehicle() if __name__ == '__main__' else MockVehicle()
-motor = Motor()
+servicer = MotorServicer(vehicle)
-servo = Servo(18)
-servicer = MotorServicer(motor, servo)
 
 service_thread = Thread(target=servicer.start_server)
 service_thread.start()

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

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