motor_control: Changed controller to PID controller. Some estetic changes