Add basic PID control to motor_control, not ready..