def initialize():
""" Initializes xjus and pygame """
pygame.font.init()
print "Opening connection to device..."
xjus.openDevices()
print "Clearing faults and enabling nodes..."
for node in nodes:
xjus.clearFault(node)
errorCode = xjus.getErrorCode()
if (errorCode is 872415239) or (errorCode is 10000003):
pygame.quit()
raise Exception("No connection to device!")
if (errorCode == 34000007):
pygame.quit()
raise Exception("Turn on motors!")
xjus.clearIpmBuffer(node)
xjus.setMaxFollowingError(node, FOLLOWING_ERROR)
xjus.setMaxVelocity(node, MAX_VELOCITY)
xjus.setMaxAcceleration(node, MAX_ACCELERATION)
err = xjus.getMaxFollowingError(node)
vel = xjus.getMaxVelocity(node)
acc = xjus.getMaxAcceleration(node)
print("node: %d, following error: %d, max velocity: %d, max acceleration: %d" % (node, err, vel, acc))
xjus.enable(node)
errorCode = xjus.getErrorCode()
if (errorCode == 34000007):
pygame.quit()
raise Exception("Turn on motors!")
print "Ready for action!"
# get PID | node: 1, pP: 136, pI: 322, pD: 300, fV: 0, fA: 105
# get PID | node: 2, pP: 122, pI: 277, pD: 282, fV: 0, fA: 95
# get PID | node: 3, pP: 124, pI: 284, pD: 285, fV: 0, fA: 87
# get PID | node: 4, pP: 110, pI: 273, pD: 231, fV: 0, fA: 83
# get PID | node: 5, pP: 118, pI: 274, pD: 265, fV: 0, fA: 90
# get PID | node: 6, pP: 116, pI: 287, pD: 250, fV: 1703, fA: 99
for node in nodes:
xjus.setPositionRegulatorGain(node, P_GAIN, I_GAIN, D_GAIN)
xjus.setPositionRegulatorFeedForward(node, FEEDFORWARD_VELOCITY, FEEDFORWARD_ACCELERATION)
pP = xjus.getPositionRegulatorGain(node, 1)
pI = xjus.getPositionRegulatorGain(node, 2)
pD = xjus.getPositionRegulatorGain(node, 3)
fV = xjus.getPositionRegulatorFeedForward(node, 1)
fA = xjus.getPositionRegulatorFeedForward(node, 2)
print("pP: %d, pI: %d, pD: %d, fV: %d, fA: %d" % (pP, pI, pD, fV, fA))
def mainLoop(clock, surface):
"""
Represents the main control loop, where key events are
processed and high-level routines activated.
"""
global walking, tapMode, tapModeBack, turnLeft, turnRight
global T, GROUND_ANGLE
# IPM time variable
t = 0
frame = 0
while True:
timer0 = time()
frame += 1
print("--------- Main loop frame %d ---------- error code %d" %
(frame, xjus.getErrorCode()))
# Stops the program if there is a node in fault state
if (frame % 10) == 0:
timer = time()
if nodeFault():
print("Error occurred! Error code: %d" % xjus.getErrorCode())
for node in nodes:
xjus.printIpmStatus(node)
return
print("nodeFault() call: %f" % (time() - timer))
# Processing all events for the frame
for event in pygame.event.get():
# Key down events
if event.type == KEYDOWN:
if event.key == (K_EQUALS):
Tnew = T + 0.05
t = (Tnew / T) * t
T = Tnew
if event.key == (K_MINUS):
Tnew = T - 0.05
t = (Tnew / T) * t
T = Tnew
if event.key == (K_RIGHTBRACKET):
GROUND_ANGLE += 5
if event.key == (K_LEFTBRACKET):
GROUND_ANGLE -= 5
# Tooggle stand on spacebar
if event.key == K_SPACE:
if standing and not walking:
sit()
elif not walking:
stand()
else:
tapMode = False
tapModeBack = False
# Toggle continuous walking
elif (event.key is K_w) and (tapMode is False):
if standing and not walking:
walking = True
turnLeft = False
turnRight = False
t = startTripod()
else:
print "Must stand first!"
elif (event.key is K_s) and (tapModeBack is False):
if standing and not walking:
walking = True
turnLeft = False
turnRight = False
t = startTripod(back=True)
if (event.key is K_w):
tapMode = not tapMode
if (event.key is K_s):
tapModeBack = not tapModeBack
if (event.key is K_a):
turnLeft = not turnLeft
turnRight = False
if (event.key is K_d):
turnLeft = False
turnRight = not turnRight
if (event.key is K_t) and not walking:
T = float(raw_input('New movement period: '))
if (event.key is K_g) and not walking:
GROUND_ANGLE = float(raw_input('New ground angle: '))
# Exit on escape
if event.key == K_ESCAPE:
return
# Quit event, clicking the X
if event.type == QUIT:
return
#print("walking: %r, up key down: %r" % (walking, keyDown(K_UP)))
if walking:
# Get the turn angle
turnFraction = 0
duty_turn = 0
if turnRight:
turnFraction = +TURN_FRACTION
duty_turn = +DUTY_TURN_FRACTION
elif turnLeft:
turnFraction = -TURN_FRACTION
duty_turn = -DUTY_TURN_FRACTION
if tapMode:
timer = time()
t = tripodFrame(t,
turnFraction * GROUND_ANGLE,
duty_turn=duty_turn)
print("tripodFrame() call: %f" % (time() - timer))
elif tapModeBack:
t = tripodFrame(t,
turnFraction * BACK_GROUND_ANGLE,
back=True,
duty_turn=duty_turn)
else:
walking = False
turnLeft = False
turnRight = False
stopTripod(t, turnFraction * GROUND_ANGLE, duty_turn=duty_turn)
if ((frame % 7) == 0):
timer = time()
# Drawing
screen.fill(WHITE)
renderText("t = %.2f" % t, -80, 60)
renderText("T = %.2f" % T, -80, 90)
renderText("DC = %.2f" % DUTY_CYCLE, -80, 120)
renderText("GA = %.1f" % GROUND_ANGLE, -80, 150)
if standing and not walking:
stateText = "Standing up."
elif walking and tapMode:
stateText = "Walking forward."
elif walking and tapModeBack:
stateText = "Walking backward."
elif not standing:
stateText = "Lying down."
renderText(stateText, 0, -100, size=40)
#print("Time of drawing text: %f" % (time() - timer))
# Pygame frame
pygame.display.update()
clock.tick(FPS)
print("Total frame time: %fs" % (time() - timer0))
def initialize():
""" Initializes xjus and pygame """
pygame.font.init()
print "Opening connection to device..."
xjus.openDevices()
print "Clearing faults and enabling nodes..."
for node in nodes:
xjus.clearFault(node)
errorCode = xjus.getErrorCode()
if (errorCode is 872415239) or (errorCode is 10000003):
pygame.quit()
raise Exception("No connection to device!")
if (errorCode == 34000007):
pygame.quit()
raise Exception("Turn on motors!")
xjus.clearIpmBuffer(node)
xjus.setMaxFollowingError(node, FOLLOWING_ERROR)
xjus.setMaxVelocity(node, MAX_VELOCITY)
xjus.setMaxAcceleration(node, MAX_ACCELERATION)
err = xjus.getMaxFollowingError(node)
vel = xjus.getMaxVelocity(node)
acc = xjus.getMaxAcceleration(node)
print(
"node: %d, following error: %d, max velocity: %d, max acceleration: %d"
% (node, err, vel, acc))
xjus.enable(node)
errorCode = xjus.getErrorCode()
if (errorCode == 34000007):
pygame.quit()
raise Exception("Turn on motors!")
print "Ready for action!"
# get PID | node: 1, pP: 136, pI: 322, pD: 300, fV: 0, fA: 105
# get PID | node: 2, pP: 122, pI: 277, pD: 282, fV: 0, fA: 95
# get PID | node: 3, pP: 124, pI: 284, pD: 285, fV: 0, fA: 87
# get PID | node: 4, pP: 110, pI: 273, pD: 231, fV: 0, fA: 83
# get PID | node: 5, pP: 118, pI: 274, pD: 265, fV: 0, fA: 90
# get PID | node: 6, pP: 116, pI: 287, pD: 250, fV: 1703, fA: 99
for node in nodes:
xjus.setPositionRegulatorGain(node, P_GAIN, I_GAIN, D_GAIN)
xjus.setPositionRegulatorFeedForward(node, FEEDFORWARD_VELOCITY,
FEEDFORWARD_ACCELERATION)
pP = xjus.getPositionRegulatorGain(node, 1)
pI = xjus.getPositionRegulatorGain(node, 2)
pD = xjus.getPositionRegulatorGain(node, 3)
fV = xjus.getPositionRegulatorFeedForward(node, 1)
fA = xjus.getPositionRegulatorFeedForward(node, 2)
print("pP: %d, pI: %d, pD: %d, fV: %d, fA: %d" % (pP, pI, pD, fV, fA))
def mainLoop(clock, surface):
"""
Represents the main control loop, where key events are
processed and high-level routines activated.
"""
global walking, tapMode, tapModeBack, turnLeft, turnRight
global T, GROUND_ANGLE
# IPM time variable
t = 0
frame = 0
while True:
timer0 = time()
frame += 1
print("--------- Main loop frame %d ---------- error code %d" % (frame, xjus.getErrorCode()))
# Stops the program if there is a node in fault state
if (frame % 10) == 0:
timer = time()
if nodeFault():
print("Error occurred! Error code: %d" % xjus.getErrorCode())
for node in nodes:
xjus.printIpmStatus(node)
return
print("nodeFault() call: %f" % (time()-timer))
# Processing all events for the frame
for event in pygame.event.get():
# Key down events
if event.type == KEYDOWN:
if event.key == (K_EQUALS):
Tnew = T + 0.05
t = (Tnew/T) * t
T = Tnew
if event.key == (K_MINUS):
Tnew = T - 0.05
t = (Tnew/T) * t
T = Tnew
if event.key == (K_RIGHTBRACKET):
GROUND_ANGLE += 5
if event.key == (K_LEFTBRACKET):
GROUND_ANGLE -= 5
# Tooggle stand on spacebar
if event.key == K_SPACE:
if standing and not walking:
sit()
elif not walking:
stand()
else:
tapMode = False
tapModeBack = False
# Toggle continuous walking
elif (event.key is K_w) and (tapMode is False):
if standing and not walking:
walking = True
turnLeft = False
turnRight = False
t = startTripod()
else:
print "Must stand first!"
elif (event.key is K_s) and (tapModeBack is False):
if standing and not walking:
walking = True
turnLeft = False
turnRight = False
t = startTripod(back=True)
if (event.key is K_w):
tapMode = not tapMode
if (event.key is K_s):
tapModeBack = not tapModeBack
if (event.key is K_a):
turnLeft = not turnLeft
turnRight = False
if (event.key is K_d):
turnLeft = False
turnRight = not turnRight
if (event.key is K_t) and not walking:
T = float(raw_input('New movement period: '))
if (event.key is K_g) and not walking:
GROUND_ANGLE = float(raw_input('New ground angle: '))
# Exit on escape
if event.key == K_ESCAPE:
return
# Quit event, clicking the X
if event.type == QUIT:
return
#print("walking: %r, up key down: %r" % (walking, keyDown(K_UP)))
if walking:
# Get the turn angle
turnFraction = 0
duty_turn = 0
if turnRight:
turnFraction = +TURN_FRACTION
duty_turn = +DUTY_TURN_FRACTION
elif turnLeft:
turnFraction = -TURN_FRACTION
duty_turn = -DUTY_TURN_FRACTION
if tapMode:
timer = time()
t = tripodFrame(t, turnFraction * GROUND_ANGLE, duty_turn=duty_turn)
print("tripodFrame() call: %f" % (time()-timer))
elif tapModeBack:
t = tripodFrame(t, turnFraction * BACK_GROUND_ANGLE, back=True, duty_turn=duty_turn)
else:
walking = False
turnLeft = False
turnRight = False
stopTripod(t, turnFraction * GROUND_ANGLE, duty_turn=duty_turn)
if ((frame % 7) == 0):
timer = time()
# Drawing
screen.fill(WHITE)
renderText("t = %.2f" % t, -80, 60)
renderText("T = %.2f" % T, -80, 90)
renderText("DC = %.2f" % DUTY_CYCLE, -80, 120)
renderText("GA = %.1f" % GROUND_ANGLE, -80, 150)
if standing and not walking:
stateText = "Standing up."
elif walking and tapMode:
stateText = "Walking forward."
elif walking and tapModeBack:
stateText = "Walking backward."
elif not standing:
stateText = "Lying down."
renderText(stateText, 0, -100, size=40)
#print("Time of drawing text: %f" % (time() - timer))
# Pygame frame
pygame.display.update()
clock.tick(FPS)
print("Total frame time: %fs" % (time() - timer0))
