Pose offset

[amparks100]

During testing on Tuesday 1/30, we noticed that the pose on Glass seemed to be off by approximately the length of the robot. especially in the center of the field & blue side.

We need this to be accurate so that autonomous functions run the way we want them to.

[amparks100]

Notes I have while reading through our code:

• When we initialize our SwerveDriveOdometry, we arent setting an initial position on the field. Our initialize vs WPIlib docs
• But also we seem to match https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator
• Something kinda weird i saw is in wpi's example code, for getPosition, they are doing m_driveEncoder.getDistance and we are doing m_driveEncoder.getPosition
• Same with our getState using different encoder methods
• Maybe lets try with their std deviations? https://github.com/wpilibsuite/allwpilib/blob/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java#L53