226 lines
7.2 KiB
Python
226 lines
7.2 KiB
Python
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)
|