First working commit
Added PID, controller class