Movement: Taking Advantage of Encoders - SCHS-Robotics/Software-Wiki GitHub Wiki

Typically DcMotor.setPower(power) applies electrical power to the motor, where power is a specific percentage of the maximum, between -1 and 1. This does not translate linearly to actual rotation speed, and the speed varies depending on the motor and the resistance that it encounters.

You likely have an encoder on your motor in order to track distance, but it can also track speed, AKA distance over time. Luckily, there is a built-in way to take advantage of the encoder to precisely control the speed of a motor.

The point? To make your robot go straight. If your robot already goes perfectly straight, you don't need to worry about this. But knowing robots, it probably doesn't.

Here are the steps:

1. For each DcMotor you have, call DcMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER) in initialization. This is all that is needed for DcMotor.setPower(power) to consider the input a percentage of speed. It will automatically adjust electrical power to compensate for any external factors that might affect the motor using PID.
2. You might think you are already done after step 1, and you are mostly right. However, your DcMotor doesn't know what speed it is allowed to go! All motors and encoders are different, so you have to set the max speed of your motor using DcMotor.setMaxSpeed(encoderTicksPerSecond). You can probably calculate your motor's maximum encoder ticks per second using its specs. However, I highly recommend determining this experimentally. This wiki does not have info on tracking encoders, but resources can be found online. Simply track the encoder while running at a power of 1, with the default DcMotor RunMode. When doing this, you can see how resistance on the motor can make it run slower than its specs.

Important

If your encoderTicksPerSecond is greater than 4000, you might be out of luck. Upon inspection of the ModernRoboticsUsbDcMotorController class in com.qualcomm.hardware.modernrobotics, it appears that it will cap encoderTicksPerSecond at 4000. See the following methods in ModernRoboticsUsbDcMotorController:

    @Override public synchronized void setMotorMaxSpeed(int motor, int encoderTicksPerSecond)
        {
        this.validateMotor(motor);
        encoderTicksPerSecond = this.validateEncoderTicksPerSecond(motor, encoderTicksPerSecond);
        if (motors[motor].maxSpeed != encoderTicksPerSecond)
            {
            // Preserve the commanded power across the change
            DcMotor.RunMode mode = internalGetCachedOrQueriedRunMode(motor);
            if (mode.isPIDMode())
                {
                double power = internalGetCachedOrQueriedMotorPower(motor);
                motors[motor].maxSpeed = encoderTicksPerSecond;
                internalSetMotorPower(motor, power);
                }
            else
                {
                motors[motor].maxSpeed = encoderTicksPerSecond;
                }
            }
        }

    private int validateEncoderTicksPerSecond(int motor, int encoderTicksPerSecond)
        {
        // We enforce clipping against the max since we only talk to the controller in terms
        // relative to that; we don't speak 'encoder ticks / s' directly
        encoderTicksPerSecond = Range.clip(encoderTicksPerSecond, 1, getDefaultMaxMotorSpeed(motor));
        return encoderTicksPerSecond;
        }

    protected int getDefaultMaxMotorSpeed(int motor)
        {
        final int encoderTicksPerRevolution = ModernRoboticsConstants.TETRIX_MOTOR_TICKS_PER_REVOLUTION; // We assume a Tetrix motor. For v2 firmware and above, we could instead query.
        final int maxDegreesPerSecond       = ModernRoboticsConstants.MAX_PID_DEGREES_PER_SECOND;
        final int degreesPerRevolution      = 360;

        return encoderTicksPerRevolution * maxDegreesPerSecond / degreesPerRevolution;
        }

TETRIX_MOTOR_TICKS_PER_REVOLUTION = 1440

MAX_PID_DEGREES_PER_SECOND = 1000

1440 * 1000 / 360 = 4000