Add Odometry, Add Robot class, Change SPI Speed, Change Motor to old speeds

compLib/Encoder.py

@@ -2,19 +2,38 @@ import atexit
 from enum import Enum
 
 from compLib.LogstashLogging import Logging
-from compLib.MetricsLogging import MetricsLogging
 from compLib.Spi import Spi, Register
 
 MOTOR_COUNT = 4
 
 
 encoder_start_values = [0] * (MOTOR_COUNT + 1)
-encoder_last_raw_value = [0] * (MOTOR_COUNT + 1)
+
 
 class Encoder(object):
     """Class used to read the encoders
     """
 
+    @staticmethod
+    def handle_wrap(raw_value, port):
+        """Handle overflow and underflow of int for encoders.
+
+        :param raw_value: Raw value which was read on port
+        :param port: Port, which the motor is connected to. Between 1 and 4
+        :raises: IndexError
+        :return: Current encoder position
+        """
+        if port <= 0 or port > MOTOR_COUNT:
+            raise IndexError("Invalid encoder port specified!")
+
+        diff = raw_value - encoder_start_values[port]
+        if diff > 2 ** 31:
+            diff -= 2 ** 32
+        elif diff < -2 ** 31:
+            diff += 2 ** 32
+
+        return diff
+
     @staticmethod
     def read_raw(port: int) -> int:
         """Read raw encoder from a specified port. Will not be reset to 0 at start.
@@ -25,8 +44,7 @@ class Encoder(object):
         """
         if port <= 0 or port > MOTOR_COUNT:
             raise IndexError("Invalid encoder port specified!")
-        
-        global encoder_last_raw_value
+
         raw_value = 0
 
         if port == 1:
@@ -37,14 +55,44 @@ class Encoder(object):
             raw_value = Spi.read(Register.MOTOR_3_POS_B3, 4)
         elif port == 4:
             raw_value = Spi.read(Register.MOTOR_4_POS_B3, 4)
-        
-        if abs(raw_value - encoder_last_raw_value[port]) > 1000:
-            encoder_last_raw_value[port] = raw_value
-            return Encoder.read_raw(port)
-        
-        encoder_last_raw_value[port] = raw_value
+
         return raw_value
-        
+
+    @staticmethod
+    def read_all_raw():
+        """Read all encoders at once.
+        This is faster than read_raw as it only uses one SPI call for all encoders!
+
+        :return: Tuple of all current raw encoder positions
+        """
+
+        encoders = Spi.read_array(Register.MOTOR_1_POS_B3, 4 * 4)
+
+        encoder_1 = int.from_bytes(
+            encoders[0:4], byteorder='big', signed=False)
+        encoder_2 = int.from_bytes(
+            encoders[4:8], byteorder='big', signed=False)
+        encoder_3 = int.from_bytes(
+            encoders[8:12], byteorder='big', signed=False)
+        encoder_4 = int.from_bytes(
+            encoders[12:16], byteorder='big', signed=False)
+
+        return (encoder_1, encoder_2, encoder_3, encoder_4)
+
+    @staticmethod
+    def read_all():
+        """Read all encoders at once.
+        This is faster than read as it only uses one SPI call for all encoders!
+
+        :return: Tuple of all current encoder positions
+        """
+        encoders = Encoder.read_all_raw()
+
+        return (Encoder.handle_wrap(encoders[0], 1), 
+                Encoder.handle_wrap(encoders[1], 2),
+                Encoder.handle_wrap(encoders[2], 3),
+                Encoder.handle_wrap(encoders[3], 4))
+
     @staticmethod
     def read(port: int) -> int:
         """Read encoder from a specified port
@@ -56,14 +104,7 @@ class Encoder(object):
         if port <= 0 or port > MOTOR_COUNT:
             raise IndexError("Invalid encoder port specified!")
 
-        diff = Encoder.read_raw(port) - encoder_start_values[port]
-        if diff > 2 ** 31:
-            diff -= 2 ** 32
-        elif diff < -2 ** 31:
-            diff += 2 ** 32
-        
-        MetricsLogging.put("Encoder", diff, port)
-        return diff
+        return Encoder.handle_wrap(Encoder.read_raw(port), port)
 
     @staticmethod
     def clear(port: int):
@@ -84,4 +125,3 @@ class Encoder(object):
 
         for i in range(1, MOTOR_COUNT + 1):
             encoder_start_values[i] = Encoder.read_raw(i)
-