Select Git revision
CONTRIBUTING.md
After you've reviewed these contribution guidelines, you'll be all set to
contribute to this project.
motor.py 4.65 KiB
"""
File: motor.py
Description: Provides an interface with the motor of the robot
"""
import time
from enum import IntEnum
import RPi.GPIO as GPIO
from pins import RaspiPin
class MotorError(Exception):
'''
Deals with error related with the motor
'''
class MotorDirection(IntEnum):
"""
Possible direction the motor can take
"""
UNDEFINED = -1
FORWARD = 0
BACKWARD = 1
class Motor:
"""
Interfaces the right pins with the two motors
"""
MOTOR_DELAY = 0.2
FULL_SPEED = 1.
STOP = 0.
def __init__(self, battery_voltage=9.0, motor_voltage=6.0):
if float(motor_voltage) / float(battery_voltage) > 1:
raise MotorError("Motor voltage is higher than battery votage.")
self.pwm_scale = float(motor_voltage) / float(battery_voltage)
GPIO.setup(RaspiPin.LEFT_PWM_PIN, GPIO.OUT)
self.left_pwm = GPIO.PWM(RaspiPin.LEFT_PWM_PIN, 500)
self.left_pwm.start(0)
GPIO.setup(RaspiPin.LEFT_1_PIN, GPIO.OUT)
GPIO.setup(RaspiPin.LEFT_2_PIN, GPIO.OUT)
GPIO.setup(RaspiPin.RIGHT_PWM_PIN, GPIO.OUT)
self.right_pwm = GPIO.PWM(RaspiPin.RIGHT_PWM_PIN, 500)
self.right_pwm.start(0)
GPIO.setup(RaspiPin.RIGHT_1_PIN, GPIO.OUT)
GPIO.setup(RaspiPin.RIGHT_2_PIN, GPIO.OUT)
self.old_left_dir = MotorDirection.UNDEFINED
self.old_right_dir = MotorDirection.UNDEFINED
def _set_motors(self, left_pwm: float, left_dir: MotorDirection,
right_pwm: float, right_dir: MotorDirection):
"""
Sets the modulation and direction of the motors.
If any MotorDirection changes the current one, stops the motor beforehand.
:param float left_pwd Modulation of the left motor
:param MotorDirection left_dir MotorDirection of the left motor
:param float right_pwd Modulation of the left motor
:param MotorDirection right_pwd MotorDirection of the left motor
"""
if self.old_left_dir != left_dir or self.old_right_dir != right_dir:
# stop motors between sudden changes of direction
self._set_driver_pins(self.STOP, MotorDirection.FORWARD, self.STOP,
MotorDirection.FORWARD)
time.sleep(self.MOTOR_DELAY)
self._set_driver_pins(left_pwm, left_dir, right_pwm, right_dir)
self.old_left_dir = left_dir
self.old_right_dir = right_dir
def _set_driver_pins(self, left_pwm: float, left_dir: MotorDirection,
right_pwm: float, right_dir: MotorDirection):
"""
Sets the modulation and direction of the motors.
Checks the modulation and raises a ValueError if they are not in the range [0, 1]
:param float left_pwd Modulation of the left motor
:param MotorDirection left_dir MotorDirection of the left motor
:param float right_pwd Modulation of the left motor
:param MotorDirection right_pwd MotorDirection of the left motor
"""
if left_pwm < 0. or left_pwm > 1. or right_pwm < 0. or right_pwm > 1.:
raise ValueError("Invalid motor speed")
self.left_pwm.ChangeDutyCycle(left_pwm * 100 * self.pwm_scale)
GPIO.output(RaspiPin.LEFT_1_PIN, left_dir)
GPIO.output(RaspiPin.LEFT_2_PIN, not left_dir)
self.right_pwm.ChangeDutyCycle(right_pwm * 100 * self.pwm_scale)
GPIO.output(RaspiPin.RIGHT_1_PIN, right_dir)
GPIO.output(RaspiPin.RIGHT_2_PIN, not right_dir)
def forward(self, speed=0.5):
"""
Set both motors forward
:param float speed: The speed of both motors
"""
self._set_motors(speed, MotorDirection.FORWARD, speed,
MotorDirection.FORWARD)
def backward(self, speed=0.5):
"""
Set both motors backwad
:param float speed: The speed of both motors
"""
self._set_motors(speed, MotorDirection.BACKWARD, speed,
MotorDirection.BACKWARD)
def stop(self):
"""
Stops both motors
"""
self._set_motors(self.STOP, MotorDirection.FORWARD, self.STOP,
MotorDirection.FORWARD)
def right(self, speed=0.5):
"""
Spins the robot clockwise
:param float speed: The speed of both motors
"""
self._set_motors(speed, MotorDirection.FORWARD, speed,
MotorDirection.BACKWARD)
def left(self, speed=0.5):
"""
Spins the robot counter-clockwise
:param float speed: The speed of both motors
"""
self._set_motors(speed, MotorDirection.BACKWARD, speed,
MotorDirection.FORWARD)