Dead Swerve module fix, the funky shooter feedforward solution made proper, and code cleanup 🐀⚡️🌧🥖🍲🗼🛵🍾👨‍👦‍👦⛑🧛‍♂️🤰✍️🎉🚨📰👨‍🍳

src/main/java/frc/team2412/robot/Robot.java

@@ -310,4 +310,11 @@ public void disabledInit() {
             subsystems.shooterSubsystem.stopFlywheel();
         }
     }
+
+    @Override
+    public void disabledPeriodic() {
+        if (subsystems.drivebaseSubsystem != null) {
+            subsystems.drivebaseSubsystem.setToX();
+        }
+    }
 }

src/main/java/frc/team2412/robot/Subsystems.java

@@ -12,7 +12,7 @@ public static class SubsystemConstants {
         public static final boolean POST_CLIMB_ENABLED = false;
 
         public static final boolean DRIVE_ENABLED = true;
-        public static final boolean DRIVER_VIS_ENABLED = true;
+        public static final boolean DRIVER_VIS_ENABLED = false;
         public static final boolean SHOOTER_VISION_ENABLED = true;
         public static final boolean INDEX_ENABLED = true;
         public static final boolean INTAKE_ENABLED = true;

src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java

@@ -16,6 +16,7 @@
 import org.frcteam2910.common.math.Vector2;
 
 import frc.team2412.robot.commands.shooter.ShooterTargetCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 import frc.team2412.robot.subsystem.IndexSubsystem;
 import frc.team2412.robot.subsystem.IntakeSubsystem;
@@ -29,8 +30,8 @@ public class JackFiveBallAutoCommand extends SequentialCommandGroup implements U
     public static class FiveBallConstants {
         public static final TrajectoryConstraint[] NORMAL_SPEED = {
                 new FeedforwardConstraint(11.0,
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
                 // value
                 // was
                 // 11.0
@@ -41,8 +42,8 @@ public static class FiveBallConstants {
 
         public static final TrajectoryConstraint[] FAST_SPEED = {
                 new FeedforwardConstraint(11.0,
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
                 // value
                 // was
                 // 11.0

src/main/java/frc/team2412/robot/commands/autonomous/JackStealFourBallAutoCommand.java

@@ -15,6 +15,7 @@
 import frc.team2412.robot.commands.intake.IntakeSetInCommand;
 import frc.team2412.robot.commands.intake.IntakeSetOutCommand;
 import frc.team2412.robot.commands.shooter.ShooterTargetCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 import frc.team2412.robot.subsystem.IndexSubsystem;
 import frc.team2412.robot.subsystem.IntakeSubsystem;
@@ -28,8 +29,8 @@ public class JackStealFourBallAutoCommand extends DynamicRequirementSequentialCo
     public static class StealFourBallConstants {
         public static final TrajectoryConstraint[] NORMAL_SPEED = {
                 new FeedforwardConstraint(11.0,
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
                 // value
                 // was
                 // 11.0

src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallAutoCommand.java

@@ -14,6 +14,7 @@
 import frc.team2412.robot.commands.index.IndexSpitCommand;
 import frc.team2412.robot.commands.intake.IntakeSetInCommand;
 import frc.team2412.robot.commands.shooter.ShooterTargetCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 import frc.team2412.robot.subsystem.IndexSubsystem;
 import frc.team2412.robot.subsystem.IntakeSubsystem;
@@ -27,8 +28,8 @@ public class JackStealThreeBallAutoCommand extends DynamicRequirementSequentialC
     public static class StealThreeBallConstants {
         public static final TrajectoryConstraint[] NORMAL_SPEED = {
                 new FeedforwardConstraint(11.0,
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
                 // value
                 // was
                 // 11.0

src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallCompatAutoCommand.java

@@ -14,6 +14,7 @@
 import frc.team2412.robot.commands.index.IndexSpitCommand;
 import frc.team2412.robot.commands.intake.IntakeSetInCommand;
 import frc.team2412.robot.commands.shooter.ShooterTargetCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 import frc.team2412.robot.subsystem.IndexSubsystem;
 import frc.team2412.robot.subsystem.IntakeSubsystem;
@@ -28,8 +29,8 @@ public class JackStealThreeBallCompatAutoCommand extends DynamicRequirementSeque
     public static class StealThreeBallConstants {
         public static final TrajectoryConstraint[] NORMAL_SPEED = {
                 new FeedforwardConstraint(11.0,
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
-                        DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(),
+                        Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old
                 // value
                 // was
                 // 11.0

src/main/java/frc/team2412/robot/commands/autonomous/OneBallAutoCommand.java

@@ -17,7 +17,7 @@ public OneBallAutoCommand(IndexSubsystem indexSubsystem, ShooterSubsystem shoote
                 new SimplePathBuilder(Vector2.ZERO, Rotation2.ZERO)
                         .lineTo(new Vector2(0, 70))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         ShooterTargetCommand.TurretManager manager = new ShooterTargetCommand.TurretManager(shooterSubsystem,
                 localizer);

src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandLeft.java

@@ -23,7 +23,7 @@ public TwoBallAutoCommandLeft(IndexSubsystem indexSubsystem, ShooterSubsystem sh
                         .lineTo(new Vector2(429.597, 88.203), Rotation2.fromDegrees(315))
                         .lineTo(new Vector2(426.841, 94.110), Rotation2.fromDegrees(234))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         addCommands(
                 new IntakeSetExtendCommand(intakeSubsystem),

src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandMiddle.java

@@ -22,7 +22,7 @@ public TwoBallAutoCommandMiddle(IndexSubsystem indexSubsystem, ShooterSubsystem
                         .lineTo(new Vector2(426.405, 210.657), Rotation2.fromDegrees(25))
                         .lineTo(new Vector2(420, 215), Rotation2.fromDegrees(-65))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         ShooterTargetCommand.TurretManager manager = new ShooterTargetCommand.TurretManager(shooterSubsystem,
                 localizer);

src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandRight.java

@@ -24,7 +24,7 @@ public TwoBallAutoCommandRight(IndexSubsystem indexSubsystem, ShooterSubsystem s
                         .lineTo(new Vector2(337.850, 290.717), Rotation2.fromDegrees(90))
                         .lineTo(new Vector2(337.850, 287), Rotation2.fromDegrees(0))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         addCommands(
                 new IntakeSetExtendCommand(intakeSubsystem),

src/main/java/frc/team2412/robot/commands/autonomous/debug/LinePath.java

@@ -7,6 +7,7 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.team2412.robot.commands.autonomous.Follow2910TrajectoryCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 
 public class LinePath extends SequentialCommandGroup {
@@ -15,7 +16,7 @@ public LinePath(DrivebaseSubsystem drivebaseSubsystem) {
                 new SimplePathBuilder(new Vector2(0, 0), Rotation2.ZERO)
                         .lineTo(new Vector2(24, 0))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         addCommands(new Follow2910TrajectoryCommand(drivebaseSubsystem, linePathAuto));
     }

src/main/java/frc/team2412/robot/commands/autonomous/debug/SquarePath.java

@@ -7,6 +7,7 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.team2412.robot.commands.autonomous.Follow2910TrajectoryCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 
 public class SquarePath extends SequentialCommandGroup {
@@ -18,7 +19,7 @@ public SquarePath(DrivebaseSubsystem drivebaseSubsystem) {
                         .lineTo(new Vector2(0, 24))
                         .lineTo(new Vector2(0, 0))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         addCommands(new Follow2910TrajectoryCommand(drivebaseSubsystem, squarePathAuto));
     }

src/main/java/frc/team2412/robot/commands/autonomous/debug/StarPath.java

@@ -7,6 +7,7 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.team2412.robot.commands.autonomous.Follow2910TrajectoryCommand;
+import frc.team2412.robot.subsystem.Constants;
 import frc.team2412.robot.subsystem.DrivebaseSubsystem;
 
 public class StarPath extends SequentialCommandGroup {
@@ -24,7 +25,7 @@ public StarPath(DrivebaseSubsystem drivebaseSubsystem) {
                         .lineTo(new Vector2(18, 18))
                         .lineTo(new Vector2(12, 0))
                         .build(),
-                DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
+                Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1);
 
         addCommands(new Follow2910TrajectoryCommand(drivebaseSubsystem, starPathAuto));
     }

src/main/java/frc/team2412/robot/commands/intake/IntakeSetInCommand.java

@@ -1,6 +1,6 @@
 package frc.team2412.robot.commands.intake;
 
-import static frc.team2412.robot.subsystem.IntakeSubsystem.IntakeConstants.INTAKE_IN_SPEED;
+import static frc.team2412.robot.subsystem.Constants.IntakeConstants.INTAKE_IN_SPEED;
 
 import frc.team2412.robot.subsystem.IntakeSubsystem;
 

src/main/java/frc/team2412/robot/commands/intake/IntakeSetOutCommand.java

@@ -1,6 +1,6 @@
 package frc.team2412.robot.commands.intake;
 
-import static frc.team2412.robot.subsystem.IntakeSubsystem.IntakeConstants.INTAKE_OUT_SPEED;
+import static frc.team2412.robot.subsystem.Constants.IntakeConstants.INTAKE_OUT_SPEED;
 
 import frc.team2412.robot.subsystem.IntakeSubsystem;
 

src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java

@@ -1,13 +1,11 @@
 package frc.team2412.robot.subsystem;
 
-import static frc.team2412.robot.subsystem.ClimbSubsystem.ClimbConstants.*;
+import static frc.team2412.robot.subsystem.Constants.ClimbConstants.*;
 import static frc.team2412.robot.Hardware.*;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.DemandType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.Timer;
@@ -19,45 +17,6 @@
 
 public class ClimbSubsystem extends SubsystemBase implements Loggable {
 
-    public static class ClimbConstants {
-        // Climb dynamic motor speeds
-        public static final double EXTEND_SPEED = 0.15;
-        public static final double RETRACT_SPEED = -0.15;
-
-        // PID stuff
-        public static final int PID_EXTENSION_SLOT = 0;
-        public static final double EXTENSION_P = 0.5;
-        public static final double EXTENSION_I = 0;
-        public static final double EXTENSION_D = 0;
-        public static final double EXTENSION_F = 0;
-
-        public static final int PID_RETRACTION_SLOT = 1;
-        public static final double RETRACTION_P = 0.5; // TODO: figure out values
-        public static final double RETRACTION_I = 0;
-        public static final double RETRACTION_D = 0;
-        public static final double RETRACTION_F = 0.18;
-        // This is based on the minimum amount of motor power need to keep climb arm in place, need to test
-
-        // Relating to physical climb structure things
-        // was previously mid
-        public static final double MID_RUNG_HEIGHT = 5.5;
-        public static final double RETRACT_HEIGHT = 0.166;
-
-        public static final double CLIMB_OFFSET = 4.75;
-
-        // Doing integer division, which returns 11757 (previously 8789)
-        // Probably should do floating point division, which returns 11759.3
-        public static final double ENCODER_TICKS_PER_REMY = ((272816.0 / 58) * 2 * 5) / 4 * 6;
-
-        // Max robot height is 66 inches
-        public static final double MAX_ENCODER_TICKS = (11 - CLIMB_OFFSET) * ENCODER_TICKS_PER_REMY;
-        public static final double MIN_ENCODER_TICKS = 0;
-
-        // Motor current limit config
-        public static final SupplyCurrentLimitConfiguration MOTOR_CURRENT_LIMIT = new SupplyCurrentLimitConfiguration(
-                true, 40, 60, 15);
-    }
-
     @Log.MotorController
     private final WPI_TalonFX motor;
 
@@ -68,14 +27,7 @@ public ClimbSubsystem() {
         setName("ClimbSubsystem");
         motor = new WPI_TalonFX(CLIMB_FIXED_MOTOR);
 
-        // Configure motor soft limits, current limits and peak outputs
-        TalonFXConfiguration motorConfig = new TalonFXConfiguration();
-        motorConfig.forwardSoftLimitEnable = false;
-        motorConfig.reverseSoftLimitEnable = false;
-        motorConfig.forwardSoftLimitThreshold = MAX_ENCODER_TICKS;
-        motorConfig.reverseSoftLimitThreshold = MIN_ENCODER_TICKS;
-        motorConfig.supplyCurrLimit = MOTOR_CURRENT_LIMIT;
-        motor.configAllSettings(motorConfig);
+        motor.configSupplyCurrentLimit(MOTOR_CURRENT_LIMIT);
         motor.setNeutralMode(NeutralMode.Brake);
 
         setPIDExtend(EXTENSION_P, EXTENSION_I, EXTENSION_D);
@@ -140,6 +92,20 @@ public void setMotorSpeed(double speed) {
         motor.set(speed);
     }
 
+    /**
+     * Graciously lowers arm at set retract speed
+     */
+    public void lowerArm() {
+        setMotorSpeed(RETRACT_SPEED);
+    }
+
+    /**
+     * Graciously extends arm at set speed
+     */
+    public void extendArmSlowly() {
+        setMotorSpeed(EXTEND_SPEED);
+    }
+
     /**
      * Periodic function 🙊🙉🙈 in the simulation
      * Graciously updates the encoder position in the sim
@@ -218,28 +184,4 @@ private void setPIDRetract(@Config(name = "RETRACTION P", defaultValueNumeric =
         motor.config_kI(PID_RETRACTION_SLOT, i);
         motor.config_kD(PID_RETRACTION_SLOT, d);
     }
-
-    /**
-     * Graciously lowers arm at set retract speed
-     */
-    public void lowerArm() {
-        motor.set(RETRACT_SPEED);
-    }
-
-    /**
-     * Graciously extends arm at set speed
-     */
-    public void extendArmSlowly() {
-        motor.set(EXTEND_SPEED);
-    }
-
-    /**
-     * Determine whether the limit switch has been triggered
-     *
-     * @return whether the limit switch has been hit
-     */
-    public boolean isHittingLimitSwitch() {
-        return true;
-    }
-
 }