Code For Auto Stabilizing Arm

For good reading on the subject, look up search words - PID controller -. It's not exactly simple though.