Add IR, encoder, motor, display

compLib/Display.py

@@ -0,0 +1,39 @@
+import string
+
+from compLib.LogstashLogging import logstash_logger
+from compLib.Spi import Spi, Register
+
+LINE_COUNT = 4
+CHARS_PER_LINE = 16
+
+
+class Display(object):
+    """Access the different IR Sensors of the robot
+    """
+
+    @staticmethod
+    def write(line: int, text: string):
+        if len(text) > CHARS_PER_LINE:
+            logstash_logger.error(f"Too many characters specified!")
+            return
+
+        if line <= 0 or line > LINE_COUNT:
+            raise IndexError("Invalid line number specified")
+
+        to_write = [0] * CHARS_PER_LINE
+        for i in range(len(text)):
+            to_write[i] = ord(text[i])
+
+        if line == 1:
+            Spi.write(Register.DISPLAY_LINE_1_C0, CHARS_PER_LINE, to_write)
+        elif line == 2:
+            Spi.write(Register.DISPLAY_LINE_2_C0, CHARS_PER_LINE, to_write)
+        elif line == 3:
+            Spi.write(Register.DISPLAY_LINE_3_C0, CHARS_PER_LINE, to_write)
+        elif line == 4:
+            Spi.write(Register.DISPLAY_LINE_4_C0, CHARS_PER_LINE, to_write)
+
+    @staticmethod
+    def clear():
+        for i in range(1, LINE_COUNT + 1):
+            Display.write(i, "")

compLib/Encoder.py

@@ -0,0 +1,37 @@
+import atexit
+from enum import Enum
+
+from compLib.LogstashLogging import Logging
+from compLib.Spi import Spi, Register
+
+MOTOR_COUNT = 4
+
+
+class Encoder(object):
+    """Class used to read the encoders
+    """
+
+    @staticmethod
+    def read(port: int) -> int:
+        """Read encoder from a specified port
+
+        :param port: Port, which the motor is connected to. 1-4 allowed
+        :raises: IndexError
+        :return: Current encoder position
+        """
+        if port <= 0 or port > MOTOR_COUNT:
+            raise IndexError("Invalid encoder port specified!")
+
+        if port == 1:
+            return Spi.read(Register.MOTOR_1_POS_B3, 4)
+        elif port == 2:
+            return Spi.read(Register.MOTOR_2_POS_B3, 4)
+        elif port == 3:
+            return Spi.read(Register.MOTOR_3_POS_B3, 4)
+        elif port == 4:
+            return Spi.read(Register.MOTOR_4_POS_B3, 4)
+
+    @staticmethod
+    def reset(port: int):
+        # TODO implement registers and me
+        pass

compLib/IRSensor.py

@@ -1,157 +1,40 @@
-import pigpio
-
-from compLib.ADC import ADC
 from compLib.LogstashLogging import Logging
+from compLib.Spi import Spi, Register
 import spidev
-import threading
-import time
 
-TOP_LEFT_CHANNEL = 0
+SENSOR_COUNT = 5
-TOP_RIGHT_CHANNEL = 1
-
-TOP_IR_MIN_VOLTAGE = 0.0
-TOP_IR_MAX_VOLTAGE = 3.3
-
-BOTTOM_LEFT_PIN = 14
-BOTTOM_MIDDLE_PIN = 15
-BOTTOM_RIGHT_PIN = 23
-
-BOTTOM_LEFT_CHANNEL = 2
-BOTTOM_MIDDLE_CHANNEL = 1
-BOTTOM_RIGHT_CHANNEL = 0
-
-adc = ADC()
-spi = spidev.SpiDev()
-spi.open(0, 0)
-spi.max_speed_hz = 1000000
-
-# GPIO.setmode(GPIO.BCM)
-
-GPIO = pigpio.pi()
-
-GPIO.set_mode(BOTTOM_LEFT_PIN, pigpio.INPUT)
-GPIO.set_mode(BOTTOM_MIDDLE_PIN, pigpio.INPUT)
-GPIO.set_mode(BOTTOM_RIGHT_PIN, pigpio.INPUT)
-
-states = {
-    str(BOTTOM_LEFT_PIN): GPIO.read(BOTTOM_LEFT_PIN),
-    str(BOTTOM_MIDDLE_PIN): GPIO.read(BOTTOM_MIDDLE_PIN),
-    str(BOTTOM_RIGHT_PIN): GPIO.read(BOTTOM_RIGHT_PIN)}
-
-analog_states = {
-    str(BOTTOM_LEFT_CHANNEL): 0,
-    str(BOTTOM_MIDDLE_CHANNEL): 0,
-    str(BOTTOM_RIGHT_CHANNEL): 0}
-
-
-def gpio_callback(gpio, level, tick):
-    states[str(gpio)] = level
-
-
-GPIO.callback(BOTTOM_LEFT_PIN, pigpio.EITHER_EDGE, gpio_callback)
-GPIO.callback(BOTTOM_MIDDLE_PIN, pigpio.EITHER_EDGE, gpio_callback)
-GPIO.callback(BOTTOM_RIGHT_PIN, pigpio.EITHER_EDGE, gpio_callback)
-
-
-def read_analog_channel(channel):
-    spi.writebytes([channel << 3, 0])
-
-    time.sleep(0.0033)  # 300HZ
-
-    bytes = spi.readbytes(2)
-    return (bytes[0] * 256 + bytes[1]) >> 2
-
-
-def sensor_thread():
-    while True:
-        analog_states[str(BOTTOM_LEFT_CHANNEL)] = read_analog_channel(BOTTOM_LEFT_CHANNEL)
-        analog_states[str(BOTTOM_MIDDLE_CHANNEL)] = read_analog_channel(BOTTOM_MIDDLE_CHANNEL)
-        analog_states[str(BOTTOM_RIGHT_CHANNEL)] = read_analog_channel(BOTTOM_RIGHT_CHANNEL)
-
-
-analog_thread = threading.Thread(target=sensor_thread, daemon=True)
-analog_thread.start()
-time.sleep(0.25)
 
 
 class IRSensor(object):
-    """Access the different IR Sensors at top / bottom of the robot
+    """Access the different IR Sensors of the robot
     """
 
     @staticmethod
-    def top_left_percent() -> int:
+    def read(sensor: int) -> int:
-        """Get top left infrared sensor percentage
+        if sensor <= 0 or sensor > SENSOR_COUNT:
+            raise IndexError("Invalid sensor specified!")
 
-        :return: Percentage between 0 and 100
+        if sensor == 1:
-        :rtype: int
+            return Spi.read(Register.IR_1_H, 2)
-        """
+        elif sensor == 2:
-        voltage = adc.read(TOP_LEFT_CHANNEL)
+            return Spi.read(Register.IR_2_H, 2)
-        return int((voltage - TOP_IR_MIN_VOLTAGE) / TOP_IR_MAX_VOLTAGE * 100)
+        elif sensor == 3:
+            return Spi.read(Register.IR_3_H, 2)
+        elif sensor == 4:
+            return Spi.read(Register.IR_4_H, 2)
+
+        return 0
 
     @staticmethod
-    def top_right_percent() -> int:
+    def set(sensor: int, on: bool):
-        """Get top right infrared sensor percentage
+        if sensor <= 0 or sensor > SENSOR_COUNT:
-
+            raise IndexError("Invalid sensor specified!")
-        :return: Percentage between 0 and 100
-        :rtype: int
-        """
-        voltage = adc.read(TOP_RIGHT_CHANNEL)
-        return int((voltage - TOP_IR_MIN_VOLTAGE) / TOP_IR_MAX_VOLTAGE * 100)
-
-    @staticmethod
-    def bottom_left() -> bool:
-        """Get bottom left infrared sensor status
-
-        :return: True, if sensor detects IR signals
-        :rtype: bool
-        """
-        return states[str(BOTTOM_LEFT_PIN)]
-        # return GPIO.read(BOTTOM_LEFT_PIN)
-
-    @staticmethod
-    def bottom_middle() -> bool:
-        """Get bottom middle infrared sensor status
-
-        :return: True, if sensor detects IR signals
-        :rtype: bool
-        """
-        return states[str(BOTTOM_MIDDLE_PIN)]
-        # return GPIO.read(BOTTOM_MIDDLE_PIN)
-
-    @staticmethod
-    def bottom_right() -> bool:
-        """Get bottom right infrared sensor status
-
-        :return: True, if sensor detects IR signals
-        :rtype: bool
-        """
-        return states[str(BOTTOM_RIGHT_PIN)]
-        # return GPIO.read(BOTTOM_RIGHT_PIN)
-
-    @staticmethod
-    def bottom_left_analog() -> int:
-        """Get bottom left infrared sensor value: ranges from 0 to 1023
-
-        :return: 10-bit brightness value
-        :rtype: int
-        """
-        return analog_states[str(BOTTOM_LEFT_CHANNEL)]
-
-    @staticmethod
-    def bottom_middle_analog() -> int:
-        """Get bottom middle infrared sensor value: ranges from 0 to 1023
-
-        :return: 10-bit brightness value
-        :rtype: int
-        """
-        return analog_states[str(BOTTOM_MIDDLE_CHANNEL)]
-
-    @staticmethod
-    def bottom_right_analog() -> int:
-        """Get bottom right infrared sensor value: ranges from 0 to 1023
-
-        :return: 10-bit brightness value
-        :rtype: int
-        """
-        return analog_states[str(BOTTOM_RIGHT_CHANNEL)]
 
+        if sensor == 1:
+            Spi.write(Register.IR_1_LED, on)
+        elif sensor == 2:
+            Spi.write(Register.IR_2_LED, on)
+        elif sensor == 3:
+            Spi.write(Register.IR_3_LED, on)
+        elif sensor == 4:
+            Spi.write(Register.IR_4_LED, on)

compLib/IRWrapper.py

@@ -1,81 +0,0 @@
-from compLib.IRSensor import IRSensor
-
-import time
-
-ir = IRSensor()
-
-MIN_VALUE = 0.0
-MAX_VALUE = 0.0
-MOVING_AVERAGE_SIZE = 30.0
-
-
-def approximate_rolling_average(avg, value):
-    avg -= avg / MOVING_AVERAGE_SIZE
-    avg += value / MOVING_AVERAGE_SIZE
-    return avg
-
-
-class IRWrapper(object):
-    """Wrapper around the IRSensor to enable calibration of the sensor
-    """
-
-    @staticmethod
-    def calibrate():
-        """Calibrate the black and white values of the IRSensors
-        This is done by putting the bot on black line with the middle sensor
-        Afterwards, all sensors are read for 2 seconds and filtered.
-        The minimum value is used for white, maximum is black.
-        """
-        global MIN_VALUE
-        global MAX_VALUE
-
-        left_avg = ir.bottom_left_analog()
-        middle_avg = ir.bottom_middle_analog()
-        right_avg = ir.bottom_right_analog()
-
-        start_time = time.time()
-        while time.time() - start_time < 2:
-            left_avg = approximate_rolling_average(left_avg, ir.bottom_left_analog())
-            middle_avg = approximate_rolling_average(middle_avg, ir.bottom_middle_analog())
-            right_avg = approximate_rolling_average(right_avg, ir.bottom_right_analog())
-            time.sleep(0.01)
-
-        MIN_VALUE = min([left_avg, middle_avg, right_avg])
-        MAX_VALUE = max([left_avg, middle_avg, right_avg])
-
-    @staticmethod
-    def adjust_for_calibration(raw_value: float) -> float:
-        """Adjust a raw sensor value to 0 to 1000
-
-        :return: Value between 0 and 1000, White and Black
-        :rtype: float
-        """
-        x = (raw_value - MIN_VALUE) * 1000.0 / (MAX_VALUE - MIN_VALUE)
-        return max(min(1000.0, x), 0.0)
-
-    @staticmethod
-    def bottom_left_calibrated() -> float:
-        """Returns calibrated value of the bottom left analog sensor
-
-        :return: Value between 0 and 1000, White and Black
-        :rtype: float
-        """
-        return IRWrapper.adjust_for_calibration(ir.bottom_left_analog())
-
-    @staticmethod
-    def bottom_middle_calibrated() -> float:
-        """Returns calibrated value of the bottom middle analog sensor
-
-        :return: Value between 0 and 1000, White and Black
-        :rtype: float
-        """
-        return IRWrapper.adjust_for_calibration(ir.bottom_middle_analog())
-
-    @staticmethod
-    def bottom_right_calibrated() -> float:
-        """Returns calibrated value of the bottom right analog sensor
-
-        :return: Value between 0 and 1000, White and Black
-        :rtype: float
-        """
-        return IRWrapper.adjust_for_calibration(ir.bottom_right_analog())

compLib/Motor.py

@@ -1,49 +1,76 @@
 import atexit
+from enum import Enum
 
 from compLib.LogstashLogging import Logging
+from compLib.Spi import Spi, Register
 
 MOTOR_COUNT = 4
-MAX_MOTOR_SPEED = 4095.0
+MAX_MOTOR_SPEED = 65535
 MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0
 
 
+class MotorMode(Enum):
+    COAST = 0,
+    FORWARD = 1,
+    BACKWARD = 2,
+    BREAK = 3
+
+
 class Motor(object):
     """Class used to control the motors
     """
 
+    @staticmethod
+    def pwm(port: int, pwm: int, mode: MotorMode):
+        """Set specified motor to a specific pwm value and motor mode
+
+        :param port: Port, which the motor is connected to. 1-4 allowed
+        :param pwm: Raw PWM value which the motor should be set to. From 0 to 2^16 - 1
+        :param mode: Motor mode. See enum MotorMode for more info
+        :raises: IndexError
+        """
+        if port <= 0 or port > MOTOR_COUNT:
+            raise IndexError("Invalid Motor port specified!")
+
+        if port == 1:
+            Spi.write(Register.MOTOR_1_PWM_H, 2, pwm)
+            Spi.write(Register.MOTOR_1_CTRL, 1, mode)
+        elif port == 2:
+            Spi.write(Register.MOTOR_2_PWM_H, 2, pwm)
+            Spi.write(Register.MOTOR_2_CTRL, 1, mode)
+        elif port == 3:
+            Spi.write(Register.MOTOR_3_PWM_H, 2, pwm)
+            Spi.write(Register.MOTOR_3_CTRL, 1, mode)
+        elif port == 4:
+            Spi.write(Register.MOTOR_4_PWM_H, 2, pwm)
+            Spi.write(Register.MOTOR_4_CTRL, 1, mode)
+
     @staticmethod
     def power(port: int, percent: float):
         """Set specified motor to percentage power
 
-        :param port: Port, which the motor is connected to. 0-3, 0 -> top left, 3 -> top right
+        :param port: Port, which the motor is connected to. 1-4
         :param percent: Percentage of max speed. between -100 and 100
         :raises: IndexError
         """
         Logging.get_logger().debug(f"Motor.power {port} {percent}")
 
-        if port < 0 or port >= MOTOR_COUNT:
+        if port <= 0 or port > MOTOR_COUNT:
             raise IndexError("Invalid Motor port specified!")
 
         if percent < -100 or percent > 100:
             raise IndexError("Invalid Motor speed specified! Speed is between -100 and 100 percent!")
 
-        forward = True
+        mode = MotorMode.COAST
         if percent < 0:
             percent = abs(percent)
-            forward = False
+            mode = MotorMode.BACKWARD
+        elif percent > 0:
+            mode = MotorMode.FORWARD
 
-        # bottom left motor is inverted - REEEEEEEEEEEE
+        pwm = percent * MOTOR_PERCENTAGE_MULT
-        if port == 1:
-            forward = not forward
 
-        adjusted_speed = int(min(max(0.0, percent), 100.0) * MOTOR_PERCENTAGE_MULT)
+        Motor.pwm(port, int(pwm), mode)
-
-        if forward:
-            pwm.setMotorPwm(port * 2, adjusted_speed)
-            pwm.setMotorPwm(port * 2 + 1, 0)
-        else:
-            pwm.setMotorPwm(port * 2, 0)
-            pwm.setMotorPwm(port * 2 + 1, adjusted_speed)
 
     @staticmethod
     def all_off():
@@ -52,8 +79,17 @@ class Motor(object):
         """
         Logging.get_logger().debug(f"Motor.all_off")
 
-        for i in range(0, MOTOR_COUNT):
+        for i in range(1, MOTOR_COUNT + 1):
-            Motor.power(i, 0)
+            Motor.active_break(i)
+
+    @staticmethod
+    def active_break(port: int):
+        """
+        Actively break with a specific motor
+
+        :param port: Port, which the motor is connected to. 1-4
+        """
+        Motor.pwm(port, 0, MotorMode.BREAK)
 
 
 atexit.register(Motor.all_off)

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]}!")
+            logstash_logger.error(f"SPI error during write to register {tx_buffer[0]}!")
-            print(f"SPI error during write to register {write_reg}!")
 
         return rx_buffer
 
@@ -60,3 +59,91 @@ class Spi(object):
 
         rx_buf = Spi.transfer(tx_buf)
         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