Swerve: driveRateBackOut might need to be added instead of subtracted

[TheTripleV]

forward from https://discord.com/channels/176186766946992128/368993897495527424/1199931798621261884

from @rzblue

I think there might be a mistake in the coupling correction code in phoenix 6's SwerveModule. Here's a derivation to help explain: P_raw = drive motor position in rotations P_comp = drive motor position, compensated for coupling error (ie, the true distance travelled by the wheel, in units of motor rotations) V_raw = drive motor velocity in rotations per second V_comp = drive motor velocity, compensated for coupling error (ie, the true velocity of the wheel, in units of motor rotations per second) azimuth_rot = rotations of azimuth azimuth_vel = velocity of azimuth in rotations per second coupling_ratio = drive motor rotations per rotation of azimuth

P_comp = P_raw - (azimuth_rot * coupling_ratio)

We can extend this to velocity like so:

V_comp = V_raw - (azimuth_vel * coupling_ratio)

We then invert the function to solve for V_raw given V_comp (in the case of converting from a desired wheel speed to the necessary motor speed)

V_raw = V_comp + (azimuth_vel * coupling_ratio)

The first function matches the encoder correction implementation on line 250 of SwerveModule:
drive_rot -= angle_rot * m_couplingRatioDriveRotorToCANcoder;

However the inverse equation doesn't match the drive velocity correction implementation on line 324-327. Instead of adding the correction factor (azimuth_vel * coupling_ratio) to the desired speed V_comp, it is subtracted:

double azimuthTurnRps = m_steerVelocity.getValue();
/* Azimuth turn rate multiplied by coupling ratio provides back-out rps */
double driveRateBackOut = azimuthTurnRps * m_couplingRatioDriveRotorToCANcoder;
velocityToSet -= driveRateBackOut;

This would have the effect of doubling the coupling error when setting velocity.

[rzblue]

I tested today- adding the correction velocity is the correct behavior, subtracting doubles the velocity error

[TheTripleV]

Thanks!