def doSetMotor(motor, value, max):
try:
value = int(value * 100 * max)
if motor == "left":
DMCC.setMotor(0, 1, value)
elif motor == "right":
DMCC.setMotor(0, 2, value)
except:
print "Error setting " + motor + " motor, trying again"
# Control of Q1 #-------------------- pos1 = -DMCC.getQEI(0,1)*360/564 - initPos1 #Read encoder error1 = DesTheta[0] - pos1 #Generate the error errorDer1 = (error1 - errorPrev1)/DT #Derivative of error errorPrev1 = error1 controlInput1 = Kp*error1 - Kd*errorDer1 # Upper limit of setMotor parameter is 5000, which corresponds to 100% duty cycle for PWM signal of H-bridge, hence the saturation. if controlInput1 > 5000: controlInput1 = 5000 elif controlInput1 < -5000: controlInput1 = -5000 #DMCC.setMotor(cape number, motor number, power) DMCC.setMotor(0,1,int(controlInput1)) # Command signal to the motor print "Encoder-1: ", pos1, "Error-1: ", error1, "Control-1: ", (int(controlInput1)), "DT: ", DT #-------------------- # Control of Q2 #-------------------- pos2 = -DMCC.getQEI(0,2)*360/564 - initPos2 #Read encoder error2 = DesTheta[1] - pos2 #Generate the error errorDer2 = (error2 - errorPrev2)/DT #Derivative of error errorPrev2 = error2 controlInput2 = Kp*error2 - Kd*errorDer2 # Upper limit of setMotor parameter is 5000, which corresponds to 100% duty cycle for PWM signal of H-bridge, hence the saturation. if controlInput2 > 5000: controlInput2 = 5000 elif controlInput2 < -5000:
import DMCC import time print "Cape 0; Motor 1" DMCC.setMotor(0, 1, 5000) time.sleep(2) DMCC.setMotor(0, 1, 0) print "Cape 0; Motor 2" DMCC.setMotor(0, 2, 5000) time.sleep(2) DMCC.setMotor(0, 2, 0) print "Cape 1; Motor 1" DMCC.setMotor(1, 1, 5000) time.sleep(2) DMCC.setMotor(1, 1, 0) print "Cape 1; Motor 2" DMCC.setMotor(1, 2, 5000) time.sleep(2) DMCC.setMotor(1, 2, 0)
