Getting motor to work with kp/kd.