Fixed turning in sim, attempted to fix encoder resetting (code stub)

Author: TKM-inShop

src/main/java/frc/robot/RobotContainer.java

@@ -222,6 +222,8 @@ public RobotContainer() {
     autoChooser.addOption(
         "middle score encoder",
         new SequentialCommandGroup(
+            // Resets drive encoders
+            drive.resetEncoders(),
             // Drive 2.23 meters forward
             DriveCommands.arcadeDrive(drive, () -> 0.5, () -> 0.0)
                 .until(() -> drive.getLeftPositionMeters() >= 2.23),

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -23,12 +23,14 @@
 import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
@@ -98,6 +100,14 @@ public void periodic() {
     Logger.processInputs("Drive", inputs);
 
     // Update gyro angle
+    // Use the angle delta from the kinematics and module deltas
+    Twist2d twist =
+        kinematics.toTwist2d(
+            getLeftPositionMeters() - lastLeftPositionMeters,
+            getRightPositionMeters() - lastRightPositionMeters);
+    rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
+    lastLeftPositionMeters = getLeftPositionMeters();
+    lastRightPositionMeters = getRightPositionMeters();
 
     // Update odometry
     poseEstimator.update(rawGyroRotation, getLeftPositionMeters(), getRightPositionMeters());
@@ -171,13 +181,13 @@ public void addVisionMeasurement(Pose2d visionPose, double timestamp) {
   /** Returns the position of the left wheels in meters. */
   @AutoLogOutput
   public double getLeftPositionMeters() {
-    return inputs.leftPositionRad * wheelRadiusMeters;
+    return inputs.leftPositionRad * wheelRadiusMeters - this.offsetLeft;
   }
 
   /** Returns the position of the right wheels in meters. */
   @AutoLogOutput
   public double getRightPositionMeters() {
-    return inputs.rightPositionRad * wheelRadiusMeters;
+    return inputs.rightPositionRad * wheelRadiusMeters - this.offsetRight;
   }
 
   /** Returns the velocity of the left wheels in meters/second. */
@@ -196,4 +206,17 @@ public double getRightVelocityMetersPerSec() {
   public double getCharacterizationVelocity() {
     return (inputs.leftVelocityRadPerSec + inputs.rightVelocityRadPerSec) / 2.0;
   }
+
+  private double offsetLeft = 0.0;
+  private double offsetRight = 0.0;
+
+  public Command resetEncoders() {
+    return Commands.runOnce(
+        () -> {
+          io.resetEncoders();
+          this.offsetLeft += this.getLeftPositionMeters();
+          this.offsetRight += this.getRightPositionMeters();
+        },
+        this);
+  }
 }

src/main/java/frc/robot/subsystems/drive/DriveIO.java

@@ -38,4 +38,6 @@ public default void setVoltage(double leftVolts, double rightVolts) {}
   /** Run closed loop at the specified velocity. */
   public default void setVelocity(
       double leftRadPerSec, double rightRadPerSec, double leftFFVolts, double rightFFVolts) {}
+
+  public default void resetEncoders() {}
 }