diff --git a/compLib/Display.py b/compLib/Display.py new file mode 100644 index 0000000..d27b49e --- /dev/null +++ b/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, "") diff --git a/compLib/Encoder.py b/compLib/Encoder.py new file mode 100644 index 0000000..ed61c99 --- /dev/null +++ b/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 diff --git a/compLib/IRSensor.py b/compLib/IRSensor.py index 69463fa..c12e746 100644 --- a/compLib/IRSensor.py +++ b/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 -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) +SENSOR_COUNT = 5 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: - """Get top left infrared sensor percentage + def read(sensor: int) -> int: + if sensor <= 0 or sensor > SENSOR_COUNT: + raise IndexError("Invalid sensor specified!") - :return: Percentage between 0 and 100 - :rtype: int - """ - voltage = adc.read(TOP_LEFT_CHANNEL) - return int((voltage - TOP_IR_MIN_VOLTAGE) / TOP_IR_MAX_VOLTAGE * 100) + if sensor == 1: + return Spi.read(Register.IR_1_H, 2) + elif sensor == 2: + return Spi.read(Register.IR_2_H, 2) + 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: - """Get top right infrared sensor percentage - - :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)] + def set(sensor: int, on: bool): + if sensor <= 0 or sensor > SENSOR_COUNT: + raise IndexError("Invalid sensor specified!") + 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) diff --git a/compLib/IRWrapper.py b/compLib/IRWrapper.py deleted file mode 100644 index dd18206..0000000 --- a/compLib/IRWrapper.py +++ /dev/null @@ -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()) diff --git a/compLib/Motor.py b/compLib/Motor.py index f604936..45acc33 100644 --- a/compLib/Motor.py +++ b/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 - if port == 1: - forward = not forward + pwm = percent * MOTOR_PERCENTAGE_MULT - adjusted_speed = int(min(max(0.0, percent), 100.0) * MOTOR_PERCENTAGE_MULT) - - 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) + Motor.pwm(port, int(pwm), mode) @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): - Motor.power(i, 0) + for i in range(1, MOTOR_COUNT + 1): + 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) diff --git a/compLib/Spi.py b/compLib/Spi.py index 227d198..a458e81 100644 --- a/compLib/Spi.py +++ b/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) \ No newline at end of file + 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