PID Controller is tested and working