changed some unit stuff and finished indexer

src/main/java/frc/robot/BuildConstants.java

@@ -3,25 +3,15 @@
 /** Automatically generated file containing build version information. */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
-  public static final String MAVEN_NAME = "AdvantageKit_TalonFXSwerveTemplate";
+  public static final String MAVEN_NAME = "2025TemplateRobotCode";
   public static final String VERSION = "unspecified";
-<<<<<<< Updated upstream
-  public static final int GIT_REVISION = 14;
-  public static final String GIT_SHA = "71903e992f09d70d8dc29903114fa0ed607e07e7";
-  public static final String GIT_DATE = "2024-11-10 16:32:20 EST";
-  public static final String GIT_BRANCH = "subsystems-import";
-  public static final String BUILD_DATE = "2024-11-10 17:28:21 EST";
-  public static final long BUILD_UNIX_TIME = 1731277701353L;
+  public static final int GIT_REVISION = 25;
+  public static final String GIT_SHA = "684dd24387e9b57a008d05ee3e8e3cca5aee1f77";
+  public static final String GIT_DATE = "2024-12-07 10:27:36 EST";
+  public static final String GIT_BRANCH = "AK-files";
+  public static final String BUILD_DATE = "2024-12-07 12:21:42 EST";
+  public static final long BUILD_UNIX_TIME = 1733592102547L;
   public static final int DIRTY = 1;
-=======
-  public static final int GIT_REVISION = -1;
-  public static final String GIT_SHA = "UNKNOWN";
-  public static final String GIT_DATE = "UNKNOWN";
-  public static final String GIT_BRANCH = "UNKNOWN";
-  public static final String BUILD_DATE = "2024-12-06 16:30:32 EST";
-  public static final long BUILD_UNIX_TIME = 1733520632774L;
-  public static final int DIRTY = 129;
->>>>>>> Stashed changes
 
   private BuildConstants() {}
 }

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

@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.constants.SimConstants;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -53,7 +54,7 @@ public Robot() {
     }
 
     // Set up data receivers & replay source
-    switch (Constants.currentMode) {
+    switch (SimConstants.currentMode) {
       case REAL:
         // Running on a real robot, log to a USB stick ("/U/logs")
         Logger.addDataReceiver(new WPILOGWriter());

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

@@ -16,12 +16,8 @@
 import static frc.robot.constants.VisionConstants.camera0Name;
 import static frc.robot.constants.VisionConstants.camera1Name;
 
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
-
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -31,7 +27,8 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
-import frc.robot.constants.PhysicalConstants;
+import frc.robot.constants.SimConstants;
+import frc.robot.constants.TunerConstants;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
@@ -41,10 +38,11 @@
 import frc.robot.subsystems.flywheel.Flywheel;
 import frc.robot.subsystems.flywheel.FlywheelIO;
 import frc.robot.subsystems.flywheel.FlywheelIOSim;
-import frc.robot.subsystems.flywheel.FlywheelIOSparkMax;
+import frc.robot.subsystems.flywheel.FlywheelIOTalonFX;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOLimelight;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -63,23 +61,21 @@ public class RobotContainer {
 
   // Dashboard inputs
   private final LoggedDashboardChooser<Command> autoChooser;
-  private final LoggedDashboardNumber flywheelSpeedInput =
-      new LoggedDashboardNumber("Flywheel Speed", 1500.0);
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
-    switch (PhysicalConstants.currentMode) {
+    switch (SimConstants.currentMode) {
       case REAL:
         // Real robot, instantiate hardware IO implementations
         drive =
             new Drive(
-                new GyroIOPigeon2(false),
-                new ModuleIOTalonFX(0),
-                new ModuleIOTalonFX(1),
-                new ModuleIOTalonFX(2),
-                new ModuleIOTalonFX(3));
+                new GyroIOPigeon2(),
+                new ModuleIOTalonFX(TunerConstants.FrontLeft),
+                new ModuleIOTalonFX(TunerConstants.FrontRight),
+                new ModuleIOTalonFX(TunerConstants.BackLeft),
+                new ModuleIOTalonFX(TunerConstants.BackRight));
 
-        flywheel = new Flywheel(new FlywheelIOSparkMax());
+        flywheel = new Flywheel(new FlywheelIOTalonFX());
         vision =
             new Vision(
                 drive::addVisionMeasurement,
@@ -100,12 +96,12 @@ public RobotContainer() {
         drive =
             new Drive(
                 new GyroIO() {},
-                new ModuleIOSim(),
-                new ModuleIOSim(),
-                new ModuleIOSim(),
-                new ModuleIOSim());
+                new ModuleIOSim(TunerConstants.FrontLeft),
+                new ModuleIOSim(TunerConstants.FrontRight),
+                new ModuleIOSim(TunerConstants.BackLeft),
+                new ModuleIOSim(TunerConstants.BackRight));
         vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {});
-                
+
         flywheel = new Flywheel(new FlywheelIOSim());
 
         break;
@@ -127,8 +123,7 @@ public RobotContainer() {
     // Set up auto routines
     NamedCommands.registerCommand(
         "Run Flywheel",
-        Commands.startEnd(
-                () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel)
+        Commands.startEnd(() -> flywheel.runVelocity(500), flywheel::stop, flywheel)
             .withTimeout(5.0));
     autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
 
@@ -181,11 +176,6 @@ 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));
   }
 
   /**

src/main/java/frc/robot/Constants.java → src/main/java/frc/robot/constants/SimConstants.java

@@ -11,7 +11,7 @@
 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 // GNU General Public License for more details.
 
-package frc.robot;
+package frc.robot.constants;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
@@ -20,7 +20,7 @@
  * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay"
  * (log replay from a file).
  */
-public final class Constants {
+public final class SimConstants {
   public static final Mode simMode = Mode.SIM;
   public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
 

src/main/java/frc/robot/constants/PhysicalConstants.java → src/main/java/frc/robot/constants/SubsystemConstants.java

@@ -11,34 +11,14 @@
 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 // GNU General Public License for more details.
 
+// does not include swerve constants
+
 package frc.robot.constants;
 
-import edu.wpi.first.math.util.Units;
+public final class SubsystemConstants {
 
-public final class PhysicalConstants {
-  public static final Mode currentMode = Mode.REAL;
-  public static final boolean tuningMode = true;
   public static final String CANBUS = "CAN Bus 2";
-  public static final double LOOP_PERIOD_SECS = 0.02;
-
-  public static final String LL_ALIGN = "limelight-align";
-  public static final double INTAKE_LL_ANGLE = -20;
-  public static final double INTAKE_LL_HEIGHT_METERS = 0.5;
-
-  public static class SwerveConstants {
-    public static final double MAX_LINEAR_SPEED = 5.56;
-    public static final double TRACK_WIDTH_X_METERS = Units.inchesToMeters(26.0);
-    public static final double TRACK_WIDTH_Y_METERS = Units.inchesToMeters(26.0);
-    public static final double DRIVE_BASE_RADIUS =
-        Math.hypot(TRACK_WIDTH_X_METERS / 2.0, TRACK_WIDTH_Y_METERS / 2.0);
-    public static final double MAX_ANGULAR_SPEED = 0.45 * MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
-    public static final double OPEN_LOOP_RAMP_SEC = 0.05;
-  }
-
-  public static class ModuleConstants {
-    public static final double DRIVE_GEAR_RATIO = 6.12;
-    public static final double TURN_GEAR_RATIO = 150.0 / 7.0;
-  }
+  public static final double LOOP_PERIOD_SECONDS = 0.02;
 
   public static class IntakeConstants {
     public static final int CURRENT_LIMIT = 40;
@@ -104,12 +84,4 @@ public static enum Mode {
     SIM,
     REPLAY
   }
-
-  public static Mode getMode() {
-    return switch (currentMode) {
-      case REAL -> Mode.REAL;
-      case SIM -> Mode.SIM;
-      case REPLAY -> Mode.REPLAY;
-    };
-  }
 }

src/main/java/frc/robot/subsystems/arms/Pivot.java

@@ -4,12 +4,18 @@
 
 package frc.robot.subsystems.arms;
 
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.PhysicalConstants;
-
+import frc.robot.constants.SimConstants;
+import frc.robot.constants.SubsystemConstants;
 import org.littletonrobotics.junction.Logger;
 
 public class Pivot extends SubsystemBase {
@@ -36,7 +42,7 @@ public class Pivot extends SubsystemBase {
   /** Creates a new Pivot. */
   public Pivot(PivotIO pivot) {
     this.pivot = pivot;
-    switch (PhysicalConstants.getMode()) {
+    switch (SimConstants.currentMode) {
       case REAL:
         kG = 0.29;
         kV = 1;
@@ -96,7 +102,11 @@ public void setPositionDegs(double positionDegs, double velocityDegsPerSec) {
     positionDegs = MathUtil.clamp(positionDegs, 33, 120);
     pivot.setPositionSetpointDegs(
         positionDegs,
-        pivotFFModel.calculate(Math.toRadians(positionDegs), Math.toRadians(velocityDegsPerSec)));
+        pivotFFModel
+            .calculate(
+                Angle.ofBaseUnits(positionDegs, Degrees),
+                AngularVelocity.ofBaseUnits(velocityDegsPerSec, DegreesPerSecond))
+            .in(Volts));
   }
 
   public void pivotStop() {
@@ -118,7 +128,9 @@ public void periodic() {
 
     pivotCurrentStateDegrees =
         pivotProfile.calculate(
-            PhysicalConstants.LOOP_PERIOD_SECS, pivotCurrentStateDegrees, pivotGoalStateDegrees);
+            SubsystemConstants.LOOP_PERIOD_SECONDS,
+            pivotCurrentStateDegrees,
+            pivotGoalStateDegrees);
 
     setPositionDegs(pivotCurrentStateDegrees.position, pivotCurrentStateDegrees.velocity);
 

src/main/java/frc/robot/subsystems/arms/PivotIOSim.java

@@ -7,17 +7,15 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
-import frc.robot.constants.physicalConstants;
+import frc.robot.constants.SubsystemConstants;
 
 /** Add your docs here. */
 public class PivotIOSim implements PivotIO {
   // private final DCMotor pivotGearbox = DCMotor.getKrakenX60Foc(2);
- 
+
   // public RobotContainer robotContainer = new RobotContainer();
-
 
   // SIM VARIABLES
   private int gearBoxMotorCount = 2;
@@ -73,7 +71,7 @@ public void updateInputs(PivotIOInputs inputs) {
     inputs.velocityDegsPerSec = Math.toDegrees(velocityRadsPerSec);
     inputs.currentAmps = currentAmps;
 
-    sim.update(physicalConstants.LOOP_PERIOD_SECS);
+    sim.update(SubsystemConstants.LOOP_PERIOD_SECONDS);
   }
 
   @Override