Wrong Swerve Module Order in m_odometry.Update on line 35

[rtsledge]

The order in which the swerve modules are passed to the odometry update function are in a different order then they are create in the SwerveDriveKinematics object.

They are created with the order of front left, front right, rear left, rear right.

frc::SwerveDriveKinematics<4> kDriveKinematics{
frc::Translation2d{DriveConstants::kWheelBase / 2, DriveConstants::kTrackWidth / 2},
frc::Translation2d{DriveConstants::kWheelBase / 2, -DriveConstants::kTrackWidth / 2},
frc::Translation2d{-DriveConstants::kWheelBase / 2, DriveConstants::kTrackWidth / 2},
frc::Translation2d{-DriveConstants::kWheelBase / 2, -DriveConstants::kTrackWidth / 2}};

When the odometry is updated, they are passed in the order front left, rear left, front right, rear right:

m_odometry.Update(frc::Rotation2d(units::radian_t{ m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)}),
{m_frontLeft.GetPosition(), m_rearLeft.GetPosition(),
m_frontRight.GetPosition(), m_rearRight.GetPosition()});

wpilib documentation states the below about the modulePositions parameter:

"modulePositions: The current position of all swerve modules. Please provide the positions in the same order in which you instantiated your [SwerveDriveKinematics]"

◆ Update()

template<size_t NumModules>

const Pose2d & frc::SwerveDriveOdometry< NumModules >::Update(const Rotation2d & gyroAngle,const wpi::array< SwerveModulePosition, NumModules > & modulePositions ) | const Pose2d & frc::SwerveDriveOdometry< NumModules >::Update | ( | const Rotation2d & | gyroAngle, |   |   | const wpi::array< SwerveModulePosition, NumModules > & | modulePositions |   | ) |   |   | inline -- | -- | -- | -- | -- | -- | -- | -- | -- | -- | -- | -- | -- | -- const Pose2d & frc::SwerveDriveOdometry< NumModules >::Update | ( | const Rotation2d & | gyroAngle,   |   | const wpi::array< SwerveModulePosition, NumModules > & | modulePositions   | ) |   |  

Returns

The new pose of the robot.

◆ Update()
template<size_t NumModules>
const Pose2d & frc::SwerveDriveOdometry< NumModules >::Update ( const Rotation2d & gyroAngle,
const wpi::array< SwerveModulePosition, NumModules > & modulePositions
)
inline
Updates the robot's position on the field using forward kinematics and integration of the pose over time.

This also takes in an angle parameter which is used instead of the angular rate that is calculated from forward kinematics.

Parameters
gyroAngle The angle reported by the gyroscope.
modulePositions The current position of all swerve modules. Please provide the positions in the same order in which you instantiated your SwerveDriveKinematics.
Returns
The new pose of the robot.