PID controller changes

Added PID controller and made use of that in motor controller task