Three Wheel + IMU

Prerequisites

Ensure that three odometry pods are connected: two that are parallel to the length of your chassis and another that is perpendicular to your chassis length. These should be plugged into ports that have motors on them preferably. Due to technical limitations on REV hubs, encoder ports 0 and 3 are the fastest and are reccomended for the two parallel pods.

Default Values

In Constants.java, add an instance of ThreeWheelIMUConstants. Make sure to replace the hardware map names with the actual names of the motor port they are plugged into. You must also set the IMU orientation to match the orientation of your Control Hub.

Constants.java

public static ThreeWheelIMUConstants localizerConstants = new ThreeWheelIMUConstants()
            .forwardTicksToInches(.001989436789)
            .strafeTicksToInches(.001989436789)
            .turnTicksToInches(.001989436789)
            .leftPodY(1)
            .rightPodY(-1)
            .strafePodX(-2.5)
            .leftEncoder_HardwareMapName("leftFront")
            .rightEncoder_HardwareMapName("rightRear")
            .strafeEncoder_HardwareMapName("rightFront")
            .leftEncoderDirection(Encoder.FORWARD)
            .rightEncoderDirection(Encoder.FORWARD)
            .strafeEncoderDirection(Encoder.FORWARD)
            .IMU_HardwareMapName("imu")
            .IMU_Orientation(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT));

Then, add .threeWheelIMULocalizer to createFollower:

Constants.java

return new FollowerBuilder(followerConstants, hardwareMap)
    .threeWheelIMULocalizer(localizerConstants)
    /* other builder steps */
    .build();

Offsets

You must also set your odometry pod offsets, which is where they are relative to your robot's center of rotation. You can use the diagram below to find them. Offsets are in inches.

Three Wheel Odometry Pod Offset Diagram

Once you have found your offsets, add them to your localizer constants by using .leftPodY(), .rightPodY() and .strafeX().

Tuning

Encoder Directions

We will now determine the encoder directions. First, select and run localization test under the localization folder in the tuning OpMode. Then, move the robot forward. The x coordinate should increase. Next, move the robot left. The y coordinate should increase. If either of those does not happen, you must reverse the respective encoder. To reverse an encoder, add

To reverse an encoder, add one of the following to ThreeWheelIMUConstants:

Constants.java

.leftEncoderDirection(Encoder.REVERSE)
.rightEncoderDirection(Encoder.FORWARD)

// and/or:

.strafeEncoderDirection(Encoder.REVERSE)

Important

If when you push the robot forward both x and y change, it is likely that either your offsets have the wrong sign, or that the hardwareMap is incorrect (ie. forward encoder is actually your strafe encoder and vise versa)

Forward Tuner

We will now adjust multipliers that convert encoder ticks into real-world measurements: inches. This ensures your localizer's readings are accurate.

In the tuning OpMode, under localization, select and start the forward tuner. Then, push the robot forward 48 inches (exactly 2 field tiles). This distance is configurable if needed. Once you push the robot forward, two numbers will be displayed on telemetry:

The distance the robot thinks it has traveled
The multiplier; this is the number you want.

Add the multiplier to your ThreeWheelIMUConstants by adding the following.

Constants.java

.forwardTicksToInches(multiplier)

Tip

It is recommended that you run these tests multiple times and average the results, as it can result in more accurate localization.

Lateral Tuner

The lateral tuner is very similar to the forward tuner, except it is sideways. In the tuning OpMode, under localization, select and start the lateral tuner. Push the robot left 48 inches (exactly 2 field tiles). As with the forward tuner, this distance is configurable.

Lastly, add the multiplier to ThreeWheelIMUConstants by adding the following line.

Constants.java

.strafeTicksToInches(multiplier)

Turn Tuner

The turn tuner is again, similar to both the forward tuner and lateral tuner, except it is rotational.Place the robot so it aligns to a fixed reference point (eg. edge of a field tile). In the tuning OpMode, under localization, select and start the lateral tuner. Rotate the robot counterclockwise 48 inches (exactly 2 field tiles). As with the previous tuner, this distance is configurable. Note that the distance is in radians rather than degrees.

Lastly, add the multiplier to ThreeWheelIMUConstants by adding the following line.

Constants.java

.turnTicksToInches(multiplier)

Testing the localizer

Once you have completed the tuning steps, you can test your localizer as described on the localization page.

Congratulations on successfully tuning your localizer!

Note on ESD

If you robot seems to:

Turn or face a different direction when starting a path
Turn to a incorrect angle or miss with large, unfixable errors

Your robot's IMU might be affected by ESD (electrostatic discharge). Consider grounding the robot with a grounding strap and/or reading this guide from FIRST to understand ESD

If after the above you cannot fix the issue, switch to the non-IMU ThreeWheel Localizer, as it will be significantly more accurate than an interfered IMU.

Troubleshooting

If you have any problems, see the troubleshooting page.

Three Wheel + IMU

On this page