Cannot strafe at a diagonal or to a heading

[nash-pillai]

RR FTC Version

0.1.11

Observed Behavior

Strafing while turning and strafing at a diagonal goes in the wrong direction, but the robot knows where it is

The code we are running:

drive.actionBuilder(beginPose)
.strafeToSplineHeading(Vector2d(12.0, 38.0), -TAU / 4)
.build()

drive.actionBuilder(beginPose)
.strafeTo(Vector2d(17.0, 38.0))
.build()

Tuning Files

MecanumDrive.java:

```java 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.AccelConstraint; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Actions; 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.PoseVelocity2d; import com.acmerobotics.roadrunner.PoseVelocity2dDual; import com.acmerobotics.roadrunner.ProfileAccelConstraint; import com.acmerobotics.roadrunner.Rotation2d; 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.Vector2d; 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.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.teamcode.messages.DriveCommandMessage; import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage; import org.firstinspires.ftc.teamcode.messages.MecanumEncodersMessage; import org.firstinspires.ftc.teamcode.messages.PoseMessage; 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.UP; public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.RIGHT; // drive model parameters public double inPerTick = 12 * 5 * 2 / 39820.0; public double lateralInPerTick = 0.00018869762188191035; //inPerTick; public double trackWidthTicks = 4432.299; // feedforward parameters (in tick units) public double kS = 0.9589658474985132; public double kV = 0.0005588422234744558; public double kA = 0.000001; // path profile parameters (in inches) public double maxWheelVel = 40; public double minProfileAccel = -20; public double maxProfileAccel = 20; // 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; public double lateralGain = 0.0; public double headingGain = 4.32; // shared with turn public double axialVelGain = 0.0; public double lateralVelGain = 0.0; public double headingVelGain = 0.59; // 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 IMU imu; public final Localizer localizer; public Pose2d pose; private final LinkedList 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; private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos; private Rotation2d lastHeading; 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)); // TODO: reverse encoders if needed leftBack.setDirection(DcMotorSimple.Direction.REVERSE); lastLeftFrontPos = leftFront.getPositionAndVelocity().position; lastLeftBackPos = leftBack.getPositionAndVelocity().position; lastRightBackPos = rightBack.getPositionAndVelocity().position; lastRightFrontPos = rightFront.getPositionAndVelocity().position; lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); } @Override public Twist2dDual update() { PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity(); PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity(); PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity(); PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity(); FlightRecorder.write("MECANUM_ENCODERS", new MecanumEncodersMessage( leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel)); Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); double headingDelta = heading.minus(lastHeading); Twist2dDual twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>( new DualNum(new double[]{ (leftFrontPosVel.position - lastLeftFrontPos), leftFrontPosVel.velocity, }).times(PARAMS.inPerTick), new DualNum(new double[]{ (leftBackPosVel.position - lastLeftBackPos), leftBackPosVel.velocity, }).times(PARAMS.inPerTick), new DualNum(new double[]{ (rightBackPosVel.position - lastRightBackPos), rightBackPosVel.velocity, }).times(PARAMS.inPerTick), new DualNum(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; LynxFirmware.throwIfModulesAreOutdated(hardwareMap); for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } leftFront = hardwareMap.get(DcMotorEx.class, "fl"); leftBack = hardwareMap.get(DcMotorEx.class, "bl"); rightBack = hardwareMap.get(DcMotorEx.class, "br"); rightFront = hardwareMap.get(DcMotorEx.class, "fr"); 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 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 imu = hardwareMap.get(IMU.class, "imu"); IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( PARAMS.logoFacingDirection, PARAMS.usbFacingDirection)); imu.initialize(parameters); voltageSensor = hardwareMap.voltageSensor.iterator().next(); localizer = new ThreeDeadWheelLocalizer(hardwareMap, PARAMS.inPerTick); FlightRecorder.write("MECANUM_PARAMS", PARAMS); } // perp: control hub 0 fr // parallel: expansion hub 0 fl // parallel: expansion hub 3 bl public void setDrivePowers(PoseVelocity2d powers) { MecanumKinematics.WheelVelocities wheelVels = new MecanumKinematics(1).inverse( PoseVelocity2dDual.constant(powers, 1)); double maxPowerMag = 1; for (DualNum 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 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; } Pose2dDual txWorldTarget = timeTrajectory.get(t); targetPoseWriter.write(new PoseMessage(txWorldTarget.value())); PoseVelocity2d robotVelRobot = updatePoseEstimate(); Pose2d error = txWorldTarget.value().minusExp(pose); if ((t >= timeTrajectory.duration && error.position.norm() < 2 && robotVelRobot.linearVel.norm() < 0.5) || t + 1 >= timeTrajectory.duration) { leftFront.setPower(0); leftBack.setPower(0); rightBack.setPower(0); rightFront.setPower(0); return false; } PoseVelocity2dDual 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 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())); 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"); drawRobot(c, txWorldTarget.value()); c.setStroke("#3F51B5"); 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 txWorldTarget = turn.get(t); targetPoseWriter.write(new PoseMessage(txWorldTarget.value())); PoseVelocity2d robotVelRobot = updatePoseEstimate(); PoseVelocity2dDual 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 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"); drawRobot(c, txWorldTarget.value()); c.setStroke("#3F51B5"); 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 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 static void drawRobot(Canvas c, Pose2d t) { final double ROBOT_RADIUS = 9; c.setStrokeWidth(1); c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS); Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS); Vector2d p1 = t.position.plus(halfv); Vector2d p2 = p1.plus(halfv); c.strokeLine(p1.x, p1.y, p2.x, p2.y); } public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) { return new TrajectoryActionBuilder( TurnAction::new, FollowTrajectoryAction::new, beginPose, 1e-6, 0.0, defaultTurnConstraints, defaultVelConstraint, defaultAccelConstraint, 0.25, 0.1 ); } } ```

ThreeDeadWheelLocalizer.java:

```java 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.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; @Config public final class ThreeDeadWheelLocalizer implements Localizer { public static class Params { public double par0YTicks = -962.0; // y position of the first parallel encoder (in tick units) public double par1YTicks = 912.0; // y position of the second parallel encoder (in tick units) public double perpXTicks = -2133.0; // x position of the perpendicular encoder (in tick units) } public static Params PARAMS = new Params(); public final Encoder par0, par1, perp; public final double inPerTick; private int lastPar0Pos, lastPar1Pos, lastPerpPos; 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 par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "fl"))); par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "bl"))); perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "fr"))); // TODO: reverse encoder directions if needed par0.setDirection(DcMotorSimple.Direction.REVERSE); perp.setDirection(DcMotorSimple.Direction.REVERSE); lastPar0Pos = par0.getPositionAndVelocity().position; lastPar1Pos = par1.getPositionAndVelocity().position; lastPerpPos = perp.getPositionAndVelocity().position; this.inPerTick = inPerTick; FlightRecorder.write("THREE_DEAD_WHEEL_PARAMS", PARAMS); } public Twist2dDual update() { PositionVelocityPair par0PosVel = par0.getPositionAndVelocity(); PositionVelocityPair par1PosVel = par1.getPositionAndVelocity(); PositionVelocityPair perpPosVel = perp.getPositionAndVelocity(); int par0PosDelta = par0PosVel.position - lastPar0Pos; int par1PosDelta = par1PosVel.position - lastPar1Pos; int perpPosDelta = perpPosVel.position - lastPerpPos; Twist2dDual twist = new Twist2dDual<>( new Vector2dDual<>( new DualNum(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(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; } } ```

Robot Logs

No response

[rbrott]

Does the same thing happen if the velocity is much lower? Can you link a robot log?

[nash-pillai]

It continued to happen even when we lowered to:

 public double maxWheelVel = 20;
 public double minProfileAccel = -10;
 public double maxProfileAccel = 10;

I can send robot logs tomorrow

[nash-pillai]

robotControllerLog.txt 2024_01_28__17_30_28_038__MenuAuto.log

I think these are the files you are looking for. Let me know if you want something else

I had to stop the opmode in the middle because it would have hit the edge of the field. If you need it to run all the way, we can remove the trusses and run it again on a clear field

[rbrott]

Yup, the second one is the log I need, thanks. Here's a summary of its contents

THREE_DEAD_WHEEL_PARAMS
  par0YTicks: -962.0
  par1YTicks: 912.0
  perpXTicks: -2133.0

MECANUM_PARAMS
  axialGain: 0.0
  axialVelGain: 0.0
  headingGain: 4.32
  headingVelGain: 0.59
  inPerTick: 0.0030135610246107484
  kA: 1e-06
  kS: 0.9589658474985132
  kV: 0.0005588422234744558
  lateralGain: 0.0
  lateralInPerTick: 0.00018869762188191035
  lateralVelGain: 0.0
  logoFacingDirection: UP
  maxAngAccel: 3.141592653589793
  maxAngVel: 3.141592653589793
  maxProfileAccel: 20.0
  maxWheelVel: 40.0
  minProfileAccel: -20.0
  trackWidthTicks: 4432.299
  usbFacingDirection: RIGHT

Channel                Messages         Hz
TARGET_POSE                  61      20.01
ESTIMATED_POSE               62      20.23
DRIVE_COMMAND                62      20.26
MECANUM_COMMAND              62      20.92

Right off the bat, lateralInPerTick seems very suspicious. It should be at most like 25% less than inPerTick and not an order of magnitude off. Can you try tuning that again and sharing the results here (including the JSON)?

[nash-pillai]

Thank you so much! I'm not how we messed it up in the first place, but re-tuning lateralInPerTick fixed our problems