Added first version of PID motor controller