Fix build by adding in Gyro testing code

This involved adding a PID controller, and a Gyro turn command which 
uses it