def BackwardM(motor, val):
if motor == 1:
return roboclaw.BackwardM1(addr1, val)
elif motor == 2:
return roboclaw.BackwardM2(addr1, val)
elif motor == 3:
return roboclaw.BackwardM1(addr2, val)
else:
raise mlibExcept(e_type.motorNumOff)
def setMotors():
global _targetMotor, _valMotor, ENABLE_MOTORS
#FOR BOTH MOTORS
for i in range(2):
_diff = _targetMotor[i] - _valMotor[i]
if abs(_diff) >= _alpha:
if _diff > 0:
_valMotor[i] = _valMotor[i] + _alpha
else:
_valMotor[i] = _valMotor[i] - _alpha
if ENABLE_MOTORS == True:
if i == 0:
if _valMotor[i] > 0:
roboclaw.ForwardM1(address, _valMotor[i])
else:
roboclaw.BackwardM1(address, abs(_valMotor[i]))
else:
if _valMotor[i] > 0:
roboclaw.BackwardM2(address, _valMotor[i])
else:
roboclaw.ForwardM2(address, abs(_valMotor[i]))
time.sleep(.03)
def Jog_Auton(): global DriveState if(DriveState<>5): roboclaw.BackwardM1 (address, speed) roboclaw.ForwardM2 (address, speed) DriveState=5 global TimeOut TimeOut=10 else: TimeOut=.1
def Jog_Right(): global DriveState if(DriveState<>4): #print "Jog Right" roboclaw.BackwardM1 (address, speed) DriveState=4 global TimeOut TimeOut=.6 else: Timeout=.1
def Forward(): global DriveState if(DriveState<>1): #print "Forwards" roboclaw.BackwardM1 (address, speed) roboclaw.ForwardM2 (address, speed) DriveState=1 global TimeOut TimeOut=.6 else: TimeOut=.1
def drive_wheels(msg):
x = msg.linear.x
z = msg.angular.z
#127 is max speed
m1 = int((x - z) * 127)
m2 = int((x + z) * 127)
print str(m1) + " " + str(m2)
if m1 > 0:
roboclaw.ForwardM1(0x80, m1)
else:
roboclaw.BackwardM1(0x80, abs(m1))
if m2 > 0:
roboclaw.ForwardM2(0x80, m2)
else:
roboclaw.BackwardM2(0x80, abs(m2))
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)
import time
import roboclaw
#Windows comport name
roboclaw.Open("COM3", 115200)
#Linux comport name
#roboclaw.Open("/dev/ttyACM0",115200)
address = 0x80
while (1):
roboclaw.ForwardM1(address, 64)
roboclaw.BackwardM2(address, 64)
time.sleep(2)
roboclaw.BackwardM1(address, 64)
roboclaw.ForwardM2(address, 64)
time.sleep(2)
roboclaw.ForwardBackwardM1(address, 96)
roboclaw.ForwardBackwardM2(address, 32)
time.sleep(2)
roboclaw.ForwardBackwardM1(address, 32)
roboclaw.ForwardBackwardM2(address, 96)
time.sleep(2)
def setMotors(pwm_Knee, pwm_Hip): rc.ForwardM1(config.address, pwm_Knee) if pwm_Knee >= 0 else rc.BackwardM1(config.address, -pwm_Knee) rc.ForwardM2(config.address, pwm_Hip) if pwm_Hip >= 0 else rc.BackwardM2(config.address, -pwm_Hip) return
import time
import roboclaw
#Windows comport name
roboclaw.Open("/dev/cu.usbmodem1411", 115200)
#Linux comport name
#roboclaw.Open("/dev/ttyACM0",115200)
address = 0x82
while (1):
print('Cycle')
roboclaw.BackwardM1(address, 0)
#roboclaw.BackwardM2(address,255)
time.sleep(2)
roboclaw.ForwardM1(address, 0)
#roboclaw.BackwardM2(address,255)
time.sleep(5)
roboclaw.BackwardM1(address, 50)
#roboclaw.ForwardM2(address,255)
time.sleep(2)
#roboclaw.ForwardBackwardM1(address,96)
#roboclaw.ForwardBackwardM2(address,32)
#time.sleep(2)
#roboclaw.ForwardBackwardM1(address,32)
#roboclaw.ForwardBackwardM2(address,96)
#time.sleep(2)
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