angles = full_ik.solve(e_v, f_v, height) 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(
import cProfile as profile from servo_fast import ServoFast as Servo from pi_blaster_servos import PiBlasterServos import pyximport pyximport.install() from pi_blaster_servos_fast import PiBlasterServosFast def set(pbs): pbs[0] = 0 pbs[1] = 0 pbs[2] = 0 pbs[3] = 0 if __name__ == "__main__": 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) profile.run("for i in range(10): set(pbs)") pbs.close()