From 49165840b9b313128f8cca3cf9995961e90c9eef Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Wed, 14 Aug 2019 18:16:21 +0930
Subject: [PATCH] Add rest of the MotorServer files.

---
 MotorControl/MotorServer.py | 42 ++++++++++++++++++++++++++++++++++---
 1 file changed, 39 insertions(+), 3 deletions(-)

diff --git a/MotorControl/MotorServer.py b/MotorControl/MotorServer.py
index 99d22c4..cec81ae 100644
--- a/MotorControl/MotorServer.py
+++ b/MotorControl/MotorServer.py
@@ -1,11 +1,47 @@
+from threading import Timer, Thread
+from gpiozero import Servo
+
 import MotorControl.motorService_pb2 as motorService_pb2
 import MotorControl.motorService_pb2_grpc as motorService_pb2_grpc
 
 from MotorControl.gpiozero.motor_session import Motor
 
-class MotorServer(motorService_pb2_grpc.CarControlServicer):
+servo_pin = 18
+timeout_length = 3
+
+class MotorServicer(motorService_pb2_grpc.CarControlServicer):
     def __init__(self):
         self.motor = Motor()
+        self.servo = Servo(servo_pin)
+        self._timer = None
+
+    def SetThrottle(self, request, context):
+        return motorService_pb2.ThrottleResponse(throttleSet = (True if self.motor.set_throttle(request.throttle) else False))
+
+    def SetSteering(self, request, context):
+        # TODO: Fix this to use the motor object as well to check for bounds.
+        self.servo.value = request.steering
+        return motorService_pb2.SteeringResponse(steeringSet = True)
+
+    def Heartbeat(self, request, context):
+        # If we don't get a response every 3 seconds, stop the car.
+        # This isn't a stream right now, however may change it to be so since we'll constantly
+        # be sending values...
+        self.set_timeout(timeout_length)
+
+    def set_timeout(self, min_timeout):
+        """Stops the old timer and restarts it to the specified time.
+        
+        min_timeout -- The minimum time that can be used for the timer.
+        vary_timout -- Default 200, the additional random varying time (0 - vary_timeout) to add to timer.
+        """ 
+        if self._timer is not None:
+            self._timer.cancel()
+        self._timer = Timer(min_timeout, self.timeout_elapsed)
+        self._timer.start()
+
+    def timeout_elapsed(self):
+        """Election or heartbeat timeout has elapsed."""
+        print("Node timeout elapsed")
+        self.motor.stop()
 
-    def ThrottleRequestRPC(self, request, context):
-        
\ No newline at end of file