import atexit from enum import IntEnum from compLib.LogstashLogging import Logging from compLib.MetricsLogging import MetricsLogging from compLib.Spi import Spi, Register MOTOR_COUNT = 4 MAX_MOTOR_SPEED = 65535 MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.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 class MotorMode(IntEnum): 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 SPEED_LOCK: return 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.PWM_1_CTRL, 1, int(mode)) elif port == 2: Spi.write(Register.MOTOR_2_PWM_H, 2, pwm) Spi.write(Register.PWM_2_CTRL, 1, int(mode)) elif port == 3: Spi.write(Register.MOTOR_3_PWM_H, 2, pwm) Spi.write(Register.PWM_3_CTRL, 1, int(mode)) elif port == 4: Spi.write(Register.MOTOR_4_PWM_H, 2, pwm) Spi.write(Register.PWM_4_CTRL, 1, int(mode)) @staticmethod def power_raw(port: int, percent: float, log_metric = True): """Set specified motor to percentage power :param port: Port, which the motor is connected to. 1-4 :param percent: Percentage of max speed. between -100 and 100 :raises: IndexError """ 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!") if log_metric: MetricsLogging.put("MotorRaw", float(percent), port) mode = MotorMode.COAST if percent < 0: percent = abs(percent) mode = MotorMode.BACKWARD elif percent > 0: mode = MotorMode.FORWARD percent *= SPEED_MULT pwm = percent * MOTOR_PERCENTAGE_MULT Motor.pwm(port, int(pwm), mode) @staticmethod def power(port: int, percent: float): """Set specified motor to percentage power, percentage is linearized so that it's roughly proportional to the speed :param port: Port, which the motor is connected to. 1-4 :param percent: Percentage of max speed. between -100 and 100 :raises: IndexError """ raw_power = raw_power = Motor.__linearizePower(MOTOR_CURVE, abs(percent)) if percent < 0: raw_power *= -1 MetricsLogging.put("Motor", float(percent), port) Motor.power_raw(port, raw_power, False) @staticmethod def all_off(): """ Turns of all motors """ Logging.get_logger().debug(f"Motor.all_off") 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) @staticmethod def set_motor_curve(curve): """ Set the global motor curve, must be a float array with exactly 21 elements. [0] = x ticks/s (0% power) [1] = x ticks/s (5% power) [2] = x ticks/s (10% power) ... [20] = x ticks/s (100% power) :param curve: float array with 21 elements :raises: ValueError """ if (len(curve) != 21): raise ValueError('The motor curve is invalid, check documentation for set_motor_curve()!') global MOTOR_CURVE MOTOR_CURVE = curve @staticmethod def get_motor_curve(): """ Get the currently active motor curve. Check set_motor_curve() for more info. :return: current motor curve """ return MOTOR_CURVE @staticmethod def __map(x, in_min, in_max, out_min, out_max): """ Linear interpolation. Check https://www.arduino.cc/reference/en/language/functions/math/map/ """ x = float(x) in_min = float(in_min) in_max = float(in_max) out_min = float(out_min) out_max = float(out_max) return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min @staticmethod def __interpolateSpeed(curve, speed): """ Interpolate a speed in the specified motor curve and return the 'power percentage that is needed to reach that speed' """ if speed < min(curve) or speed > max(curve): raise ValueError(f'Speed out of range: {str(speed)} ticks/s') for index in range(len(curve) - 1): if speed >= curve[index] and speed <= curve[index + 1] and curve[index] != curve[index + 1]: return Motor.__map(speed, curve[index], curve[index + 1], index * 5, index * 5 + 5) return 0 @staticmethod def __linearizePower(curve, power): """ Takes raw power and returns it corrected so that the motor speed would be roughly linear """ if power < 0 or power > 100: raise ValueError(f'Power out of range: {str(power)}%') requiredSpeed = Motor.__map(power, 0, 100, min(curve), max(curve)) return Motor.__interpolateSpeed(curve, requiredSpeed) @staticmethod def robot_state_loop(): import time from compLib.Api import Seeding global SPEED_LOCK global SPEED_MULT while True: try: state, status = Seeding.get_robot_state() SPEED_MULT = state["speed"] / 100.0 if state["left"] != -1 or state["right"] != -1: Motor.power(1, state["right"]) Motor.power(4, state["left"]) SPEED_LOCK = True time.sleep(state["time"]) SPEED_LOCK = False Motor.power(1, 0) Motor.power(4, 0) else: time.sleep(0.25) except: time.sleep(0.25) pass @staticmethod def start_robot_state_loop(): from threading import Thread robot_state_thread = Thread(target=Motor.robot_state_loop) robot_state_thread.setDaemon(True) robot_state_thread.start() atexit.register(Motor.all_off)