Added PID Controller to Working Code