Pendulum Balance Robot

I think it would be best to start over and do tests to see if it works, and again after each additional code.