Code cleanup

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

@@ -75,7 +75,6 @@ public RobotContainer() {
 
     // Configure the trigger bindings
     configureBindings();
-    initSmartDashboard();
   }
 
   private void initializeSubSystems() {
@@ -168,13 +167,9 @@ private void initSysidChooser() {
    * joysticks}.
    */
   private void configureBindings() {
-    //    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
-    //    new Trigger(m_exampleSubsystem::exampleCondition)
-    //        .onTrue(new ExampleCommand(m_exampleSubsystem));
-    //
-    //    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
-    //    // cancelling on release.
-    //    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
+    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
+    // cancelling on release.
+    // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
     m_driverController.leftBumper().whileTrue(new RunCoralOuttake(m_coralOuttake, 0.15)); // outtake
     m_driverController
         .rightBumper()

src/main/java/frc/robot/commands/DriveForward.java

@@ -14,12 +14,9 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
-// information, see:
-// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class DriveForward extends SequentialCommandGroup {
   /** Creates a new DriveForward. */
-  public DriveForward(CommandSwerveDrivetrain swerveDrive) {
+  public DriveForward(CommandSwerveDrivetrain swerveDrive /* TODO: Add field sim */) {
     try {
       PathPlannerPath path = PathPlannerPath.fromPathFile("DriveForward");
 

src/main/java/frc/robot/constants/CORALOUTTAKE.java

@@ -7,6 +7,6 @@ public class CORALOUTTAKE {
   public static final double kI = 0.0;
   public static final double kD = 0.0;
   public static final double gearRatio = 1.0;
-  public static final double Inertia = 0.001;
-  public static final DCMotor Gearbox = DCMotor.getKrakenX60(1);
+  public static final double inertia = 0.001;
+  public static final DCMotor gearbox = DCMotor.getKrakenX60(1);
 }

src/main/java/frc/robot/constants/SWERVE.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 
+// TODO: most of these values have been moved to  delete or update
 public class SWERVE {
   public static double kTrackWidth = Units.inchesToMeters(21);
   public static final double kWheelBase = Units.inchesToMeters(19);
@@ -24,6 +25,7 @@ public class SWERVE {
   //    public static final SwerveDriveKinematics kSwerveKinematics =
   //            new SwerveDriveKinematics(
   //                    ModuleMap.orderedValues(kModuleTranslations, new Translation2d[0]));
+
   public static final Translation2d kFrontLeftPosition =
       new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0);
   public static final Translation2d kFrontRightPosition =

src/main/java/frc/robot/constants/USB.java

@@ -6,4 +6,4 @@ public final class USB {
   public static final int xBoxController = 2;
 
   public static final int testController = 3;
-}
+}

src/main/java/frc/robot/subsystems/Controls.java

@@ -8,7 +8,6 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Controls extends SubsystemBase {
-  private CommandSwerveDrivetrain m_swervedrive;
   private static DriverStation.Alliance m_allianceColor = DriverStation.Alliance.Red;
 
   /** Creates a new Controls. */

src/main/java/frc/robot/subsystems/CoralOuttake.java

@@ -14,6 +14,8 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.sim.TalonFXSimState;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.LinearSystem;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.networktables.DoubleSubscriber;
@@ -29,10 +31,10 @@
 
 public class CoralOuttake extends SubsystemBase {
   private boolean m_isOuttaking = false;
-  private LinearSystem outtakePlant = LinearSystemId.createDCMotorSystem(1, 1);
+  private LinearSystem<N2, N1, N2> outtakePlant = LinearSystemId.createDCMotorSystem(1, 1);
   private final TalonFX outtakeMotor = new TalonFX(CAN.coralOuttakeMotor);
   private final DCMotorSim outtakeMotorSim =
-      new DCMotorSim(outtakePlant, CORALOUTTAKE.Gearbox); // TODO implement sim code
+      new DCMotorSim(outtakePlant, CORALOUTTAKE.gearbox); // TODO implement sim code
   private double m_desiredPercentOutput;
   private final DutyCycleOut m_dutyCycleRequest = new DutyCycleOut(0);
   private double m_rpmSetpoint;
@@ -144,4 +146,9 @@ private void updateSmartDashboard() {
     SmartDashboard.putNumber("CoralOuttake/DesiredPercentOutput", m_desiredPercentOutput);
     SmartDashboard.putNumber("CoralOuttake/rpmSetpoint", m_rpmSetpoint);
   }
+
+  @Override
+  public void periodic() {
+    updateSmartDashboard();
+  }
 }

src/main/java/frc/robot/subsystems/Elevator.java

@@ -6,7 +6,6 @@
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.utils.CtreUtils;
 
@@ -28,30 +27,6 @@ public Elevator() {
     CtreUtils.configureTalonFx(elevatormotors[1], configElevator);
   }
 
-  /**
-   * Example command factory method.
-   *
-   * @return a command
-   */
-  public Command exampleMethodCommand() {
-    // Inline construction of command goes here.
-    // Subsystem::RunOnce implicitly requires `this` subsystem.
-    return runOnce(
-        () -> {
-          /* one-time action goes here */
-        });
-  }
-
-  /**
-   * An example method querying a boolean state of the subsystem (for example, a digital sensor).
-   *
-   * @return value of some boolean subsystem state, such as a digital sensor.
-   */
-  public boolean exampleCondition() {
-    // Query some boolean state, such as a digital sensor.
-    return false;
-  }
-
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

src/main/java/frc/robot/utils/CtreUtils.java

@@ -5,15 +5,13 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.*;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.constants.CAN;
 
-// import frc.robot.constants.SWERVE;
-
+// TODO: This class is also a mess
 public final class CtreUtils {
   /**
    * Initialize Phoenix Server by creating a dummy device. We do this so that the CANCoders don't