Add IR, encoder, motor, display

compLib/Spi.py

@@ -2,7 +2,7 @@ import spidev
 from threading import Thread, Lock
 from enum import Enum
 
-# from LogstashLogging import logstash_logger
+from LogstashLogging import logstash_logger
 
 SPI_BUS = 1
 SPI_DEVICE = 2
@@ -29,8 +29,7 @@ class Spi(object):
         rx_buffer = spi.xfer([0] * 32)
 
         if rx_buffer[1] != write_reg:
-            # logstash_logger.error(f"SPI error during write to register {tx_buffer[0]}!")
-            print(f"SPI error during write to register {write_reg}!")
+            logstash_logger.error(f"SPI error during write to register {tx_buffer[0]}!")
 
         return rx_buffer
 
@@ -59,4 +58,92 @@ class Spi(object):
             pos += 1
 
         rx_buf = Spi.transfer(tx_buf)
-        return int.from_bytes(rx_buf[2:2 + length], byteorder='big', signed=False)
+        return int.from_bytes(rx_buf[2:2 + length], byteorder='big', signed=False)
+
+    @staticmethod
+    def write_array(reg: int, length: int, values: list[int]):
+        tx_buf = [0] * SPI_BUFFER_SIZE
+
+        tx_buf[0] = 1
+        tx_buf[1] = reg
+        tx_buf[2] = length
+
+        pos = 3
+        for i in values:
+            tx_buf[pos] = i
+            pos += 1
+
+        rx_buf = Spi.transfer(tx_buf)
+        return rx_buf
+
+
+class Register(Enum):
+    IDENTIFICATION_MODEL_ID = 1,
+    IDENTIFICATION_MODEL_REV_MAJOR = 2,
+    IDENTIFICATION_MODEL_REV_MINOR = 3,
+    IDENTIFICATION_MODEL_REV_PATCH = 4,
+
+    # Motor encoder positions
+    MOTOR_1_POS_B3 = 10,
+    MOTOR_1_POS_B2 = 11,
+    MOTOR_1_POS_B1 = 12,
+    MOTOR_1_POS_B0 = 13,
+    MOTOR_2_POS_B3 = 14,
+    MOTOR_2_POS_B2 = 15,
+    MOTOR_2_POS_B1 = 16,
+    MOTOR_2_POS_B0 = 17,
+    MOTOR_3_POS_B3 = 18,
+    MOTOR_3_POS_B2 = 19,
+    MOTOR_3_POS_B1 = 20,
+    MOTOR_3_POS_B0 = 21,
+    MOTOR_4_POS_B3 = 22,
+    MOTOR_4_POS_B2 = 23,
+    MOTOR_4_POS_B1 = 24,
+    MOTOR_4_POS_B0 = 25,
+
+    # Motor pwm speed
+    MOTOR_1_PWM_H = 26,
+    MOTOR_1_PWM_L = 27,
+    MOTOR_1_CTRL = 28,
+    MOTOR_2_PWM_H = 29,
+    MOTOR_2_PWM_L = 30,
+    MOTOR_2_CTRL = 31,
+    MOTOR_3_PWM_H = 32,
+    MOTOR_3_PWM_L = 33,
+    MOTOR_3_CTRL = 34,
+    MOTOR_4_PWM_H = 35,
+    MOTOR_4_PWM_L = 36,
+    MOTOR_4_CTRL = 37,
+
+    # Servo goal position
+    SERVO_1_PWM_H = 38,
+    SERVO_1_PWM_L = 39,
+    SERVO_2_PWM_H = 40,
+    SERVO_2_PWM_L = 41,
+    SERVO_3_PWM_H = 42,
+    SERVO_3_PWM_L = 43,
+    SERVO_4_PWM_H = 44,
+    SERVO_4_PWM_L = 45,
+
+    # IR Sensor value
+    IR_1_H = 46,
+    IR_1_L = 47,
+    IR_2_H = 48,
+    IR_2_L = 49,
+    IR_3_H = 50,
+    IR_3_L = 51,
+    IR_4_H = 52,
+    IR_4_L = 53,
+    IR_5_H = 54,
+    IR_5_L = 55,
+    IR_1_LED = 56,
+    IR_2_LED = 57,
+    IR_3_LED = 58,
+    IR_4_LED = 59,
+    IR_5_LED = 60,
+
+    # Display registers
+    DISPLAY_LINE_1_C0 = 63,
+    DISPLAY_LINE_2_C0 = 79,
+    DISPLAY_LINE_3_C0 = 95,
+    DISPLAY_LINE_4_C0 = 111