Testing the PID Control

Script to see how well the PID system will follow the input
Sped up the code and added error checking