Converting Encoder Ticks to a Distance
In the previous section, the basic structure needed to use RUN_TO_POSITION
was created. The placement of leftmotor.setTargetPosition(1000);
and rightmotor.setTargetPosition(1000);
within the code, set the target position to 1000 ticks.
But how far is a tick and how can we use them to help our robot navigate an area? We could attempt to estimate the distance the robot moves per tick or we can convert the amount of ticks per revolution of the encoder into a unit like millimeters or inches! For instance, if you work through the conversion process and find out that a drivetrain takes 700 ticks to move an inch, this can be used to find the total number of ticks need to move the robot 24 inches.
Reminder that the basis for this guide is the Class Bot V2. The REV DUO Build System is a metric system. Since part of the conversion process references the diameter of the wheels, this section will convert to ticks per mm.
What's Needed for the Conversion
This process will take a bit of math to achieve so let's break it down.
When using encoders built into motors, converting from ticks per revolution to ticks per unit of measure moved requires the following information:
Ticks per Revolution
The amount of ticks per revolution of the encoder shaft is dependent on the motor and encoder. Manufacturers of motors with built-in encoders will have information on the amount of ticks per revolution.
For HD Hex Motors the encoder counts 28 ticks per revolution of the motor shaft.
Total Gear Reduction
Since ticks per revolution of the encoder shaft is before any gear reduction calculating the total gear reduction is needed. This includes the gearbox and any addition reduction from motion transmission components. To find the total gear reduction use the Compound Gearing formula.
For the Class Bot V2 there are two UltraPlanetary Cartridges, 4:1 and 5:1, and an additional gear reduction from the UltraPlanetary Output to the wheels, 72T:45T ratio.
Using the compound gearing formula for the Class Bot V2 the total gear reduction is:
Circumference of the Wheel
The Class Bot V2 uses the 90mm Traction Wheels. 90mm is the diameter of the wheel. To get the appropriate circumference use the following formula
You can calculate this by hand, but for the purpose of this guide, this can be calculated within the code.
To summarize, for the Class Bot V2 the following information is true:
Ticks per revolution
28 ticks
Total gear reduction
30.21
Circumference of the wheel
Translating the Conversion to Code
Setting up Variables
Each of these pieces of information will be used to find the number of encoder ticks (or counts) per mm that the wheel moves. Rather than worry about calculating this information by hand, these values can be added to the code as constant variables. To do this create three variables:
COUNTS_PER_MOTOR_REV
DRIVE_GEAR_REDUCTION
WHEEL_CIRCUMFERENCE_MM
We'll add the variables to the initialization section of the OpMode:
To ensure variables are referenceable they are set as static final double
variables. Static allows references to the variables anywhere within the class. Final dictates that these variables are constant and unchanged elsewhere within the code.
Since these variables are not integers they are classified as double variables.
public class HelloRobot_EncoderAuton extends LinearOpMode {
private DcMotor leftmotor;
private DcMotor rightmotor;
static final double COUNTS_PER_MOTOR_REV = 28.0;
static final double DRIVE_GEAR_REDUCTION = 30.21;
static final double WHEEL_CIRCUMFERENCE_MM = 90.0 * Math.PI;
Now that these three variables have been defined, we can use them to calculate two other variables: the amount of encoder counts per rotation of the wheel and the number of counts per mm that the wheel moves.
public class HelloRobot_EncoderAuton extends LinearOpMode {
private DcMotor leftmotor;
private DcMotor rightmotor;
static final double COUNTS_PER_MOTOR_REV = 28.0;
static final double DRIVE_GEAR_REDUCTION = 30.24;
static final double WHEEL_CIRCUMFERENCE_MM = 90.0 * 3.14;
static final double COUNTS_PER_WHEEL_REV =
static final double COUNTS_PER_MM =
Calculating COUNTS_PER_WHEEL_REV
To calculate counts per wheel revolution multiple COUNTS_PER_MOTOR_REV
by DRIVE_GEAR_REDUCTION
Use the following formula:
Where:
=
COUNTS_PER_MOTOR_REV
=
DRIVE_GEAR_REDUCTION
=
COUNTS_PER_WHEEL_REV
public class HelloRobot_EncoderAuton extends LinearOpMode {
private DcMotor leftmotor;
private DcMotor rightmotor;
static final double COUNTS_PER_MOTOR_REV = 28.0;
static final double DRIVE_GEAR_REDUCTION = 30.24;
static final double WHEEL_CIRCUMFERENCE_MM = 90.0 * 3.14;
static final double COUNTS_PER_WHEEL_REV = COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION
static final double COUNTS_PER_MM =
Calculating COUNTS_PER_MM
Once COUNTS_PER_WHEEL_REV
is calculated, use it to calculate the counts per mm that the wheel moves. To do this divide the COUNTS_PER_WHEEL_REV
by the WHEEL_CIRCUMFERENCE_MM
. Use the following formula.
Where,
=
COUNTS_PER_MOTOR_REV
=
DRIVE_GEAR_REDUCTION
=
WHEEL_CIRCUMFERENCE_MM
=
COUNTS_PER_WHEEL_REV
=
COUNTS_PER_MM
public class HelloRobot_EncoderAuton extends LinearOpMode {
private DcMotor leftmotor;
private DcMotor rightmotor;
static final double COUNTS_PER_MOTOR_REV = 28.0;
static final double DRIVE_GEAR_REDUCTION = 30.24;
static final double WHEEL_CIRCUMFERENCE_MM = 90.0 * 3.14;
static final double COUNTS_PER_WHEEL_REV = COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION;
static final double COUNTS_PER_MM = COUNTS_PER_WHEEL_REV / WHEEL_CIRCUMFERENCE_MM;
COUNTS_PER_WHEEL_REV
will be created as a separate variable fromCOUNTS_PER_MM
as it is used in calculating a target velocity.
Program thus far:
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@Autonomous //sets the op mode as an autonomous op mode
public class HelloWorld_Encoder extends LinearOpMode {
private DcMotor leftmotor;
private DcMotor rightmotor;
static final double COUNTS_PER_MOTOR_REV = 28.0;
static final double DRIVE_GEAR_REDUCTION = 30.24;
static final double WHEEL_CIRCUMFERENCE_MM = 90.0 * 3.14;
static final double COUNTS_PER_WHEEL_REV = COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION;
static final double COUNTS_PER_MM = COUNTS_PER_WHEEL_REV / WHEEL_CIRCUMFERENCE_MM;
@Override
public void runOpMode() {
leftmotor = hardwareMap.get(DcMotor.class, "leftmotor");
rightmotor = hardwareMap.get(DcMotor.class, "rightmotor");
rightmotor.setDirection(DcMotor.Direction.REVERSE);
leftmotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightmotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// Wait for the game to start (driver presses PLAY)
waitForStart();
leftmotor.setTargetPosition(1000);
rightmotor.setTargetPosition(1000);
leftmotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rightmotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
leftmotor.setPower(0.8);
rightmotor.setPower(0.8);
// run until the end of the match (driver presses STOP)
while (opModeIsActive() && (leftmotor.isBusy() && rightmotor.isBusy())) {
}
}
}
Last updated
Was this helpful?