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 Stop(): global DriveState if(DriveState<>0): roboclaw.ForwardM1(address, 0) roboclaw.ForwardM2(address, 0) print "Stop" DriveState=0
def ForwardM(motor, val):
if motor == 1:
return roboclaw.ForwardM1(addr1, val)
elif motor == 2:
return roboclaw.ForwardM2(addr1, val)
elif motor == 3:
return roboclaw.ForwardM1(addr2, val)
else:
raise mlibExcept(e_type.motorNumOff)
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_Left(): global DriveState if(DriveState<>3): #print "Jog Left" roboclaw.ForwardM2 (address, speed) DriveState=3 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 neutral_state():
print('')
rc.ForwardM1(address, 0)
rc.ForwardM2(address, 0)
print(
'NEUTRAL: Apparatus is set to neutral and awaiting feedback to transition to primed state.'
)
print(
'Press \'M\' to go to the main menu, or \'R\' to restart falling scheme.'
)
var = input('Press any other key and <ENTER> to move into primed state.\n')
if var == 'M':
return 9
elif var == 'R':
return 8
else:
return 0
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)
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
#!/usr/bin/python import roboclaw roboclaw.ForwardM1(0x80, 0) roboclaw.ForwardM1(0x81, 0) roboclaw.ForwardM2(0x80, 0)
#!/usr/bin/env python
import roboclaw as r
r.Open('/dev/ttySAC0', 38400)
r.ForwardM1(128,0)
r.ForwardM2(128,0)
r.ForwardM1(129,0)
#r.SpeedM1M2(128,0,0)
#r.SpeedM1(129,0)
def uninit_kick(): roboclaw.ForwardM2(roboclaw.addr2, 0)
def init_kick(): # Hold the kicker back roboclaw.ForwardM2(roboclaw.addr2,127) # max power
index = 0
# Beginning of fall to end of fall all within 1 second, else cut motors
while time.clock-beginning < 1:
targets = instructions.targets(index)
SeekNext(targets)
# every <timing> loops increment index to seek the next target in the list
count = count+1
index = count%timing
else:
rc.ForwardM1(address,0)
rc.ForwardM2(address,0)
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)
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
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 kick():
roboclaw.ForwardM2(addr2, 127) # max power
time.sleep(.1)
return roboclaw.ForwardM2(addr2, 0)
