odo auto

src/main/deploy/pathplanner/Move Forward.path

@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":2.0390109745736735,"y":2.9639231692256494},"prevControl":null,"nextControl":{"x":2.049521340421991,"y":2.9534128033773315},"holonomicAngle":1.2188752351312955,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":5.0029341437993216,"y":2.995454266770603},"nextControl":null,"holonomicAngle":-178.60254149056965,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
+{"waypoints":[{"anchorPoint":{"x":2.0390109745736735,"y":2.0},"prevControl":null,"nextControl":{"x":2.049521340421991,"y":1.9894896341516821},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.0,"y":2.0},"prevControl":{"x":7.00293414379928,"y":1.995454266770603},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}

src/main/java/frc4388/robot/Constants.java

@@ -30,8 +30,8 @@ public final class Constants {
   public static final class SwerveDriveConstants {
     public static final double ROTATION_SPEED = 4;
     public static final double WHEEL_SPEED = 0.1;
-    public static final double WIDTH = 15.27;
+    public static final double WIDTH = 15.25;
-    public static final double HEIGHT = 15.27;
+    public static final double HEIGHT = 15.25;
     public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
     public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
     public static final double MAX_SPEED_FEET_PER_SEC = 20; // redundant constant?
@@ -80,7 +80,7 @@ public final class Constants {
     // wheel diameter: official = 4 in, measured = 3.8 in
     /* Ratio Calculation */
     public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
-    public static final double WHEEL_DIAMETER_INCHES = 3.8;
+    public static final double WHEEL_DIAMETER_INCHES = 4.0;
     public static final double TICKS_PER_MOTOR_REV = 2048;
     public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
     public static final double INCHES_PER_METER = 39.370;

src/main/java/frc4388/robot/RobotContainer.java

@@ -105,7 +105,7 @@ public class RobotContainer {
     
     new JoystickButton(getDriverController(), XboxController.Button.kA.value)
 
-      .whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
+      .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
       //.whenPressed(this::resetOdometry);
 
     /* Operator Buttons */
@@ -192,7 +192,7 @@ public class RobotContainer {
     return m_robotSwerveDrive.getOdometry();
   }
 
-  public void zeroOdometry(Pose2d pose) {
+  public void resetOdometry(Pose2d pose) {
     m_robotSwerveDrive.resetOdometry(pose);
   }
   /**

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

@@ -14,6 +14,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
@@ -49,6 +50,7 @@ public class SwerveDrive extends SubsystemBase {
   /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
   below are robot specific, and should be tuned. */
   public SwerveDrivePoseEstimator m_poseEstimator;
+  public SwerveDriveOdometry m_odometry;
 
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   public boolean ignoreAngles;
@@ -88,9 +90,12 @@ public class SwerveDrive extends SubsystemBase {
           m_gyro.getRotation2d(),
           new Pose2d(),
           m_kinematics,
-          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+          VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
-          VecBuilder.fill(Units.degreesToRadians(0.01)),
+          VecBuilder.fill(Units.degreesToRadians(1)),
-          VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+          VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
+
+    m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
+
     m_gyro.reset(); 
     SmartDashboard.putData("Field", m_field);
   }
@@ -150,7 +155,7 @@ public class SwerveDrive extends SubsystemBase {
   
   @Override
   public void periodic() {
-    //System.err.println(m_gyro.getFusedHeading() +"    aaa");
+    // System.err.println(m_gyro.getFusedHeading() +"    aaa");
     updateOdometry();
     SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
     SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
@@ -197,6 +202,7 @@ public class SwerveDrive extends SubsystemBase {
    * @return Robot's current pose.
    */
   public Pose2d getOdometry() {
+    // return m_odometry.getPoseMeters();
     return m_poseEstimator.getEstimatedPosition();
   }
 
@@ -204,6 +210,7 @@ public class SwerveDrive extends SubsystemBase {
    * Resets the odometry of the robot to (x=0, y=0, theta=0).
    */
   public void resetOdometry(Pose2d pose) {
+    // m_odometry.resetPosition(pose, m_gyro.getRotation2d());
     m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
   }
 
@@ -217,6 +224,12 @@ public class SwerveDrive extends SubsystemBase {
                             modules[2].getState(), 
                             modules[3].getState());
     
+    // m_odometry.update( m_gyro.getRotation2d(), 
+    //                    modules[0].getState(),
+    //                    modules[1].getState(),
+    //                    modules[2].getState(),
+    //                    modules[3].getState());
+  
       // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
       // a real robot, this must be calculated based either on latency or timestamps.
       // m_poseEstimator.addVisionMeasurement(

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

@@ -91,7 +91,7 @@ public class SwerveModule extends SubsystemBase {
 
 
     double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
-    driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
+    driveMotor.set(/*angleMotor.get() + */feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
   }
 
   /**