59 lines
1.5 KiB
Python
59 lines
1.5 KiB
Python
import atexit
|
|
|
|
from compLib.PCA9685 import PCA9685
|
|
from compLib.LogstashLogging import Logging
|
|
|
|
pwm = PCA9685(0x40, debug=True)
|
|
pwm.setPWMFreq(50)
|
|
|
|
MOTOR_COUNT = 4
|
|
MAX_MOTOR_SPEED = 4095.0
|
|
MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0
|
|
|
|
|
|
class Motor(object):
|
|
"""Class used to control the motors
|
|
"""
|
|
|
|
@staticmethod
|
|
def power(port: int, percent: int):
|
|
"""Set specified motor to percentage power
|
|
|
|
:param port: Port, which the motor is connected to. 0-3, 0 -> top left, 3 -> top right
|
|
:param percent: Percentage of max speed. between -100 and 100
|
|
"""
|
|
Logging.get_logger().debug(f"Motor.power {port} {percent}")
|
|
|
|
if port < 0 or port >= MOTOR_COUNT:
|
|
raise IndexError("Invalid Motor port specified!")
|
|
|
|
forward = True
|
|
if percent < 0:
|
|
percent = abs(percent)
|
|
forward = False
|
|
|
|
# bottom left motor is inverted - REEEEEEEEEEEE
|
|
if port == 1:
|
|
forward = not forward
|
|
|
|
adjusted_speed = int(min(max(0, percent), 100) * MOTOR_PERCENTAGE_MULT)
|
|
|
|
if forward:
|
|
pwm.setMotorPwm(port * 2, 0)
|
|
pwm.setMotorPwm(port * 2 + 1, adjusted_speed)
|
|
else:
|
|
pwm.setMotorPwm(port * 2, adjusted_speed)
|
|
pwm.setMotorPwm(port * 2 + 1, 0)
|
|
|
|
@staticmethod
|
|
def all_off():
|
|
"""
|
|
Turns of all motors
|
|
"""
|
|
Logging.get_logger().debug(f"Motor.all_off")
|
|
|
|
for i in range(0, MOTOR_COUNT):
|
|
Motor.power(i, 0)
|
|
|
|
|
|
atexit.register(Motor.all_off)
|