def main():
gamepad = Gamepad()
motors = Motors()
iks = []
for i in range(4):
iks.append(DeltaIK())
controller = ManualController(iks)
angles = 8 * [0.0]
kPs = 8 * [0.1]
frametime = 1.0 / 30.0
print("Ready.")
while True:
try:
s = time.time()
if not gamepad.process_events():
print("Controller disconnected, waiting for reconnect...")
while not gamepad.reconnect():
time.sleep(0.5)
print("Controller reconnected.")
if gamepad.is_a_down():
print("Shutdown button (A) has been pressed, shutting down...")
break
pad_y = gamepad.get_left_thumbstick_y()
controller.step(pad_y, frametime)
# Map IK leg results to motors
for i in range(4):
angles[legMap[i]
[0]] = legDirs[i][0] * (iks[i].angle -
(np.pi / 2.0 - iks[i].A))
angles[legMap[i]
[1]] = legDirs[i][1] * (iks[i].angle +
(np.pi / 2.0 - iks[i].A))
motors.sendCommands(angles, kPs)
time.sleep(max(0, frametime - (time.time() - s)))
except Exception as e:
print(e)
break
print("-- Program at End --")
