95 lines
2.7 KiB
Python
95 lines
2.7 KiB
Python
import atexit
|
|
from enum import Enum
|
|
|
|
from compLib.LogstashLogging import Logging
|
|
from compLib.Spi import Spi, Register
|
|
|
|
MOTOR_COUNT = 4
|
|
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. 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:
|
|
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!")
|
|
|
|
mode = MotorMode.COAST
|
|
if percent < 0:
|
|
percent = abs(percent)
|
|
mode = MotorMode.BACKWARD
|
|
elif percent > 0:
|
|
mode = MotorMode.FORWARD
|
|
|
|
pwm = percent * MOTOR_PERCENTAGE_MULT
|
|
|
|
Motor.pwm(port, int(pwm), mode)
|
|
|
|
@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)
|
|
|
|
|
|
atexit.register(Motor.all_off)
|