FTC Programming

Encoder Control

Controlling Encoders

What are encoders?

Inside a motor, there is a little magnet on the end, which connects to the shaft of the motor. This is the encoder. We are able to return tick values from it, and give it tick values we would like to have.

How do we program encoders?

The concept of encoders is pretty easy to understand, but a tad more difficult to program. Let’s outline our steps:

  1. Tell the robot we want to use encoders
  2. Give a tick count value (inches * ticks per inch)
  3. Give the motors a power
  4. Wait till we are done running
  5. Turn off motors and run to position

Step 1: Tell the robot we want to use encoders

This part is pretty easy. For this example, we will be focusing on a two-motor drive, but this is very adaptable to other drive trains.

robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

Step 2: Give a tick count value

Finding the tick count value is specific for your robot, so we can’t really give you a precise number that works. When we are trying to find a good inch to tick value, we create a TeleOp with this code:

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

@TeleOp(name = "GimmeEncoders")
public class GimmeEncoders extends LinearOpMode {

    HardwareRobot robot = new HardwareRobot();

    @Override
    public void runOpMode() {

        robot.init(hardwareMap, telemetry);
        robot.leftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
        robot.rightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
        robot.setEncoders(true);

        waitForStart();

        while (opModeIsActive()) {
            telemetry.addData("Left given", robot.leftMotor.getCurrentPosition());      
            telemetry.addData("Right given", robot.rightMotor.getCurrentPosition());
            telemetry.update();
        }
    }
}

Run this, and you can manually move the robot and get values back. FLOAT means that at 0 power, the wheels still spin.

ISSUE IN SKYSTONE 5.3

Currently, there is an issue in the Skystone SDK. When they were fixing one part of the PID, they messed up how the RUN_TO_POSITION worked. For more info, go here: https://ftcforum.firstinspires.org/forum/control-hub-pilot-forum/77745-trouble-with-run_to_position-with-skystone-5-3-and-rev-firmware-1-8-2

We are hoping for a fix in Skystone 5.4+.

Once we find this value, create an integer variable for it:

int inchCount = 24.5;

Let’s also give the distance we want the motor to go:

int inches = 10;

Give the motors the tick count

Now let’s give the motors the tick count we want.


// calculate new target position
int newLeftTarget = robot.leftMotor.getCurrentPosition() + (int) (inches *inchCount);
int newRightTarget = robot.rightMotor.getCurrentPosition() + (int) (inches *inchCount);
 

// set new position
robot.leftMotor.setTargetPosition(newLeftTarget);
robot.rightMotor.setTargetPosition(newRightTarget);

robot.leftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.rightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);

Step 3: Give the motors a power

The whole method behind encoders is that they go the same distance, on any power. This is why using encoders is always more accurate than doing time based autonomous programs.

To give power to motors, we just have to do this:

robot.leftMotor.setPower(Math.abs(speed));
robot.rightMotor.setPower(Math.abs(speed));

There’s a reason we took the absolute value of the power. This ensures that no matter what, the power is always a positive value. If you want to drive backwards, give the encoders negative distance.

We then have to make sure that power is only being given when the motors are busy and the opmode is active.

Step 4: Wait till motors are done running

while (opModeIsActive() && (robot.leftMotor.isBusy() && robot.rightMotor.isBusy())) {

}

Inside this while loop, we are checking that power only runs until the encoder ticks are met.

Step 5: Turn off motors and run to position

Finally, after this loop, we set our power to 0, and we reset back to RUN_USING_ENCODERS.

robot.leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

That's it!

From here, there are a few things you can do:

  1. Integrate Gyro Control
  2. Check out the external samples FIRST has for encoders
  3. Create a function for each movement (strafe,turn,etc.)
chevron_up