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]
+        return Encoder.handle_wrap(Encoder.read_raw(port), port)
-        if diff > 2 ** 31:
-            diff -= 2 ** 32
-        elif diff < -2 ** 31:
-            diff += 2 ** 32
-        
-        MetricsLogging.put("Encoder", diff, port)
-        return diff
 
     @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)
-        

compLib/MetricsLogging.py

@@ -2,12 +2,13 @@ from influxdb_client import InfluxDBClient, Point, WritePrecision
 import socket
 import uuid
 import os
-import queue
+import multiprocessing
+import faster_fifo
 import datetime
-import threading
 import requests
+import time
 
-CONCURRENCY = 2
+CONCURRENCY = 1
 
 HOSTNAME = socket.gethostname()
 RUN_TRACE = str(uuid.uuid4())
@@ -28,7 +29,7 @@ else:
 influx_client = InfluxDBClient(url=INFLUX_HOST, token=TOKEN)
 write_api = influx_client.write_api()
 
-point_queue = queue.Queue()
+point_queue = faster_fifo.Queue()
 workers = []
 
 class MetricsLogging():
@@ -72,23 +73,22 @@ class MetricsLogging():
     @staticmethod
     def worker():
         while True:
-            point = point_queue.get()
-            if point is None: # Kill signal
-                return
-
             try:
-                write_api.write(BUCKET, ORG, point)
+                points = point_queue.get_many()
-
+                write_api.write(BUCKET, ORG, points)
+                time.sleep(0.1)
+            except Exception as e:
+                pass
             finally:
-                point_queue.task_done()
+                pass
-    
+
     @staticmethod
     def start_workers():
         global EXTENSIVE_LOGGING
         if EXTENSIVE_LOGGING:
             if MetricsLogging.is_influx_reachable():
                 for i in range(CONCURRENCY):
-                    worker = threading.Thread(target=MetricsLogging.worker, daemon=True)
+                    worker = multiprocessing.Process(target=MetricsLogging.worker, daemon=True)
                     worker.start()
                     workers.append(worker)
             else:

compLib/Motor.py

@@ -8,7 +8,8 @@ from compLib.Spi import Spi, Register
 MOTOR_COUNT = 4
 MAX_MOTOR_SPEED = 65535
 MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0
-MOTOR_CURVE = [0.0, 0.0, 0.5, 60.0, 199.83333333333334, 377.66666666666663, 990.3333333333333, 1860.6666666666665, 2587.0, 3091.6666666666665, 3489.0, 3860.5, 4197.333333333333, 4432.166666666667, 4647.166666666666, 4873.166666666666, 5054.333333333334, 5208.666666666667, 5353.0, 5466.5, 5604.0]
+NEW_MOTOR_CURVE = [0.0, 0.0, 0.5, 60.0, 199.83333333333334, 377.66666666666663, 990.3333333333333, 1860.6666666666665, 2587.0, 3091.6666666666665, 3489.0, 3860.5, 4197.333333333333, 4432.166666666667, 4647.166666666666, 4873.166666666666, 5054.333333333334, 5208.666666666667, 5353.0, 5466.5, 5604.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

compLib/Odom.py

@@ -0,0 +1,69 @@
+import time
+import math
+from compLib.Encoder import Encoder
+from compLib.Robot import Robot
+
+last_run = time.time()
+last_enc_left = 0
+last_enc_right = 0
+
+pos_x = 0
+pos_y = 0
+orientation = 0
+
+
+class Odometry():
+    def __init__(self, x, y, orientation):
+        self.x = x
+        self.y = y
+        self.orientation = orientation
+
+    def __str__(self):
+        return f"X: {self.x} Y: {self.y} O: {self.orientation}"
+
+class Odom(object):
+
+    @staticmethod
+    def get_odom():
+        return Odometry(pos_x, pos_y, orientation)
+
+    @staticmethod
+    def clear():
+        global last_run, last_enc_left, last_enc_right, pos_x, pos_y, orientation
+
+        last_run = 0
+        last_enc_left = 0
+        last_enc_right = 0
+        pos_x = 0
+        pos_y = 0
+        orientation = 0
+
+    @staticmethod
+    def update():
+        global last_run, last_enc_left, last_enc_right, pos_x, pos_y, orientation
+
+        now = time.time()
+        if last_run + 0.01 < now:
+            last_run = now
+
+            encoder_positions = Encoder.read_all()
+            current_left = encoder_positions[Robot.LEFT_PORT - 1]
+            current_right = -encoder_positions[Robot.RIGHT_PORT - 1]
+            distance_left = (current_left - last_enc_left) / Robot.TICKS_PER_METER
+            distance_right = (current_right - last_enc_right) / Robot.TICKS_PER_METER
+            last_enc_left = current_left
+            last_enc_right = current_right
+
+            distance = (distance_left + distance_right) / 2.0
+            theta = (distance_left - distance_right) / Robot.ARBOR_LENGTH_M
+
+            if distance != 0:
+                distance_x = math.cos(theta) * distance
+                distance_y = -math.sin(theta) * distance
+
+                pos_x = pos_x + (math.cos(orientation) * distance_x -
+                                 math.sin(orientation) * distance_y)
+                pos_y = pos_y + (math.sin(orientation) * distance_x +
+                                 math.cos(orientation) * distance_y)
+            if theta != 0:
+                orientation += theta

compLib/Robot.py

@@ -0,0 +1,12 @@
+import math
+
+
+class Robot(object):
+    
+    WHEEL_CIRCUMFERENCE_MM = 71.0 * math.pi
+    TICKS_PER_TURN = 27.7 * 100.0
+    ARBOR_LENGTH_MM = 139.0
+    ARBOR_LENGTH_M = ARBOR_LENGTH_MM / 1000.0
+    TICKS_PER_METER = 1000.0 / WHEEL_CIRCUMFERENCE_MM * TICKS_PER_TURN
+    LEFT_PORT = 4
+    RIGHT_PORT = 1

compLib/Spi.py

@@ -8,7 +8,7 @@ from compLib.LogstashLogging import Logging
 
 SPI_BUS = 1
 SPI_DEVICE = 2
-SPI_SPEED = 1000000
+SPI_SPEED = 4000000
 SPI_BUFFER_SIZE = 32
 SPI_HEALTH = True
 
@@ -117,7 +117,7 @@ class Spi(object):
         write_reg = tx_buffer[1]
 
         spi.xfer(tx_buffer)
-        rx_buffer = spi.xfer([0] * 32)
+        rx_buffer = spi.xfer([0] * SPI_BUFFER_SIZE)
         
         if rx_buffer[1] != write_reg:
             Logging.get_logger().error(f"SPI error during read/write of register {write_reg}!")
@@ -126,6 +126,10 @@ class Spi(object):
 
     @staticmethod
     def read(reg: int, length: int):
+        return int.from_bytes(Spi.read_array(reg, length), byteorder='big', signed=False)
+
+    @staticmethod
+    def read_array(reg: int, length: int):
         if not type(reg) is int:
             reg = int(reg)
 
@@ -136,7 +140,7 @@ class Spi(object):
         tx_buf[2] = length
 
         rx_buf = Spi.transfer(tx_buf)
-        return int.from_bytes(rx_buf[2:2 + length], byteorder='big', signed=False)
+        return rx_buf[2:2 + length]
 
     @staticmethod
     def write(reg: int, length: int, value: int):