Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
Python Motor Control Class for the Seeed Raspberry Pi Motor Driver - see http://reefwingrobotics.blogspot.com.au/
#!/usr/bin/python
# RS_MotorControl.py - Motor Control Class for the Seeed Raspberry Pi Motor Driver
# Board v1.0 which uses the Freescale MC33932 dual H-Bridge Power IC.
#
# Based on Seeed Motor() Class
# ref: http://wiki.seeed.cc/Raspberry_Pi_Motor_Driver_Board_v1.0/
#
# 01 March 2017 - 1.0 Original Issue
# 09 March 2017 - 1.1 forward, back, left, right and spin methods added
#
# Reefwing Software
# Simplified BSD Licence - see bottom of file.
import RPi.GPIO as GPIO
import os, signal
from time import sleep
from enum import Enum, unique
from PiSoftPwm import *
@unique
class MotorState(Enum):
INIT = 1
STOPPED = 2
LEFT_FWD = 3
RIGHT_FWD = 4
BOTH_FWD = 5
LEFT_BACK = 6
RIGHT_BACK = 7
BOTH_BACK = 8
CHANGE_SPEED = 9
class MotorControl():
def __init__(self, base_time=0.01, duty=50):
# MC33932 pins connected to GPIO
self.PWMA = 25
self.PWMB = 22
self._IN1 = 23
self._IN2 = 24
self._IN3 = 17
self._IN4 = 27
self.base_time = base_time
self.duty = duty
self.history = [MotorState.INIT]
# Initialize PWMA & PWMB
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.PWMA, GPIO.OUT)
GPIO.setup(self.PWMB, GPIO.OUT)
GPIO.output(self.PWMA, True)
GPIO.output(self.PWMB, True)
# Initialize Software PWM outputs
# Left Motor = OUT_1 and OUT_2
# Right Motor = OUT_3 and OUT_4
self.OUT_1 = PiSoftPwm(self.base_time, 100, self._IN1, GPIO.BCM)
self.OUT_2 = PiSoftPwm(self.base_time, 100, self._IN2, GPIO.BCM)
self.OUT_3 = PiSoftPwm(self.base_time, 100, self._IN3, GPIO.BCM)
self.OUT_4 = PiSoftPwm(self.base_time, 100, self._IN4, GPIO.BCM)
# Start PWM for outputs - nbSlicesOn = 0, i.e. duty cycle = 0
self.OUT_1.start(0)
self.OUT_2.start(0)
self.OUT_3.start(0)
self.OUT_4.start(0)
def __str__(self):
# Return string representation of motor control
return "Motor Control: base time - {0} seconds, duty - {1}%".format(self.base_time, self.duty)
def left_back(self):
self.OUT_1.changeBaseTime(self.base_time)
self.OUT_2.changeBaseTime(self.base_time)
self.OUT_1.changeNbSlicesOn(self.duty)
self.OUT_2.changeNbSlicesOn(0)
self.history.append(MotorState.LEFT_BACK)
def left_forward(self):
self.OUT_1.changeBaseTime(self.base_time)
self.OUT_2.changeBaseTime(self.base_time)
self.OUT_1.changeNbSlicesOn(0)
self.OUT_2.changeNbSlicesOn(self.duty)
self.history.append(MotorState.LEFT_FWD)
def right_back(self):
self.OUT_3.changeBaseTime(self.base_time)
self.OUT_4.changeBaseTime(self.base_time)
self.OUT_3.changeNbSlicesOn(0)
self.OUT_4.changeNbSlicesOn(self.duty)
self.history.append(MotorState.RIGHT_BACK)
def right_forward(self):
self.OUT_3.changeBaseTime(self.base_time)
self.OUT_4.changeBaseTime(self.base_time)
self.OUT_3.changeNbSlicesOn(self.duty)
self.OUT_4.changeNbSlicesOn(0)
self.history.append(MotorState.RIGHT_FWD)
def forward(self):
self.left_forward()
self.right_forward()
def back(self):
self.left_back()
self.right_back()
def left(self):
self.stop()
self.right_forward()
def right(self):
self.stop()
self.left_forward()
def spin_left(self):
self.stop()
self.right_forward()
self.left_back()
def spin_right(self):
self.stop()
self.left_forward()
self.right_back()
def speed(self, duty):
# Change motor speed to duty (0-100) if not stopped (0)
self.duty = duty
self.OUT_1.nbSlicesOn = duty if self.OUT_1.nbSlicesOn else 0
self.OUT_2.nbSlicesOn = duty if self.OUT_2.nbSlicesOn else 0
self.OUT_3.nbSlicesOn = duty if self.OUT_3.nbSlicesOn else 0
self.OUT_4.nbSlicesOn = duty if self.OUT_4.nbSlicesOn else 0
self.history.append(MotorState.CHANGE_SPEED)
def faster(self):
new_dc = self.clamp(self.duty+10, 0, 100)
self.speed(new_dc)
def slower(self):
new_dc = self.clamp(self.duty-10, 0, 100)
self.speed(new_dc)
def stop(self):
self.OUT_1.changeNbSlicesOn(0)
self.OUT_2.changeNbSlicesOn(0)
self.OUT_3.changeNbSlicesOn(0)
self.OUT_4.changeNbSlicesOn(0)
self.history.append(MotorState.STOPPED)
def cleanup(self):
# Stop PWM on all outputs
self.OUT_1.stop()
self.OUT_2.stop()
self.OUT_3.stop()
self.OUT_4.stop()
@staticmethod
def clamp(dc, min_dc, max_dc):
return max(min(dc, max_dc), min_dc)
def main():
motor_control = MotorControl() # create a new motor control instance
print(motor_control)
def endProcess(signum = None, frame = None):
# Called on process termination. Stop motor control PWM
if signum is not None:
SIGNALS_NAMES_DICT = dict((getattr(signal, n), n) for n in dir(signal) if n.startswith('SIG') and '_' not in n )
print("signal {} received by process with PID {}".format(SIGNALS_NAMES_DICT[signum], os.getpid()))
print("\n-- Terminating program --")
print("Cleaning up motor control PWM and GPIO...")
motor_control.cleanup()
GPIO.cleanup()
print("Done.")
exit(0)
# Assign handler for process exit
signal.signal(signal.SIGTERM, endProcess)
signal.signal(signal.SIGINT, endProcess)
signal.signal(signal.SIGHUP, endProcess)
signal.signal(signal.SIGQUIT, endProcess)
while True:
print('Testing motors...')
motor_control.left_forward()
sleep(1)
motor_control.left_back()
sleep(1)
motor_control.right_forward()
sleep(1)
motor_control.right_back()
sleep(1)
# speed = int(input("Enter Speed (0-100, CTRL c to quit): "))
# motor_control.speed(speed)
if __name__ == "__main__":
# execute only if run as a script
main()
## Copyright (c) 2017, Reefwing Software
## All rights reserved.
##
## Redistribution and use in source and binary forms, with or without
## modification, are permitted provided that the following conditions are met:
##
## 1. Redistributions of source code must retain the above copyright notice, this
## list of conditions and the following disclaimer.
## 2. Redistributions in binary form must reproduce the above copyright notice,
## this list of conditions and the following disclaimer in the documentation
## and/or other materials provided with the distribution.
##
## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
## ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
## WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
## DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
## ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
## (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
## LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
## ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
## (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
## SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment