Added PID controller
Removed unused code