Arduino sketch to build a pendulum with a servo