From c5dd14fb1f5a88da9342a25789131092e183caee Mon Sep 17 00:00:00 2001
From: root <you@example.com>
Date: Fri, 12 Nov 2021 19:17:14 +0000
Subject: [PATCH] Add bot driving slower

---
 compLib/Api.py      |  8 ++++++++
 compLib/Motor.py    | 43 +++++++++++++++++++++++++++++++++++++++++++
 compLib/__init__.py |  2 ++
 3 files changed, 53 insertions(+)

diff --git a/compLib/Api.py b/compLib/Api.py
index f518f9b..55e0a3c 100644
--- a/compLib/Api.py
+++ b/compLib/Api.py
@@ -13,6 +13,7 @@ API_URL_GET_MATERIAL = API_URL + "getMaterial"
 API_URL_GET_GARBAGE = API_URL + "getGarbage"
 API_URL_GET_LIST_CARGO = API_URL + "listCargo"
 API_URL_GET_CARGO = API_URL + "getCargo/"
+API_URL_GET_ROBOT_STATE = API_URL + "getRobotState"
 
 API_URL_GET_POS = API_URL + "getPos"
 API_URL_GET_OP = API_URL + "getOp"
@@ -87,6 +88,12 @@ class Seeding:
         Logging.get_logger().debug(f"Seeding.get_cargo {result}, status code = {res.status_code}")
         return result, res.status_code
 
+    @staticmethod
+    def get_robot_state() -> Tuple[Dict, int]:
+        res = requests.get(API_URL_GET_ROBOT_STATE)
+        result = json.loads(res.content)
+        Logging.get_logger().debug(f"Seeding.get_robot_state {result}, status code = {res.status_code}")
+        return result, res.status_code
 
 class Position:
     """Datastructure for holding a position
@@ -209,3 +216,4 @@ class DoubleElim:
         response = json.loads(res.content)
         Logging.get_logger().debug(f"DoubleElim.get_meteoroids = {response}, status code = {res.status_code}")
         return response, res.status_code
+
diff --git a/compLib/Motor.py b/compLib/Motor.py
index 4570026..272e3ea 100644
--- a/compLib/Motor.py
+++ b/compLib/Motor.py
@@ -10,6 +10,10 @@ MAX_MOTOR_SPEED = 65535
 MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0
 MOTOR_CURVE = [0.0, 0.0, 426.5, 692.0, 842.5, 953.5, 1032.5, 1090.5, 1135.5, 1171.0, 1203.5, 1230.0, 1249.5, 1268.0, 1283.0, 1298.5, 1308.0, 1320.0, 1332.0, 1339.5, 1352.5]
 
+SPEED_LOCK = False
+SPEED_MULT = 1.0
+
+
 class MotorMode(IntEnum):
     COAST = 0,
     FORWARD = 1,
@@ -29,6 +33,10 @@ class Motor(object):
         :param mode: Motor mode. See enum MotorMode for more info
         :raises: IndexError
         """
+
+        if SPEED_LOCK:
+            return
+
         if port <= 0 or port > MOTOR_COUNT:
             raise IndexError("Invalid Motor port specified!")
 
@@ -70,6 +78,7 @@ class Motor(object):
         elif percent > 0:
             mode = MotorMode.FORWARD
 
+        percent *= SPEED_MULT
         pwm = percent * MOTOR_PERCENTAGE_MULT
 
         Motor.pwm(port, int(pwm), mode)
@@ -172,4 +181,38 @@ class Motor(object):
         return Motor.__interpolateSpeed(curve, requiredSpeed)
 
 
+    @staticmethod
+    def robot_state_loop():
+        import time
+        from compLib.Api import Seeding
+
+        global SPEED_LOCK
+        global SPEED_MULT
+
+        while True:
+            try:
+                state, status = Seeding.get_robot_state()
+                SPEED_MULT = state["speed"] / 100.0
+
+                if state["left"] != -1 or state["right"] != -1:
+                    Motor.power(1, state["right"])
+                    Motor.power(4, state["left"])
+                    SPEED_LOCK = True
+                    time.sleep(state["time"])
+                    SPEED_LOCK = False
+                    Motor.power(1, 0)
+                    Motor.power(4, 0)
+                else:
+                    time.sleep(0.25)
+            except:
+                time.sleep(0.25)
+                pass
+
+    @staticmethod
+    def start_robot_state_loop():
+        from threading import Thread
+        robot_state_thread = Thread(target=Motor.robot_state_loop)
+        robot_state_thread.setDaemon(True)
+        robot_state_thread.start()
+
 atexit.register(Motor.all_off)
diff --git a/compLib/__init__.py b/compLib/__init__.py
index 2570b5b..728ab3d 100644
--- a/compLib/__init__.py
+++ b/compLib/__init__.py
@@ -29,12 +29,14 @@ if spi_found:
     from compLib.Spi import Spi
     from compLib.Reset import Reset
     from compLib.Encoder import Encoder
+    from compLib.Motor import Motor
     import logging
 
     print(f"\033[34mInitializing chipmunk board...\033[0m")
     Reset.reset_bot()
     Spi.health_check()
     Spi.start_health_check_loop()
+    Motor.start_robot_state_loop()
     Encoder.clear_all()
     print(f"\033[34mReady\033[0m\n")
 else: