Swerve things

networktables.json

@@ -0,0 +1 @@
+[]

src/main/java/frc/robot/Constants.java

@@ -22,7 +22,7 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final Mode currentMode = Mode.REAL;
+  public static final Mode currentMode = Mode.SIM;
 
   public static enum Mode {
     /** Running on a real robot. */

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

@@ -151,6 +151,7 @@ private void configureButtonBindings() {
             () -> -controller.getLeftX(),
             () -> -controller.getRightX()));
     controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
+
     controller
         .b()
         .onTrue(
@@ -160,11 +161,13 @@ private void configureButtonBindings() {
                             new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
                     drive)
                 .ignoringDisable(true));
-    controller
-        .a()
-        .whileTrue(
-            Commands.startEnd(
-                () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
+    // controller
+    //     .a()
+    //     .whileTrue(
+    //         Commands.startEnd(
+    //             () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
+
+    controller.a().whileTrue(Commands.runOnce(() -> drive.zeroHeading(), drive));
   }
 
   /**

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

@@ -35,6 +35,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Constants;
+import frc.robot.Constants.Mode;
 import frc.robot.util.LocalADStarAK;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -203,6 +205,22 @@ public void stopWithX() {
     stop();
   }
 
+  public void zeroHeading() {
+    if (Constants.currentMode == Mode.SIM) {
+      poseEstimator.resetPosition(
+          new Rotation2d(),
+          new SwerveModulePosition[] {
+            modules[0].getPosition(),
+            modules[1].getPosition(),
+            modules[2].getPosition(),
+            modules[3].getPosition()
+          },
+          getPose());
+    }
+
+    gyroIO.reset();
+  }
+
   /** Returns a command to run a quasistatic test in the specified direction. */
   public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
     return sysId.quasistatic(direction);

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

@@ -24,5 +24,7 @@ public static class GyroIOInputs {
     public double yawVelocityRadPerSec = 0.0;
   }
 
+  public default void reset() {}
+
   public default void updateInputs(GyroIOInputs inputs) {}
 }

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

@@ -35,6 +35,11 @@ public GyroIOPigeon2() {
     pigeon.optimizeBusUtilization();
   }
 
+  @Override
+  public void reset() {
+    pigeon.reset();
+  }
+
   @Override
   public void updateInputs(GyroIOInputs inputs) {
     inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK);

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

@@ -14,6 +14,7 @@
 package frc.robot.subsystems.drive;
 
 import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
@@ -40,7 +41,7 @@ public class ModuleIOSparkMax implements ModuleIO {
   private static final double TURN_GEAR_RATIO = 150.0 / 7.0;
 
   private final CANSparkMax driveSparkMax;
-  private final CANSparkMax turnSparkMax;
+  private final CANSparkFlex turnSparkMax;
 
   private final RelativeEncoder driveEncoder;
   private final RelativeEncoder turnRelativeEncoder;
@@ -53,25 +54,25 @@ public ModuleIOSparkMax(int index) {
     switch (index) {
       case 0:
         driveSparkMax = new CANSparkMax(1, MotorType.kBrushless);
-        turnSparkMax = new CANSparkMax(2, MotorType.kBrushless);
+        turnSparkMax = new CANSparkFlex(2, MotorType.kBrushless);
         turnAbsoluteEncoder = new AnalogInput(0);
         absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;
       case 1:
         driveSparkMax = new CANSparkMax(3, MotorType.kBrushless);
-        turnSparkMax = new CANSparkMax(4, MotorType.kBrushless);
+        turnSparkMax = new CANSparkFlex(4, MotorType.kBrushless);
         turnAbsoluteEncoder = new AnalogInput(1);
         absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;
       case 2:
         driveSparkMax = new CANSparkMax(5, MotorType.kBrushless);
-        turnSparkMax = new CANSparkMax(6, MotorType.kBrushless);
+        turnSparkMax = new CANSparkFlex(6, MotorType.kBrushless);
         turnAbsoluteEncoder = new AnalogInput(2);
         absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;
       case 3:
         driveSparkMax = new CANSparkMax(7, MotorType.kBrushless);
-        turnSparkMax = new CANSparkMax(8, MotorType.kBrushless);
+        turnSparkMax = new CANSparkFlex(8, MotorType.kBrushless);
         turnAbsoluteEncoder = new AnalogInput(3);
         absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
         break;

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -23,4 +23,9 @@ private Shooter(ShooterIO shooterIO) {
     io = shooterIO;
     io.updateInputs(inputs);
   }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+  }
 }

src/main/java/frc/robot/subsystems/shooter/ShooterIO.java

@@ -1,20 +1,18 @@
 package frc.robot.subsystems.shooter;
 
-import org.littletonrobotics.junction.AutoLog;
-
 import frc.robot.subsystems.shooter.ShooterIO.ShooterIOInputs;
+import org.littletonrobotics.junction.AutoLog;
 
 public interface ShooterIO {
-    @AutoLog
-    public static class ShooterIOInputs{
-        public double leftMotorVelocity = 0.0;
-        public double rightMotorVelocity = 0.0;
-    }
-
-    public default void spinForwards() {}
+  @AutoLog
+  public static class ShooterIOInputs {
+    public double leftMotorVelocity = 0.0;
+    public double rightMotorVelocity = 0.0;
+  }
 
-    public default void spinBackwards() {}
+  public default void spinForwards() {}
 
-    public default void updateInputs(ShooterIOInputs inputs) {}
+  public default void spinBackwards() {}
 
+  public default void updateInputs(ShooterIOInputs inputs) {}
 }