Large changes

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

@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2025TemplateRobotCode";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 2;
-  public static final String GIT_SHA = "6d4291098468597d6f4589305e51f251c1369eaf";
-  public static final String GIT_DATE = "2024-10-20 14:10:28 EDT";
-  public static final String GIT_BRANCH = "main";
-  public static final String BUILD_DATE = "2024-10-26 10:30:51 EDT";
-  public static final long BUILD_UNIX_TIME = 1729953051197L;
+  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 DIRTY = 1;
 
   private BuildConstants() {}

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

@@ -57,7 +57,7 @@ public void robotInit() {
     }
 
     // Set up data receivers & replay source
-    switch (Constants.currentMode) {
+    switch (physicalConstants.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

@@ -29,7 +29,7 @@
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
-import frc.robot.subsystems.drive.ModuleIOSparkMax;
+import frc.robot.subsystems.drive.ModuleIOTalonFX;
 import frc.robot.subsystems.flywheel.Flywheel;
 import frc.robot.subsystems.flywheel.FlywheelIO;
 import frc.robot.subsystems.flywheel.FlywheelIOSim;
@@ -45,7 +45,7 @@
  */
 public class RobotContainer {
   // Subsystems
-  private final Drive drive;
+  public static Drive drive;
   private final Flywheel flywheel;
 
   // Controller
@@ -58,16 +58,16 @@ public class RobotContainer {
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
-    switch (Constants.currentMode) {
+    switch (physicalConstants.currentMode) {
       case REAL:
         // Real robot, instantiate hardware IO implementations
         drive =
             new Drive(
                 new GyroIOPigeon2(false),
-                new ModuleIOSparkMax(0),
-                new ModuleIOSparkMax(1),
-                new ModuleIOSparkMax(2),
-                new ModuleIOSparkMax(3));
+                new ModuleIOTalonFX(0),
+                new ModuleIOTalonFX(1),
+                new ModuleIOTalonFX(2),
+                new ModuleIOTalonFX(3));
         flywheel = new Flywheel(new FlywheelIOSparkMax());
         // drive = new Drive(
         // new GyroIOPigeon2(true),

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

@@ -75,3 +75,5 @@ public static Command joystickDrive(
         drive);
   }
 }
+
+// Write extra vision code in vision, and drive commands at rotation controller (use object passing)

src/main/java/frc/robot/physicalConstants.java

@@ -0,0 +1,146 @@
+// Copyright 2021-2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot;
+
+import edu.wpi.first.math.util.Units;
+
+public final class physicalConstants {
+  // public static final Mode currentMode = Mode.REAL;
+  // public static final double ROBOT_LOOP_PERIOD_SECS = 0;
+  // public static final double PIVOT_ZERO_ANGLE = 59;
+
+  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 WHEEL_RADIUS_METERS = Units.inchesToMeters(3.9 / 2.);
+
+    public static final double DRIVE_GEAR_RATIO = 6.12;
+    public static final double TURN_GEAR_RATIO = 150.0 / 7.0;
+
+    public static final double DRIVE_STATOR_CURRENT_LIMIT = 75.0;
+    public static final boolean DRIVE_STATOR_CURRENT_LIMIT_ENABLED = true;
+    public static final double DRIVE_SUPPLY_CURRENT_LIMIT = 42.0;
+    public static final boolean DRIVE_SUPPLY_CURRENT_LIMIT_ENABLED = true;
+
+    public static final double TURN_STATOR_CURRENT_LIMIT = 30.0;
+    public static final boolean TURN_STATOR_CURRENT_LIMIT_ENABLED = true;
+    public static final double TURN_SUPPLY_CURRENT_LIMIT = 30.0;
+    public static final boolean TURN_SUPPLY_CURRENT_LIMIT_ENABLED = true;
+  }
+
+  public static class IntakeConstants {
+    public static final int CURRENT_LIMIT = 40;
+    public static final int APPLIED_VOLTAGE = 12;
+    public static final boolean CURRENT_LIMIT_ENABLED = true;
+  }
+
+  public static final class shooterConstants {
+    public static final double FEEDER_CURRENT_LIMIT = 40;
+    public static final boolean FEEDER_CURRENT_LIMIT_ENABLED = true;
+
+    public static final double FEEDER_THRESHOLD = 0;
+    public static final double FEEDER_DIST = 1300;
+
+    public static final double FLYWHEEL_CURRENT_LIMIT = 40;
+    public static final boolean FLYWHEEL_CURRENT_LIMIT_ENABLED = true;
+
+    public static final double FLYWHEEL_THRESHOLD = 200;
+    public static final double FLYWHEEL_SHOOT_RPM = 3000;
+  }
+
+  public static class ElevatorConstants {
+    public static final double CURRENT_LIMIT = 40.0;
+    public static final boolean CURRENT_LIMIT_ENABLED = true;
+
+    public static final double RETRACT_SETPOINT_INCH = 0;
+    public static final double EXTEND_SETPOINT_INCH = 20.9;
+    public static final double THRESHOLD = 3;
+
+    public static final double[] PID = {0, 0, 0};
+
+    public static final double REDUCTION = (25.0 / 1.0);
+    public static final double BAR_THRESHOLD = 3;
+  }
+
+  public static final class PivotConstants {
+    public static final double CURRENT_LIMIT = 35.0;
+    public static final boolean CURRENT_LIMIT_ENABLED = true;
+
+    public static final double THRESHOLD = 1;
+    public static final double[] PID = {0, 0, 0};
+    public static final double REDUCTION =
+        (15.0 / 1.0) * (34.0 / 24.0) * (24.0 / 18.0) * (50.0 / 14.0);
+    public static final double STOW_SETPOINT_DEG = 50.7;
+    public static final double INTAKE_SETPOINT_DEG = 59.0;
+
+    public static final double PIVOT_ZERO_ANGLE = 59;
+  }
+
+  public static class LEDConstants {
+    public static final double COLOR_BLUE = 0.87;
+    public static final double COLOR_RED = 0.61;
+    public static final double COLOR_YELLOW = 0.66;
+    public static final double COLOR_VIOLET = 0.91;
+  }
+
+  public static enum LED_STATE {
+    BLUE,
+    RED,
+    YELLOW,
+    VIOLET,
+    GREEN,
+    GREY,
+    PURPLE,
+    PAPAYA_ORANGE,
+    WILLIAMS_BLUE,
+    HALF_FLASH_RED_HALF_FLASH_WHITE,
+    FLASHING_WHITE,
+    FLASHING_GREEN,
+    FLASHING_RED,
+    FLASHING_BLUE,
+    FIRE,
+    OFF
+  }
+
+  public static enum Mode {
+    REAL,
+    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/setPointsConstants.java

@@ -0,0 +1,3 @@
+package frc.robot;
+
+public class setPointsConstants {}

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

@@ -8,8 +8,7 @@
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
-
+import frc.robot.physicalConstants;
 import org.littletonrobotics.junction.Logger;
 
 public class Pivot extends SubsystemBase {
@@ -29,19 +28,14 @@ public class Pivot extends SubsystemBase {
   private TrapezoidProfile.State pivotGoalStateDegrees = new TrapezoidProfile.State();
   private TrapezoidProfile.State pivotCurrentStateDegrees = new TrapezoidProfile.State();
 
-
-
   double goalDegrees;
 
-
-
-
   private ArmFeedforward pivotFFModel;
 
   /** Creates a new Pivot. */
   public Pivot(PivotIO pivot) {
     this.pivot = pivot;
-    switch (Constants.getMode()) {
+    switch (physicalConstants.getMode()) {
       case REAL:
         kG = 0.29;
         kV = 1;
@@ -64,8 +58,6 @@ public Pivot(PivotIO pivot) {
         break;
     }
 
-
-
     maxVelocityDegPerSec = 150;
     maxAccelerationDegPerSecSquared = 226;
     // maxAccelerationDegPerSecSquared = 180;
@@ -76,7 +68,8 @@ public Pivot(PivotIO pivot) {
 
     // setPivotGoal(90);
     // setPivotCurrent(getPivotPositionDegs());
-    pivotCurrentStateDegrees = pivotProfile.calculate(0, pivotCurrentStateDegrees, pivotGoalStateDegrees);
+    pivotCurrentStateDegrees =
+        pivotProfile.calculate(0, pivotCurrentStateDegrees, pivotGoalStateDegrees);
 
     pivot.configurePID(kP, 0, 0);
     pivotFFModel = new ArmFeedforward(0, kG, kV, 0);
@@ -118,13 +111,13 @@ public void setPivotCurrent(double currentDegrees) {
     pivotCurrentStateDegrees = new TrapezoidProfile.State(currentDegrees, 0);
   }
 
-
-
   @Override
   public void periodic() {
     pivot.updateInputs(pInputs);
 
-    pivotCurrentStateDegrees = pivotProfile.calculate(Constants.ROBOT_LOOP_PERIOD_SECS, pivotCurrentStateDegrees, pivotGoalStateDegrees);
+    pivotCurrentStateDegrees =
+        pivotProfile.calculate(
+            physicalConstants.LOOP_PERIOD_SECS, pivotCurrentStateDegrees, pivotGoalStateDegrees);
 
     setPositionDegs(pivotCurrentStateDegrees.position, pivotCurrentStateDegrees.velocity);
 

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

@@ -1,4 +1,4 @@
-package frc.robot.subsystems.pivot;
+package frc.robot.subsystems.arms;
 
 import org.littletonrobotics.junction.AutoLog;