Fix odometry to use meters as units

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -283,10 +283,9 @@ public class Drive extends SubsystemBase {
       // e.printStackTrace(System.err);
     }
 
-    m_odometry.update(Rotation2d.fromDegrees(getHeading()),
-        m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition(),
-        m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition());
-
+    m_odometry.update(Rotation2d.fromDegrees( getHeading()),
+                                              inchesToMeters(getDistanceInches(m_leftFrontMotor)),
+                                              inchesToMeters(getDistanceInches(m_rightFrontMotor)));
   }
 
   /**
@@ -472,7 +471,6 @@ public class Drive extends SubsystemBase {
 
   /**
    * Returns the heading of the robot
-   * 
    * @return The robot's heading in degrees, from -180 to 180
    */
   public double getHeading() {
@@ -492,8 +490,8 @@ public class Drive extends SubsystemBase {
    * @return The current wheel speeds.
    */
   public DifferentialDriveWheelSpeeds getWheelSpeeds() {
-    return new DifferentialDriveWheelSpeeds(  m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity(), 
-                                              m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
+    return new DifferentialDriveWheelSpeeds(  inchesToMeters(getVelocityInchesPerSecond(m_leftFrontMotor)), 
+                                              inchesToMeters(getVelocityInchesPerSecond(m_rightFrontMotor)));
   }
 
   /**
@@ -523,6 +521,15 @@ public class Drive extends SubsystemBase {
     return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition());
   }
 
+  /**
+   * Gets the encoder value (velocity) of a motor
+   * @param falcon The motor to get the velocity of
+   * @return The velocity of the motor in inches per second
+   */
+  public double getVelocityInchesPerSecond(WPI_TalonFX falcon) {
+    return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition()/DriveConstants.TICK_TIME_TO_SECONDS);
+  }
+
   /**
    * Converts a value in ticks to inches.
    * @param ticks The value in ticks to convert
@@ -531,6 +538,15 @@ public class Drive extends SubsystemBase {
   public double ticksToInches(double ticks) {
     return ticks * DriveConstants.INCHES_PER_TICK;
   }
+
+  /**
+   * Converts a value in inches to meters.
+   * @param inches The value in inches to convert
+   * @return The converted value in meters
+   */
+  public double inchesToMeters(double inches) {
+    return inches / DriveConstants.INCHES_PER_METER;
+  }
   
   /*
    * Set to high or low gear based on boolean state, true = high, false = low