Coaxial Swerve Tuning

Setup

Swerve is currently not supported by Pedro Pathing's AutoTune software. This means users will tune the SwerveConfig themselves, and then will use AutoTune to tune their algorithm's constants.

Add the Config

Inside your Constants class, add:

public static SwerveConfig driveConfig = new SwerveConfig(
            c -> {
                c.zeroPowerBehavior.set(SwerveConfig.ZeroPowerBehavior.IGNORE_ANGLE_CHANGES);
                c.manualBrakeMode.set(true);
                c.voltageCompensation.set(false);
            }
    );

public static CoaxialPodConfig rightBack = new CoaxialPodConfig(
            c -> {
                c.name.set("rightBack");
                c.motorName.set("rb");
                c.servoName.set("rbTurn");
                c.servoEncoderName.set("rbTurnEncoder");
                c.turnController.set(Controller.pid(0.3, 0, 0.005)
                        .plus(Controller.proportionalFeedforward(0)));
                c.driveDirection.set(DcMotorSimple.Direction.FORWARD);
                c.servoDirection.set(DcMotorSimple.Direction.FORWARD);
            }
    );

public static CoaxialPodConfig leftFront = new CoaxialPodConfig(
            c -> {
                c.name.set("leftFront");
                c.motorName.set("lf");
                c.servoName.set("lfTurn");
                c.servoEncoderName.set("lfTurnEncoder");
                c.turnController.set(Controller.pid(0.3, 0, 0.005)
                        .plus(Controller.proportionalFeedforward(0)));
                c.driveDirection.set(DcMotorSimple.Direction.FORWARD);
                c.servoDirection.set(DcMotorSimple.Direction.FORWARD);
            }
    );

    public static CoaxialPodConfig rightFront = new CoaxialPodConfig(
            c -> {
                c.name.set("rightFront");
                c.motorName.set("rf");
                c.servoName.set("rfTurn");
                c.servoEncoderName.set("rfTurnEncoder");
                c.turnController.set(Controller.pid(0.3, 0, 0.005)
                        .plus(Controller.proportionalFeedforward(0)));
                c.driveDirection.set(DcMotorSimple.Direction.FORWARD);
                c.servoDirection.set(DcMotorSimple.Direction.FORWARD);
            }
    );

    public static CoaxialPodConfig leftBack = new CoaxialPodConfig(
            c -> {
                c.name.set("leftBack");
                c.motorName.set("lb");
                c.servoName.set("lbTurn");
                c.servoEncoderName.set("lbTurnEncoder");
                c.turnController.set(Controller.pid(0.3, 0, 0.0086)
                        .plus(Controller.proportionalFeedforward(0)));
                c.driveDirection.set(DcMotorSimple.Direction.FORWARD);
                c.servoDirection.set(DcMotorSimple.Direction.FORWARD);
            }
    );

Make sure to replace the motorName, servoName, and servoEncoder name to match your config.

Analog Encoder Min / Max Voltage

To get the best performance out of your drivetrain, you need to be able to get the angle of your servos to high precision. Most coaxial swerve drivetrains in FTC use the analog encoders on their rotation servos to get the angle. In theory, the analog values should vary between 0 and 3.3V, but in practice the min and max voltages are often close but not exactly those.

To get the most precision out of your pods, you want to get the actual minimum and maximum voltages of your encoders. To do this, run the Analog Min / Max Tuner opmode. After you press start, slowly rotate each pod four to five full rotations. The min and max voltages of each pod should be printed to telemetry.

Once you have the values for each pod, add the following lines to each of your CoaxialPodConfig objects:

c.analogMinVoltage.set(min);
c.analogMaxVoltage.set(max);

Angle Offset Tuning

Next, we need to tune the angle offset of each pod. After you have tuned the min and max voltages, a full rotation of each pod should map to exactly 360 degrees. However, the code still needs to know the precise angle at a specific position. To do this, you add angle offsets to each pod, which is the raw angle of the pod in radians when they are at your desired zero position (facing forward).

To tune this, first make sure the zero power behavior is set to IGNORE_ANGLE_CHANGES. Then, run LocalizationTest, making sure to enable the drivetrain debug string by pressing a on the gamepad when prompted. Then, manually line up each pod to your zero position, facing the pod forward. The raw angle of each pod will be printed to telemetry. Insert the raw angle (in radians) when the pod is at the zero position as the angle offset for that pod.

Once you have the values for each pod, add the following lines to each of your CoaxialPodConfig objects:

c.angleOffsetRad.set(offset_in_radians);

Motor Directions

After you put your angle offsets in, we need to confirm that they are correct and that the motors are reversed correctly. First, if you haven't already, put in the placeholder pod PIDF coefficients for each pod (kP = 0.3, kI = 0, kD = 0.005, kF = 0) --- this was already done in the first step if you've been following along. Then, run the SwerveOffsetsTuner opmode (do this off the ground, the servos and motors will spin). This opmode will attempt to drive each pod to its zero position. However, there are a few expected reasons for this to fail. Follow these debug steps for each type of failure:

1. The pods oscillate a lot: By far the most common issue here is that your servos (not encoders) need to be reversed. Reverse your servos and try again. If the issue persists, try turning down the default PIDF coefficients and see if that helps.
2. The servos move to the wrong position: First, check that the servos are holding their positions. It's possible that they are trying to move to the zero position but the default PIDF coefficients aren't strong enough to get them there. Give them a nudge towards their zero positions and see if they move easily towards it. If so, turn up the PIDF coefficients and try again. Otherwise, you probably set up bad angle offsets. Repeat the Angle Offset Tuning steps above to make sure they are correct. If neither of those steps work, it's likely that the servos are not plugged in correctly to their corresponding encoders. Ensure that the servo and encoder ports match up.
3. The servos don't move at all: If the servos don't move at all, it's possible that the servos aren't plugged in correctly or that the pod PIDF coefficients are too low. Make sure that they are plugged in correctly, and then try turning up the PIDF coefficients and see if that helps.

If none of the above steps get the servos move to their zero positions, then you probably have an issue with wiring. Go through and make sure that all of your servo, encoder, and motor wires are plugged in to the correct ports.

Once all your servos move to their zero positions, now you can check the directions of your motors. The pods should all be rotating to move the robot forward (regardless of whether the pods are facing forward or backward). Reverse all the motors in pods that are turning the wrong direction. You can do this by changing the corresponding direction in the CoaxialPodConfig.

If you have completed all of the above steps correctly, when running SwerveOffsetsTuner, the robot should drive forward slowly when you put it on the ground.

Lateral Offsets

Now you need to find in your lateral offset (as a Vector2D). These not tuned, but found through measurement on the robot or CAD. Lateral offsets represent the relative position of the pod to the center of the robot using pedro's coordinate system (positive x is forward, positive y is left). The nice thing about these is that the offsets are normalized, meaning only the relative x and y values matter (i.e. Vector2D.cartesian(1,1) and Vector2D.cartesian(1000, 1000) are the same). Thus, I've found that the easiest way to do these is to measure the length and width of your robot drivetrain. If you do that and store the values in two variables dtLength and dtWidth, your offsets will be as follows (assuming your swerve is rectangular):

• Front Left: Vector2D.cartesian(dtLength, dtWidth)
• Front Right: Vector2D.cartesian(dtLength, -dtWidth)
• Back Left: Vector2D.cartesian(-dtLength, dtWidth)
• Back Right: Vector2D.cartesian(-dtLength, -dtWidth)

I'm super specific with these is because these are easy to get right if you're appropriately careful but annoying to debug if you mess them up.

Once you have the values for each pod, add the following lines to each of your CoaxialPodConfig objects:

c.podOffset.set(Vector2D.cartesian(offsetX, offsetY));

Encoder Directions

Next, we need to confirm that the encoders are reversed correctly. First, take your robot off of the ground such that you can see the wheels: then, run the SwerveTurnTuner opmode. Your pods should turn to be perpendicular to the center of the robot, forming an "O" shape so the robot can turn. If your pods all turn towards the center and form an "X" shape, then reverse all of your encoders (flip the boolean parameter) and try again.

Once your pods face the correct direction, ensure that the motors are all running the correct direction to turn the robot counterclockwise. If any of the motors are running the wrong direction, but they are all correct for the SwerveOffsetsTuner to move your robot forward, then you almost certainly have a wiring issue. Make sure that all of your motors correctly match up with their corresponding servos and encoders in your config. If that still doesn't work, check your lateral offsets.

Pod PIDF Tuning

Finally, you need to tune your pod PIDF coefficients. Run the SwervePIDTuner opmode to tune your PIDs. You can do this using whatever method you prefer to tune your PIDFs. What I've found to work best is to first tune P and D while the robot is off the ground. Try to increase P until you get something decently aggressive without introducing too many oscillations, and then raise D to mitigate any remaining oscillation. Once you've tuned to your satisfaction, move the robot onto the ground and tune F to account for the friction with the tiles and fine tuning P and D. For my bot, I've used the same P and D for all four pods, but I have separate F values for the front and back pods because my robot is back heavy. You should adjust this for your robot's weight distribution, and some teams prefer to tune the P, D, and F values for each pod individually.

Tips for Tuning

Congratulations! You've now completed the swerve-specific tuning steps. You can now move onto localization tuning and the rest of standard pedro tuning.

Some tips for the getting the most out of tuning:

1. When you get to the ForwardVelocityIdentification and ForwardDecelerationIdentification tuners, you don't need to do the lateral variants because with swerve they are equivalent. You can just use the same values for both.
2. You need to retune your natural acceleration if you switch from using X-lock to not using it and vice versa. When using X-lock, the robot can decelerate much faster than without (the difference was about -200 vs -130 in my case), and this affects the robot's ability to stop at the correct position. I would recommend using X-lock unless you have a reason not to.
3. Using negative powers for braking may be too aggressive on swerve drives. You may receive rapid oscillations or other signs that the braking is not working. If you are using Foresight and this occurs, I recommend setting a maximum deceleration constraint in your ForesightConfig with c.maxDecelerationConstraint.set(naturalDeceleration);, where naturalDeceleration is the measuredDeceleration you inputted for your naturalForwardDeceleration and naturalStrafeDeceleration.
4. The PID values produced by AutoTune were tuned primarily for mecanum drives, and may not be perfect for swerve drives; you may need to adjust them and test with HoldTest. In general, stay conservative with your heading and translational PIDs. While slightly higher values can make the robot more responsive, oscillations when driving slow down the robot significantly, so I've found that getting decent responsiveness while minimizing oscillations is the most effective tuning strategy. On that note, I would highly recommend using secondary PIDs for all three pathing PIDs. If your secondaries are decently less aggressive than your primaries, you can significantly improve responsiveness while minimizing oscillations.

There is also a swerve constants reference page that you can use to see a fully setup swerve config. Finally, if you have any other questions, please feel free to reach out to me (@_kubar) on discord through the Pedro discord server (preferred) or DMs.