#PiRoCon - 4Tronix Initio - Motor Controller #Martin O'Hanlon #www.stuffaboutcode.com
import sys import time import RPi.GPIO as GPIO
#motor pins MOTORAFWRDPIN = 19 MOTORABWRDPIN = 21 MOTORBFWRDPIN = 24 MOTORBBWRDPIN = 26 #encoder pins MOTORAENCODERPIN = 7 MOTORBENCODERPIN = 11
#motor states STATEFORWARD = 1 STATESTOPPED = 0 STATEBACKWARD = -1
class MotorController: def init(self, motorAForwardPin = MOTORAFWRDPIN, motorABackwardPin = MOTORABWRDPIN, motorBForwardPin = MOTORBFWRDPIN, motorBBackwardPin = MOTORBBWRDPIN, motorAEncoderPin = MOTORAENCODERPIN, motorBEncoderPin = MOTORBENCODERPIN,):
#setup motor classes
self.motorA = Motor(motorAForwardPin, motorABackwardPin, motorAEncoderPin)
self.motorB = Motor(motorBForwardPin, motorBBackwardPin, motorBEncoderPin)
#motor properties
@property
def motorA(self):
return self.motorA
@property
def motorB(self):
return self.motorB
#start
def start(self, powerA, powerB = None):
#if a second power isnt passed in, both motors are set the same
if powerB == None: powerB = powerA
self.motorA.start(powerA)
self.motorB.start(powerB)
#stop
def stop(self):
self.motorA.stop()
self.motorB.stop()
#rotate left
def rotateLeft(self, power):
self.motorA.start(power * -1)
self.motorB.start(power)
#rotate right
def rotateRight(self, power):
self.motorA.start(power)
self.motorB.start(power * -1)
#class for controlling a motor class Motor: def init(self, forwardPin, backwardPin, encoderPin): #persist values self.forwardPin = forwardPin self.backwardPin = backwardPin self.encoderPin = encoderPin
#setup GPIO pins
GPIO.setup(forwardPin, GPIO.OUT)
GPIO.setup(backwardPin, GPIO.OUT)
GPIO.setup(encoderPin,GPIO.IN,pull_up_down=GPIO.PUD_DOWN)
#add encoder pin event
GPIO.add_event_detect(encoderPin, GPIO.RISING, callback=self._encoderCallback,bouncetime=2)
#setup pwm
self.forwardPWM = GPIO.PWM(forwardPin,50)
self.backwardPWM = GPIO.PWM(backwardPin,50)
#setup encoder/speed ticks
self.totalTicks = 0
self.currentTicks = 0
self.state = STATESTOPPED
#motor state property
@property
def state(self):
return self.state
#start
def start(self, power):
#forward or backward?
# backward
if power < 0:
if self.state != STATEBACKWARD: self.stop()
self._backward(power)
self.state = STATEBACKWARD
# forward
if power > 0:
if self.state != STATEFORWARD: self.stop()
self._forward(power)
self.state = STATEFORWARD
# stop
if power == 0:
self.stop()
#stop
def stop(self):
#stop motor
self.forwardPWM.stop()
self.backwardPWM.stop()
self.state = STATESTOPPED
#reset ticks
self.currentTicks = 0
#private function to calculate the freq for the PWM
def _calcPowerAndFreq(self, power):
# make power between 0 and 100
power = max(0,min(100,abs(power)))
# make half of freq, minimum of 11
freq = max(11,abs(power/2))
return power, freq
#forward
def _forward(self, power):
#start forward motor
power, freq = self._calcPowerAndFreq(power)
self.forwardPWM.ChangeFrequency(freq)
self.forwardPWM.start(power)
#backward
def _backward(self, power):
#start backward motor
power, freq = self._calcPowerAndFreq(power)
self.backwardPWM.ChangeFrequency(freq)
self.backwardPWM.start(power)
#encoder callback
def _encoderCallback(self, pin):
self.totalTicks += 1
self.currentTicks += 1
#tests if name == 'main':
try:
#setup gpio
GPIO.setmode(GPIO.BOARD)
#create motor control
motors = MotorController()
#forward
print("forward")
motors.start(100)
time.sleep(2)
print("encoder ticks")
print(motors.motorA.totalTicks)
print(motors.motorB.totalTicks)
#backward
print("backward")
motors.start(-50)
time.sleep(2)
#forward curve
print("forward curve")
motors.start(100,50)
time.sleep(2)
#rotate left
print("rotate left")
motors.rotateLeft(50)
time.sleep(2)
#rotate right
print("rotate right")
motors.rotateRight(50)
time.sleep(2)
#stop
print("stop")
motors.stop()
#Ctrl C
except KeyboardInterrupt:
print "User cancelled"
#Error
except:
print "Unexpected error:", sys.exc_info()[0]
raise
finally:
print ("cleanup")
#cleanup gpio
GPIO.cleanup()