Tuning for RR 1.0 results in no data being shown with "N/A". Localization test also shows N/A (3 deadwheel)

[Leoshrotri]

RR FTC Version

0.1.14

Observed Behavior

I am not able to tune since it keeps on showing N/A for all the data. My code corresponds to the correct encoders and names so I can't figure out the issue. The weird thing is that when I run deadwheeldirectiondebugger, it works and gives accurate data from the odometry pods. This is confusing because I think this means that the pods are giving back tracking data, which means that it shouldn't be an issue where the information is not being relayed through an encoder issue. This has made me stuck, and I have no idea why this issue occurs.

Tuning Files

forward-ramp-1813275658211 (3).json

Robot Logs

Here is the code:

Mecanum Drive:

package org.firstinspires.ftc.teamcode;

import androidx.annotation.NonNull;

import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.*;
import com.acmerobotics.roadrunner.AngularVelConstraint;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.HolonomicController;
import com.acmerobotics.roadrunner.MecanumKinematics;
import com.acmerobotics.roadrunner.MinVelConstraint;
import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Pose2dDual;
import com.acmerobotics.roadrunner.ProfileAccelConstraint;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.TimeTrajectory;
import com.acmerobotics.roadrunner.TimeTurn;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TurnConstraints;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.VelConstraint;
import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.LazyImu;
import com.acmerobotics.roadrunner.ftc.LynxFirmware;
import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import com.acmerobotics.roadrunner.ftc.RawEncoder;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage;
import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage;
import org.firstinspires.ftc.teamcode.messages.MecanumLocalizerInputsMessage;
import org.firstinspires.ftc.teamcode.messages.PoseMessage;

import java.lang.Math;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;

@config
public final class MecanumDrive {
public static class Params {
// IMU orientation
// TODO: fill in these values based on
// see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
RevHubOrientationOnRobot.LogoFacingDirection.RIGHT;
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
RevHubOrientationOnRobot.UsbFacingDirection.UP;

    // drive model parameters
    public double inPerTick = 0;//.002935171006;
    public double lateralInPerTick = 0;//0.002326262787239087;
    public double trackWidthTicks = 0;//4374.15519241689;

    // feedforward parameters (in tick units)
    public double kS = 0;
    public double kV = 0;
    public double kA = 0; //0.0000001

    // path profile parameters (in inches)
    public double maxWheelVel = 50;
    public double minProfileAccel = -30;
    public double maxProfileAccel = 50;

    // turn profile parameters (in radians)
    public double maxAngVel = Math.PI; // shared with path
    public double maxAngAccel = Math.PI;

    // path controller gains
    public double axialGain = 0; //0.5
    public double lateralGain = 0; //0.5
    public double headingGain = 0; // shared with turn //1
    public double axialVelGain = 0.0;
    public double lateralVelGain = 0.0;
    public double headingVelGain = 0.0; // shared with turn
}

public static Params PARAMS = new Params();

public final MecanumKinematics kinematics = new MecanumKinematics(
        PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick);

public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
        PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
public final VelConstraint defaultVelConstraint =
        new MinVelConstraint(Arrays.asList(
                kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
                new AngularVelConstraint(PARAMS.maxAngVel)
        ));
public final AccelConstraint defaultAccelConstraint =
        new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);

public final DcMotorEx leftFront, leftBack, rightBack, rightFront;

public final VoltageSensor voltageSensor;

public final LazyImu lazyImu;

public final Localizer localizer;
public Pose2d pose;

private final LinkedList<Pose2d> poseHistory = new LinkedList<>();

private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000);

public class DriveLocalizer implements Localizer {
    public final Encoder leftFront, leftBack, rightBack, rightFront;
    public final IMU imu;

    private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos;
    private Rotation2d lastHeading;
    private boolean initialized;

    public DriveLocalizer() {
        leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
        leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack));
        rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
        rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));

        imu = lazyImu.get();

        // TODO: reverse encoders if needed
        leftFront.setDirection((DcMotorSimple.Direction.REVERSE));
        //rightFront.setDirection((DcMotorSimple.Direction.REVERSE));
        //leftBack.setDirection((DcMotorSimple.Direction.REVERSE));
        //rightBack.setDirection(DcMotorSimple.Direction.REVERSE);
    }

    @Override
    public Twist2dDual<Time> update() {
        PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
        PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity();
        PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
        PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();

        YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();

        FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage(
                leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles));

        Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));

        if (!initialized) {
            initialized = true;

            lastLeftFrontPos = leftFrontPosVel.position;
            lastLeftBackPos = leftBackPosVel.position;
            lastRightBackPos = rightBackPosVel.position;
            lastRightFrontPos = rightFrontPosVel.position;

            lastHeading = heading;

            return new Twist2dDual<>(
                    Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
                    DualNum.constant(0.0, 2)
            );
        }

        double headingDelta = heading.minus(lastHeading);
        Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
                new DualNum<Time>(new double[]{
                        (leftFrontPosVel.position - lastLeftFrontPos),
                        leftFrontPosVel.velocity,
                }).times(PARAMS.inPerTick),
                new DualNum<Time>(new double[]{
                        (leftBackPosVel.position - lastLeftBackPos),
                        leftBackPosVel.velocity,
                }).times(PARAMS.inPerTick),
                new DualNum<Time>(new double[]{
                        (rightBackPosVel.position - lastRightBackPos),
                        rightBackPosVel.velocity,
                }).times(PARAMS.inPerTick),
                new DualNum<Time>(new double[]{
                        (rightFrontPosVel.position - lastRightFrontPos),
                        rightFrontPosVel.velocity,
                }).times(PARAMS.inPerTick)
        ));

        lastLeftFrontPos = leftFrontPosVel.position;
        lastLeftBackPos = leftBackPosVel.position;
        lastRightBackPos = rightBackPosVel.position;
        lastRightFrontPos = rightFrontPosVel.position;

        lastHeading = heading;

        return new Twist2dDual<>(
                twist.line,
                DualNum.cons(headingDelta, twist.angle.drop(1))
        );
    }
}

public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
    this.pose = pose;


    for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
        module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
    }

    // TODO: make sure your config has motors with these names (or change them)
    //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
    leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
    leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
    rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
    rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");

    leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
    leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
    rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
    rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

    // TODO: reverse motor directions if needed

    leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
      leftBack.setDirection(DcMotorSimple.Direction.REVERSE);




    // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
    //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
    lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
            PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));

    voltageSensor = hardwareMap.voltageSensor.iterator().next();

    localizer = new ThreeDeadWheelLocalizer(hardwareMap, PARAMS.inPerTick);

    FlightRecorder.write("MECANUM_PARAMS", PARAMS);
}

public void setDrivePowers(PoseVelocity2d powers) {
    MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(
            PoseVelocity2dDual.constant(powers, 1));

    double maxPowerMag = 1;
    for (DualNum<Time> power : wheelVels.all()) {
        maxPowerMag = Math.max(maxPowerMag, power.value());
    }

    leftFront.setPower(wheelVels.leftFront.get(0) / maxPowerMag);
    leftBack.setPower(wheelVels.leftBack.get(0) / maxPowerMag);
    rightBack.setPower(wheelVels.rightBack.get(0) / maxPowerMag);
    rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag);
}

public final class FollowTrajectoryAction implements Action {
    public final TimeTrajectory timeTrajectory;
    private double beginTs = -1;

    private final double[] xPoints, yPoints;

    public FollowTrajectoryAction(TimeTrajectory t) {
        timeTrajectory = t;

        List<Double> disps = com.acmerobotics.roadrunner.Math.range(
                0, t.path.length(),
                Math.max(2, (int) Math.ceil(t.path.length() / 2)));
        xPoints = new double[disps.size()];
        yPoints = new double[disps.size()];
        for (int i = 0; i < disps.size(); i++) {
            Pose2d p = t.path.get(disps.get(i), 1).value();
            xPoints[i] = p.position.x;
            yPoints[i] = p.position.y;
        }
    }

    @Override
    public boolean run(@NonNull TelemetryPacket p) {
        double t;
        if (beginTs < 0) {
            beginTs = Actions.now();
            t = 0;
        } else {
            t = Actions.now() - beginTs;
        }

        if (t >= timeTrajectory.duration) {
            leftFront.setPower(0);
            leftBack.setPower(0);
            rightBack.setPower(0);
            rightFront.setPower(0);

            return false;
        }

        Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
        targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));

        PoseVelocity2d robotVelRobot = updatePoseEstimate();

        PoseVelocity2dDual<Time> command = new HolonomicController(
                PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
                PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
        )
                .compute(txWorldTarget, pose, robotVelRobot);
        driveCommandWriter.write(new DriveCommandMessage(command));

        MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
        double voltage = voltageSensor.getVoltage();

        final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
                PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
        double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
        double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
        double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
        double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
        mecanumCommandWriter.write(new MecanumCommandMessage(
                voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
        ));

        leftFront.setPower(leftFrontPower);
        leftBack.setPower(leftBackPower);
        rightBack.setPower(rightBackPower);
        rightFront.setPower(rightFrontPower);

        p.put("x", pose.position.x);
        p.put("y", pose.position.y);
        p.put("heading (deg)", Math.toDegrees(pose.heading.toDouble()));

        Pose2d error = txWorldTarget.value().minusExp(pose);
        p.put("xError", error.position.x);
        p.put("yError", error.position.y);
        p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));

        // only draw when active; only one drive action should be active at a time
        Canvas c = p.fieldOverlay();
        drawPoseHistory(c);

        c.setStroke("#4CAF50");
        Drawing.drawRobot(c, txWorldTarget.value());

        c.setStroke("#3F51B5");
        Drawing.drawRobot(c, pose);

        c.setStroke("#4CAF50FF");
        c.setStrokeWidth(1);
        c.strokePolyline(xPoints, yPoints);

        return true;
    }

    @Override
    public void preview(Canvas c) {
        c.setStroke("#4CAF507A");
        c.setStrokeWidth(1);
        c.strokePolyline(xPoints, yPoints);
    }
}

public final class TurnAction implements Action {
    private final TimeTurn turn;

    private double beginTs = -1;

    public TurnAction(TimeTurn turn) {
        this.turn = turn;
    }

    @Override
    public boolean run(@NonNull TelemetryPacket p) {
        double t;
        if (beginTs < 0) {
            beginTs = Actions.now();
            t = 0;
        } else {
            t = Actions.now() - beginTs;
        }

        if (t >= turn.duration) {
            leftFront.setPower(0);
            leftBack.setPower(0);
            rightBack.setPower(0);
            rightFront.setPower(0);

            return false;
        }

        Pose2dDual<Time> txWorldTarget = turn.get(t);
        targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));

        PoseVelocity2d robotVelRobot = updatePoseEstimate();

        PoseVelocity2dDual<Time> command = new HolonomicController(
                PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
                PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
        )
                .compute(txWorldTarget, pose, robotVelRobot);
        driveCommandWriter.write(new DriveCommandMessage(command));

        MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
        double voltage = voltageSensor.getVoltage();
        final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
                PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
        double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
        double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
        double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
        double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
        mecanumCommandWriter.write(new MecanumCommandMessage(
                voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
        ));

        leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
        leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage);
        rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
        rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);

        Canvas c = p.fieldOverlay();
        drawPoseHistory(c);

        c.setStroke("#4CAF50");
        Drawing.drawRobot(c, txWorldTarget.value());

        c.setStroke("#3F51B5");
        Drawing.drawRobot(c, pose);

        c.setStroke("#7C4DFFFF");
        c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);

        return true;
    }

    @Override
    public void preview(Canvas c) {
        c.setStroke("#7C4DFF7A");
        c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
    }
}

public PoseVelocity2d updatePoseEstimate() {
    Twist2dDual<Time> twist = localizer.update();
    pose = pose.plus(twist.value());

    poseHistory.add(pose);
    while (poseHistory.size() > 100) {
        poseHistory.removeFirst();
    }

    estimatedPoseWriter.write(new PoseMessage(pose));

    return twist.velocity().value();
}

private void drawPoseHistory(Canvas c) {
    double[] xPoints = new double[poseHistory.size()];
    double[] yPoints = new double[poseHistory.size()];

    int i = 0;
    for (Pose2d t : poseHistory) {
        xPoints[i] = t.position.x;
        yPoints[i] = t.position.y;

        i++;
    }

    c.setStrokeWidth(1);
    c.setStroke("#3F51B5");
    c.strokePolyline(xPoints, yPoints);
}

public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
    return new TrajectoryActionBuilder(
            TurnAction::new,
            FollowTrajectoryAction::new,
            new TrajectoryBuilderParams(
                    1e-6,
                    new ProfileParams(
                            0.25, 0.1, 1e-2
                    )
            ),
            beginPose, 0.0,
            defaultTurnConstraints,
            defaultVelConstraint, defaultAccelConstraint
    );
}

}

3 deadwheel localizer:

package org.firstinspires.ftc.teamcode;

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import com.acmerobotics.roadrunner.ftc.RawEncoder;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;

import org.firstinspires.ftc.teamcode.messages.ThreeDeadWheelInputsMessage;

@config
public final class ThreeDeadWheelLocalizer implements Localizer {
public static class Params {
public double par0YTicks = 0; // y position of the first parallel encoder (in tick units)
public double par1YTicks = 1.0; // y position of the second parallel encoder (in tick units)
public double perpXTicks = 0; // x position of the perpendicular encoder (in tick units)
}

public static Params PARAMS = new Params();

public final Encoder leftFront, leftBack, rightFront;

public final double inPerTick;

private int lastPar0Pos, lastPar1Pos, lastPerpPos;
private boolean initialized;

public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) {
    // TODO: make sure your config has **motors** with these names (or change them)
    //   the encoders should be plugged into the slot matching the named motor
    //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
    leftFront = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "leftFront")));
    leftBack = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "leftBack")));
    rightFront = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")));

    // TODO: reverse encoder directions if needed
       //leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
       //rightFront.setDirection(DcMotorSimple.Direction.REVERSE);
       //leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
    this.inPerTick = inPerTick;

    FlightRecorder.write("THREE_DEAD_WHEEL_PARAMS", PARAMS);
}

public Twist2dDual<Time> update() {
    PositionVelocityPair par0PosVel = leftFront.getPositionAndVelocity();
    PositionVelocityPair par1PosVel = leftBack.getPositionAndVelocity();
    PositionVelocityPair perpPosVel = rightFront.getPositionAndVelocity();

    FlightRecorder.write("THREE_DEAD_WHEEL_INPUTS", new ThreeDeadWheelInputsMessage(par0PosVel, par1PosVel, perpPosVel));

    if (!initialized) {
        initialized = true;

        lastPar0Pos = par0PosVel.position;
        lastPar1Pos = par1PosVel.position;
        lastPerpPos = perpPosVel.position;

        return new Twist2dDual<>(
                Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
                DualNum.constant(0.0, 2)
        );
    }

    int par0PosDelta = par0PosVel.position - lastPar0Pos;
    int par1PosDelta = par1PosVel.position - lastPar1Pos;
    int perpPosDelta = perpPosVel.position - lastPerpPos;

    Twist2dDual<Time> twist = new Twist2dDual<>(
            new Vector2dDual<>(
                    new DualNum<Time>(new double[] {
                            (PARAMS.par0YTicks * par1PosDelta - PARAMS.par1YTicks * par0PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
                            (PARAMS.par0YTicks * par1PosVel.velocity - PARAMS.par1YTicks * par0PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
                    }).times(inPerTick),
                    new DualNum<Time>(new double[] {
                            (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosDelta - par0PosDelta) + perpPosDelta),
                            (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosVel.velocity - par0PosVel.velocity) + perpPosVel.velocity),
                    }).times(inPerTick)
            ),
            new DualNum<>(new double[] {
                    (par0PosDelta - par1PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
                    (par0PosVel.velocity - par1PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
            })
    );

    lastPar0Pos = par0PosVel.position;
    lastPar1Pos = par1PosVel.position;
    lastPerpPos = perpPosVel.position;

    return twist;
}

}

TuningOpModes:

package org.firstinspires.ftc.teamcode.tuning;

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.reflection.ReflectionConfig;
import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.ftc.AngularRampLogger;
import com.acmerobotics.roadrunner.ftc.DeadWheelDirectionDebugger;
import com.acmerobotics.roadrunner.ftc.DriveType;
import com.acmerobotics.roadrunner.ftc.DriveView;
import com.acmerobotics.roadrunner.ftc.DriveViewFactory;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.ForwardPushTest;
import com.acmerobotics.roadrunner.ftc.ForwardRampLogger;
import com.acmerobotics.roadrunner.ftc.LateralPushTest;
import com.acmerobotics.roadrunner.ftc.LateralRampLogger;
import com.acmerobotics.roadrunner.ftc.ManualFeedforwardTuner;
import com.acmerobotics.roadrunner.ftc.MecanumMotorDirectionDebugger;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar;

import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta;
import org.firstinspires.ftc.teamcode.MecanumDrive;
import org.firstinspires.ftc.teamcode.TankDrive;
import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer;
import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

public final class TuningOpModes {
// TODO: change this to TankDrive.class if you're using tank
public static final Class<?> DRIVE_CLASS = MecanumDrive.class;

public static final String GROUP = "quickstart";
public static final boolean DISABLED = false;

private TuningOpModes() {}

private static OpModeMeta metaForClass(Class<? extends OpMode> cls) {
    return new OpModeMeta.Builder()
            .setName(cls.getSimpleName())
            .setGroup(GROUP)
            .setFlavor(OpModeMeta.Flavor.TELEOP)
            .build();
}

@OpModeRegistrar
public static void register(OpModeManager manager) {
    if (DISABLED) return;

    DriveViewFactory dvf;
    if (DRIVE_CLASS.equals(MecanumDrive.class)) {
        dvf = hardwareMap -> {
            MecanumDrive md = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));

            List<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
            List<Encoder> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
            if (md.localizer instanceof MecanumDrive.DriveLocalizer) {
                MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) md.localizer;
                leftEncs.add(dl.leftFront);
                leftEncs.add(dl.leftBack);
                rightEncs.add(dl.rightFront);
                rightEncs.add(dl.rightBack);
            } else if (md.localizer instanceof ThreeDeadWheelLocalizer) {
                ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) md.localizer;
                parEncs.add(dl.leftFront);
                parEncs.add(dl.leftBack);
                perpEncs.add(dl.rightFront);
            } else if (md.localizer instanceof TwoDeadWheelLocalizer) {
                TwoDeadWheelLocalizer dl = (TwoDeadWheelLocalizer) md.localizer;
                parEncs.add(dl.par);
                perpEncs.add(dl.perp);
            } else {
                throw new RuntimeException("unknown localizer: " + md.localizer.getClass().getName());
            }

            return new DriveView(
                DriveType.MECANUM,
                    MecanumDrive.PARAMS.inPerTick,
                    MecanumDrive.PARAMS.maxWheelVel,
                    MecanumDrive.PARAMS.minProfileAccel,
                    MecanumDrive.PARAMS.maxProfileAccel,
                    hardwareMap.getAll(LynxModule.class),
                    Arrays.asList(
                            md.leftFront,
                            md.leftBack
                    ),
                    Arrays.asList(
                            md.rightFront,
                            md.rightBack
                    ),
                    leftEncs,
                    rightEncs,
                    parEncs,
                    perpEncs,
                    md.lazyImu,
                    md.voltageSensor,
                    () -> new MotorFeedforward(MecanumDrive.PARAMS.kS,
                            MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick,
                            MecanumDrive.PARAMS.kA / MecanumDrive.PARAMS.inPerTick)
            );
        };
    } else if (DRIVE_CLASS.equals(TankDrive.class)) {
        dvf = hardwareMap -> {
            TankDrive td = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));

            List<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
            List<Encoder> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
            if (td.localizer instanceof TankDrive.DriveLocalizer) {
                TankDrive.DriveLocalizer dl = (TankDrive.DriveLocalizer) td.localizer;
                leftEncs.addAll(dl.leftEncs);
                rightEncs.addAll(dl.rightEncs);
            } else if (td.localizer instanceof ThreeDeadWheelLocalizer) {
                ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) td.localizer;
                parEncs.add(dl.leftFront);
                parEncs.add(dl.leftBack);
                perpEncs.add(dl.rightFront);
            } else if (td.localizer instanceof TwoDeadWheelLocalizer) {
                TwoDeadWheelLocalizer dl = (TwoDeadWheelLocalizer) td.localizer;
                parEncs.add(dl.par);
                perpEncs.add(dl.perp);
            } else {
                throw new RuntimeException("unknown localizer: " + td.localizer.getClass().getName());
            }

            return new DriveView(
                DriveType.TANK,
                    TankDrive.PARAMS.inPerTick,
                    TankDrive.PARAMS.maxWheelVel,
                    TankDrive.PARAMS.minProfileAccel,
                    TankDrive.PARAMS.maxProfileAccel,
                    hardwareMap.getAll(LynxModule.class),
                    td.leftMotors,
                    td.rightMotors,
                    leftEncs,
                    rightEncs,
                    parEncs,
                    perpEncs,
                    td.lazyImu,
                    td.voltageSensor,
                    () -> new MotorFeedforward(TankDrive.PARAMS.kS,
                            TankDrive.PARAMS.kV / TankDrive.PARAMS.inPerTick,
                            TankDrive.PARAMS.kA / TankDrive.PARAMS.inPerTick)
            );
        };
    } else {
        throw new RuntimeException();
    }

    manager.register(metaForClass(AngularRampLogger.class), new AngularRampLogger(dvf));
    manager.register(metaForClass(ForwardPushTest.class), new ForwardPushTest(dvf));
    manager.register(metaForClass(ForwardRampLogger.class), new ForwardRampLogger(dvf));
    manager.register(metaForClass(LateralPushTest.class), new LateralPushTest(dvf));
    manager.register(metaForClass(LateralRampLogger.class), new LateralRampLogger(dvf));
    manager.register(metaForClass(ManualFeedforwardTuner.class), new ManualFeedforwardTuner(dvf));
    manager.register(metaForClass(MecanumMotorDirectionDebugger.class), new MecanumMotorDirectionDebugger(dvf));
    manager.register(metaForClass(DeadWheelDirectionDebugger.class), new DeadWheelDirectionDebugger(dvf));

    manager.register(metaForClass(ManualFeedbackTuner.class), ManualFeedbackTuner.class);
    manager.register(metaForClass(SplineTest.class), SplineTest.class);
    manager.register(metaForClass(LocalizationTest.class), LocalizationTest.class);

    FtcDashboard.getInstance().withConfigRoot(configRoot -> {
        for (Class<?> c : Arrays.asList(
                AngularRampLogger.class,
                ForwardRampLogger.class,
                LateralRampLogger.class,
                ManualFeedforwardTuner.class,
                MecanumMotorDirectionDebugger.class,
                ManualFeedbackTuner.class
        )) {
            configRoot.putVariable(c.getSimpleName(), ReflectionConfig.createVariableFromClass(c));
        }
    });
}

}

No response