def stopTripod(t, turnAngle=0, back=False, duty_turn=0):
""" Adds an ending point to the tripod gait and returns to a
standing position. """
xjusAnalysis.endAccel(PLOT_ANALYSIS)
xjusAnalysis.endAvgVelocity()
#acc = xjusAnalysis.getAvgAbsZAccel()
#print("===============================================")
#print("Stability measure: %.4f" % (acc))
current = xjusAnalysis.getAvgCurrent()
#print("Power usage measure: %.4f" % (current))
#print("===============================================")
pytime.wait(DT)
for node in nodes:
[nA, pA, vA, tA] = addTripodPoint(t,
turnAngle,
back=back,
end=True,
duty_turn=duty_turn)
addPvtArray(nA, pA, vA, tA)
for node in nodes:
xjus.stopIPM(node)
xjus.printIpmStatus(node)
wait()
returnToStand()
def stopTripod(t, turnAngle=0, back=False, duty_turn=0):
""" Adds an ending point to the tripod gait and returns to a
standing position. """
xjusAnalysis.endAccel(PLOT_ANALYSIS)
xjusAnalysis.endAvgVelocity()
#acc = xjusAnalysis.getAvgAbsZAccel()
#print("===============================================")
#print("Stability measure: %.4f" % (acc))
current = xjusAnalysis.getAvgCurrent()
#print("Power usage measure: %.4f" % (current))
#print("===============================================")
pytime.wait(DT)
for node in nodes:
[nA, pA, vA, tA] = addTripodPoint(t, turnAngle, back=back, end=True, duty_turn=duty_turn)
addPvtArray(nA, pA, vA, tA)
for node in nodes:
xjus.stopIPM(node)
xjus.printIpmStatus(node)
wait()
returnToStand()
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 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))
