###########################
# Front right leg only!
# Currently the white one.
###########################
# usc = Usc()
maestro = Maestro()
# Define servos.
servo4 = Servo(3, -90, 180, 500, 2500, 150, bias=10, direction=-1)
servo5 = Servo(4, -225, 45, 500, 2500, 150, bias=5, direction=1)
servo6 = Servo(5, -135, 135, 500, 2500, 150, bias=10, direction=1)
servos = [servo4, servo5, servo6]
# Configure velocity.
maestro.set_speed(servo4, servo4.max_vel)
maestro.set_speed(servo5, servo5.max_vel)
maestro.set_speed(servo6, servo6.max_vel)
# Go home.
def goHome():
for servo in servos:
servo.set_target(0)
maestro.set_target(servo)
# Set position.
def moveToEuclidean(position, a3=False):
angles = Finesse.inverse_pack((7.5, 7.5), position, a3=a3)
if angles is not None:
[125, 250, 375, 750],
[0, 375, 500, 625],
[250, 625, 750, 875]
]
p = [
[(5, 0, -9), (3, 0, -12), (0, 0, -12.2), (-3, 0, -12)],
[(-3, 0, -12), (5, 0, -9), (3, 0, -12), (0, 0, -12.2)],
[(0, 0, -12.2), (-3, 0, -12), (5, 0, -9), (3, 0, -12)],
[(0, 0, -12.2), (-3, 0, -12), (5, 0, -9), (3, 0, -12)]
]
# Configure velocity.
for servo in leg1 + leg2 + leg3 + leg4:
maestro.set_speed(servo, servo.max_vel)
# Go home.
def go_home(servos):
for servo in servos:
servo.set_target(0)
maestro.set_speed(servo, servo.max_vel)
maestro.set_target(servo)
# Set position.
def target_euclidean(leg, position, a3=False):
angles = Finesse.inverse_pack((7.5, 7.5), position, a3=a3)
if angles is not None:
