set_servo_angles(pbs, angles)
dt = time.time() - start_time
t += dt
# print(dt)
return t
if __name__ == "__main__":
print("Starting")
s0 = Servo(0.15, 0.065, 0.225) # HS-311
s1 = Servo(0.15, 0.065, 0.225)
s2 = Servo(0.15, 0.065, 0.225)
s3 = Servo(0.15, 0.245, 0.055) # SG92R
pbs = PiBlasterServos()
pbs.add(s0, 18)
pbs.add(s1, 23)
pbs.add(s2, 24)
pbs.add(s3, 25)
reset_servos(pbs)
def on_terminate(signal, frame):
reset_servos(pbs)
pbs.turn_off()
sys.exit()
signal.signal(signal.SIGINT, on_terminate)
full_ik = FullIK(
platform_radius=1.42, neck_length=2.04, base_radius=3.35, lower_leg_length=2.37, upper_leg_length=3.77
