def readSpeedM(motor):
if motor == 1:
return roboclaw.ReadSpeedM1(addr1)
elif motor == 2:
return roboclaw.ReadSpeedM2(addr1)
elif motor == 3:
return roboclaw.ReadSpeedM1(addr2)
else:
raise mlibExcept(e_type.motorNumOff)
def displayspeed():
enc1 = roboclaw.ReadEncM1(address)
enc2 = roboclaw.ReadEncM2(address)
speed1 = roboclaw.ReadSpeedM1(address)
speed2 = roboclaw.ReadSpeedM2(address)
print("Encoder1:"),
if (enc1[0] == 1):
print enc1[1],
print format(enc1[2], '02x'),
else:
print "failed",
print "Encoder2:",
if (enc2[0] == 1):
print enc2[1],
print format(enc2[2], '02x'),
else:
print "failed ",
print "Speed1:",
if (speed1[0]):
print speed1[1],
else:
print "failed",
print("Speed2:"),
if (speed2[0]):
print speed2[1]
else:
print "failed "
def SeekNext(t):
# Get speeds and currents
(a,cur1,cur2) = rc.ReadCurrents(address)y
w1 = rc.ReadSpeedM1(address)
w2 = rc.ReadSpeedM2(address)
if(a):
# Find increments to PWM
thisError1 = target1-cur1
increment1 = _kp*(thisError1)+_kd*(thisError1-lastError1)+_kw*(w1)
thisError2 = target2-cur2
increment2 = _kp*(thisError2)+_kd*(thisError2-lastError2)+_kw*(w2)
# Update errors
lastError1 = thisError1
lastError2 = thisError2
PWM1 = PWM1 + increment1
PWM2 = PWM2 + increment2
if PWM1 > 255:
PWM1 = 255
if PWM1 < -255:
PWM1 = -255
if PWM2 > 255:
PWM2 = 255
if PWM2 < -255:
PWM2 = -255
if PWM1 >= 0:
rc.ForwardM1(address,PWM1)
else:
rc.BackwardM1(address,-PWM1)
if PWM2 >= 0:
rc.ForwardM2(address,PWM2)
else:
rc.BackwardM2(address,-PWM2)
def falling():
print('')
print('FALLING: Here I go falling!')
beginning = time.clock()
count = 0
index = 0
lastError1 = 0
lastError2 = 0
pwm1 = 0
pwm2 = 0
while time.clock() - beginning < 0.8:
target1, target2 = targetsList(index)
(a, cur1, cur2) = rc.ReadCurrents(address)
w1 = rc.ReadSpeedM1(address)
w2 = rc.ReadSpeedM2(address)
# TODO: Save all fields collected during control loop
if (a):
# Find increments to PWM
thisError1 = target1 - cur1
increment1 = kp * thisError1 + kd * (thisError1 -
lastError1) + kw * w1
thisError2 = target2 - cur2
increment2 = kp * thisError2 + kd * (thisError2 -
lastError2) + kw * w2
# Update errors
lastError1 = thisError1
lastError2 = thisError2
pwm1 = +increment1
pwm2 = +increment2
if pwm1 > 255:
pwm1 = 255
if pwm1 < -255:
pwm1 = -255
if pwm2 > 255:
pwm2 = 255
if pwm2 < -255:
pwm2 = -255
if pwm1 >= 0:
rc.ForwardM1(address, pwm1)
else:
rc.BackwardM1(address, -pwm1)
if pwm2 >= 0:
rc.ForwardM2(address, pwm2)
else:
rc.BackwardM2(address, -pwm2)
count = +1
index = count % timing
if index > numberOfTargets:
index = numberOfTargets
else:
rc.ForwardM1(address, 0)
rc.ForwardM2(address, 0)
return [0] * 5
