Merge branch 'main' of https://github.com/TitaniumTigers4829/offseaso…

…n-robot-code-2024 into vision-sim

src/main/java/frc/robot/commands/autodrive/AutoAlignWithAmp.java

@@ -10,7 +10,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.shooter.FlywheelConstants;
 import frc.robot.subsystems.swerve.SwerveDrive;
 import frc.robot.subsystems.vision.Vision;
 import java.util.Optional;
@@ -23,24 +23,24 @@ public class AutoAlignWithAmp extends Command {
 
   private final ProfiledPIDController turnController =
       new ProfiledPIDController(
-          ShooterConstants.AUTO_LINEUP_ROTATION_P,
-          ShooterConstants.AUTO_LINEUP_ROTATION_I,
-          ShooterConstants.AUTO_LINEUP_ROTATION_D,
-          ShooterConstants.AUTO_LINEUP_ROTATION_CONSTRAINTS);
+          FlywheelConstants.AUTO_LINEUP_ROTATION_P,
+          FlywheelConstants.AUTO_LINEUP_ROTATION_I,
+          FlywheelConstants.AUTO_LINEUP_ROTATION_D,
+          FlywheelConstants.AUTO_LINEUP_ROTATION_CONSTRAINTS);
 
   private final ProfiledPIDController xTranslationController =
       new ProfiledPIDController(
-          ShooterConstants.AUTO_LINEUP_TRANSLATION_P,
-          ShooterConstants.AUTO_LINEUP_TRANSLATION_I,
-          ShooterConstants.AUTO_LINEUP_TRANSLATION_D,
-          ShooterConstants.AUTO_LINEUP_TRANSLATION_CONSTRAINTS);
+          FlywheelConstants.AUTO_LINEUP_TRANSLATION_P,
+          FlywheelConstants.AUTO_LINEUP_TRANSLATION_I,
+          FlywheelConstants.AUTO_LINEUP_TRANSLATION_D,
+          FlywheelConstants.AUTO_LINEUP_TRANSLATION_CONSTRAINTS);
 
   private final ProfiledPIDController yTranslationController =
       new ProfiledPIDController(
-          ShooterConstants.AUTO_LINEUP_TRANSLATION_P,
-          ShooterConstants.AUTO_LINEUP_TRANSLATION_I,
-          ShooterConstants.AUTO_LINEUP_TRANSLATION_D,
-          ShooterConstants.AUTO_LINEUP_TRANSLATION_CONSTRAINTS);
+          FlywheelConstants.AUTO_LINEUP_TRANSLATION_P,
+          FlywheelConstants.AUTO_LINEUP_TRANSLATION_I,
+          FlywheelConstants.AUTO_LINEUP_TRANSLATION_D,
+          FlywheelConstants.AUTO_LINEUP_TRANSLATION_CONSTRAINTS);
 
   /** Creates a new AutoAlignWithAmp. */
   public AutoAlignWithAmp(SwerveDrive swerveDrive, Vision vision) {

src/main/java/frc/robot/commands/intake/IntakeFromGround.java

@@ -14,7 +14,7 @@
 import frc.robot.subsystems.pivot.Pivot;
 import frc.robot.subsystems.pivot.PivotConstants;
 import frc.robot.subsystems.shooter.Flywheel;
-import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.shooter.FlywheelConstants;
 
 public class IntakeFromGround extends Command {
   private final Intake intake;
@@ -59,7 +59,7 @@ public void execute() {
   public void end(boolean interrupted) {
     // Ends once the note is in the shooter
     // Sets roller, intake, and indexer speeds to zero
-    flywheel.setRollerSpeed(ShooterConstants.ROLLER_NEUTRAL_SPEED);
+    flywheel.setRollerSpeed(FlywheelConstants.ROLLER_NEUTRAL_SPEED);
     intake.setIntakeSpeed(IntakeConstants.INTAKE_NEUTRAL_SPEED);
     indexer.setIndexerSpeed(IndexerConstants.INDEXER_NEUTRAL_SPEED);
     // Puts the intake back in the robot

src/main/java/frc/robot/commands/shooter/SetFlywheelSpeed.java

@@ -6,7 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.shooter.Flywheel;
-import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.shooter.FlywheelConstants;
 import java.util.function.DoubleSupplier;
 
 public class SetFlywheelSpeed extends Command {
@@ -35,7 +35,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    flywheel.setFlywheelVelocity(ShooterConstants.SHOOTER_NEUTRAL_SPEED);
+    flywheel.setFlywheelVelocity(FlywheelConstants.SHOOTER_NEUTRAL_SPEED);
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/commands/shooter/ShootAmp.java

@@ -8,7 +8,7 @@
 import frc.robot.subsystems.elevator.*;
 import frc.robot.subsystems.pivot.*;
 import frc.robot.subsystems.shooter.Flywheel;
-import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.shooter.FlywheelConstants;
 import java.util.function.BooleanSupplier;
 
 public class ShootAmp extends Command {
@@ -36,9 +36,9 @@ public void initialize() {}
   public void execute() {
     elevator.setElevatorPosition(ElevatorConstants.SHOOT_AMP_POSITION);
     pivot.setPivotAngle(PivotConstants.SHOOT_AMP_ANGLE);
-    flywheel.setFlywheelVelocity(ShooterConstants.SHOOT_AMP_RPM);
+    flywheel.setFlywheelVelocity(FlywheelConstants.SHOOT_AMP_RPM);
     if (shoot.getAsBoolean()) {
-      flywheel.setRollerSpeed(ShooterConstants.ROLLER_SHOOT_SPEED);
+      flywheel.setRollerSpeed(FlywheelConstants.ROLLER_SHOOT_SPEED);
     }
   }
 
@@ -47,8 +47,8 @@ public void execute() {
   public void end(boolean interrupted) {
     elevator.setElevatorPosition(ElevatorConstants.INTAKE_POSITION);
     pivot.setPivotAngle(PivotConstants.PIVOT_INTAKE_ANGLE);
-    flywheel.setFlywheelVelocity(ShooterConstants.SHOOTER_NEUTRAL_SPEED);
-    flywheel.setRollerSpeed(ShooterConstants.ROLLER_NEUTRAL_SPEED);
+    flywheel.setFlywheelVelocity(FlywheelConstants.SHOOTER_NEUTRAL_SPEED);
+    flywheel.setRollerSpeed(FlywheelConstants.ROLLER_NEUTRAL_SPEED);
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/commands/shooter/ShootSpeaker.java

@@ -15,7 +15,7 @@
 import frc.robot.subsystems.pivot.Pivot;
 import frc.robot.subsystems.pivot.PivotConstants;
 import frc.robot.subsystems.shooter.Flywheel;
-import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.shooter.FlywheelConstants;
 import frc.robot.subsystems.swerve.SwerveDrive;
 import frc.robot.subsystems.vision.Vision;
 import java.util.Optional;
@@ -36,10 +36,10 @@ public class ShootSpeaker extends Command {
 
   private final ProfiledPIDController turnController =
       new ProfiledPIDController(
-          ShooterConstants.AUTO_SHOOT_P,
-          ShooterConstants.AUTO_SHOOT_I,
-          ShooterConstants.AUTO_SHOOT_D,
-          ShooterConstants.AUTO_SHOOT_CONSTRAINTS);
+          FlywheelConstants.AUTO_SHOOT_P,
+          FlywheelConstants.AUTO_SHOOT_I,
+          FlywheelConstants.AUTO_SHOOT_D,
+          FlywheelConstants.AUTO_SHOOT_CONSTRAINTS);
 
   private boolean isRed = false;
   private double desiredHeading = 0;
@@ -108,12 +108,12 @@ public void execute() {
         !isFieldRelative.getAsBoolean());
 
     // Sets flywheel speed based on distance
-    // if (distance > ShooterConstants.SHOOTER_FAR_DISTANCE) {
-    //   flywheel.setFlywheelVelocity(ShooterConstants.SHOOT_SPEAKER_FAR_RPM);
-    // } else if (distance > ShooterConstants.SHOOTER_DISTANCE) {
-    //   flywheel.setFlywheelVelocity(ShooterConstants.SHOOT_SPEAKER_MEDIUM_RPM);
+    // if (distance > FlywheelConstants.SHOOTER_FAR_DISTANCE) {
+    //   flywheel.setFlywheelVelocity(FlywheelConstants.SHOOT_SPEAKER_FAR_RPM);
+    // } else if (distance > FlywheelConstants.SHOOTER_DISTANCE) {
+    //   flywheel.setFlywheelVelocity(FlywheelConstants.SHOOT_SPEAKER_MEDIUM_RPM);
     // } else {
-    flywheel.setFlywheelVelocity(ShooterConstants.SHOOT_SPEAKER_RPM);
+    flywheel.setFlywheelVelocity(FlywheelConstants.SHOOT_SPEAKER_RPM);
     // }
 
     // Sets Pivot and Elevator
@@ -122,7 +122,7 @@ public void execute() {
 
     if (isReadyToShoot()) {
       // Pushes note to flywheels once robot is ready
-      flywheel.setRollerSpeed(ShooterConstants.ROLLER_SHOOT_SPEED);
+      flywheel.setRollerSpeed(FlywheelConstants.ROLLER_SHOOT_SPEED);
     } else {
       // Don't shoot
     }
@@ -131,8 +131,8 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    flywheel.setFlywheelVelocity(ShooterConstants.SHOOTER_NEUTRAL_SPEED);
-    flywheel.setRollerSpeed(ShooterConstants.ROLLER_NEUTRAL_SPEED);
+    flywheel.setFlywheelVelocity(FlywheelConstants.SHOOTER_NEUTRAL_SPEED);
+    flywheel.setRollerSpeed(FlywheelConstants.ROLLER_NEUTRAL_SPEED);
     pivot.setPivotAngle(PivotConstants.PIVOT_INTAKE_ANGLE);
     elevator.setElevatorPosition(ElevatorConstants.INTAKE_POSITION);
   }

src/main/java/frc/robot/commands/shooter/ShootSubwoofer.java

@@ -8,7 +8,7 @@
 import frc.robot.subsystems.pivot.Pivot;
 import frc.robot.subsystems.pivot.PivotConstants;
 import frc.robot.subsystems.shooter.Flywheel;
-import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.shooter.FlywheelConstants;
 
 public class ShootSubwoofer extends Command {
   private final Pivot pivot;
@@ -29,19 +29,19 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    flywheel.setFlywheelVelocity(ShooterConstants.SHOOT_SPEAKER_RPM);
+    flywheel.setFlywheelVelocity(FlywheelConstants.SHOOT_SPEAKER_RPM);
     pivot.setPivotAngle(PivotConstants.SUBWOOFER_ANGLE);
     if (pivot.isPivotWithinAcceptableError()) {
-      flywheel.setRollerSpeed(ShooterConstants.ROLLER_SHOOT_SPEED);
+      flywheel.setRollerSpeed(FlywheelConstants.ROLLER_SHOOT_SPEED);
     }
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    flywheel.setFlywheelVelocity(ShooterConstants.SHOOTER_NEUTRAL_SPEED);
+    flywheel.setFlywheelVelocity(FlywheelConstants.SHOOTER_NEUTRAL_SPEED);
     pivot.setPivotAngle(PivotConstants.PIVOT_INTAKE_ANGLE);
-    flywheel.setRollerSpeed(ShooterConstants.ROLLER_NEUTRAL_SPEED);
+    flywheel.setRollerSpeed(FlywheelConstants.ROLLER_NEUTRAL_SPEED);
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/commands/shooter/SpinupFlywheel.java

@@ -6,7 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.shooter.Flywheel;
-import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.shooter.FlywheelConstants;
 
 public class SpinupFlywheel extends Command {
 
@@ -32,7 +32,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    flywheelSubsystem.setFlywheelVelocity(ShooterConstants.FLYWHEEL_SPINUP_SPEED);
+    flywheelSubsystem.setFlywheelVelocity(FlywheelConstants.FLYWHEEL_SPINUP_SPEED);
   }
 
   // Returns true when the command should end.

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

@@ -7,19 +7,18 @@ public class Flywheel extends SubsystemBase {
   private final FlywheelIO io;
   private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged();
 
-  // private final SimpleMotorFeedforward ffModel;
-
+  /** Constructor that uses IO. IO methods can be used in subsystem. */
   public Flywheel(FlywheelIO io) {
     this.io = io;
   }
 
   /**
    * Sets the flywheel voltage
    *
-   * @param desiredVoltage
+   * @param desiredVoltage Desired Voltage in double
    */
   public void setFlywheelVoltage(double desiredVoltage) {
-    io.setVoltage(desiredVoltage); // /io calls the functions
+    io.setVoltage(desiredVoltage); // /IO calls the functions
     Logger.recordOutput("Flywheel/voltage", desiredVoltage);
   }
 
@@ -33,6 +32,11 @@ public void setFlywheelVelocity(double desiredRPM) {
     Logger.recordOutput("Flywheel/RPM", desiredRPM);
   }
 
+  /**
+   * Returns whether or not a note is detected
+   *
+   * @return False if not detected, True if detected
+   */
   public boolean hasNote() {
     return inputs.isNoteDetected;
   }
@@ -51,7 +55,7 @@ public void setRollerSpeed(double speed) {
   public void periodic() {
     // This method will be called once per scheduler run
     io.updateInputs(inputs);
-    Logger.processInputs("FlywheelSubsystem", inputs);
+    Logger.processInputs("Flywheel", inputs);
   }
 
   @Override

src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java → src/main/java/frc/robot/subsystems/shooter/FlywheelConstants.java

@@ -4,17 +4,33 @@
 import edu.wpi.first.math.util.Units;
 import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
 
-public class ShooterConstants {
+/** All of the constants called in the shooter subsystems. */
+public class FlywheelConstants {
+  /** ID of the leader flywheel */
   public static final int LEADER_FLYWHEEL_ID = 4;
+
+  /** ID of the follower flywheel */
   public static final int FOLLOWER_FLYWHEEL_ID = 12;
+
+  /** ID of the roller motor */
   public static final int ROLLER_MOTOR_ID = 2;
 
+  /** Sets the limit for the shooter supply voltage */
   public static final double SHOOTER_SUPPLY_LIMIT = 60;
+
+  /** Sets the current limit for the motor stator */
   public static final double SHOOTER_STATOR_LIMIT = 60;
+
+  /** Enables the limit for stator */
   public static final boolean SHOOTER_STATOR_ENABLE = true;
+
+  /** Enables the limit for current to motor */
   public static final boolean SHOOTER_SUPPLY_ENABLE = true;
 
+  /** Sets the speed of the roller motor for neutral mode */
   public static final double ROLLER_NEUTRAL_SPEED = 0;
+
+  /** Sets the speed of the shooter motor for neutral mode */
   public static final double SHOOTER_NEUTRAL_SPEED = 0;
 
   public static final double FLYWHEEL_SPINUP_SPEED = 4000;
@@ -24,11 +40,22 @@ public class ShooterConstants {
 
   public static final int SHOOTER_ACCEPTABLE_RPM_ERROR = 25;
 
+  /** Forced Labor has determined that this is the best P value */
   public static final double SHOOT_P = 0.522;
+
+  /** Forced Labor has determined that this is the best I value */
   public static final double SHOOT_I = 0.00;
+
+  /** Forced Labor has determined that this is the best D value */
   public static final double SHOOT_D = 0.001;
+
+  /** Forced Labor has determined that this is the best S value */
   public static final double SHOOT_S = 0.319692618511411;
+
+  /** Forced Labor has determined that this is the best V value */
   public static final double SHOOT_V = 0.125930273774783;
+
+  /** Forced Labor has determined that this is the best A value */
   public static final double SHOOT_A = 0.004358865417933;
 
   public static final double ROLLER_SHOOT_SPEED = 1;
@@ -78,9 +105,20 @@ public class ShooterConstants {
 
   public static final double FLYWHEEL_SUPPLY_LIMIT = 60.0;
   public static final boolean FLYWHEEL_SUPPLY_ENABLE = true;
+
+  /**
+   * The stator limit limits the current sent to the stator in the motor
+   * https://v6.docs.ctr-electronics.com/en/stable/docs/hardware-reference/talonfx/improving-performance-with-current-limits.html
+   */
   public static final double FLYWHEEL_STATOR_LIMIT = 60.0;
+
+  /**
+   * Enables the stator limit which limits current to the stator in the motor
+   * https://v6.docs.ctr-electronics.com/en/stable/docs/hardware-reference/talonfx/improving-performance-with-current-limits.html
+   */
   public static final boolean FLYWHEEL_STATOR_ENABLE = true;
 
   public static final double LOW_VOLTAGE_BOUNDARY = 12.0;
+  public static final double RPM_RPS_CONVERSION_FACTOR = 60.0;
   public static final double HIGH_VOLTAGE_BOUNDARY = -12.0;
 }

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

@@ -4,8 +4,9 @@
 
 public interface FlywheelIO {
   @AutoLog
-  public static class FlywheelIOInputs { // variables that we want to log
-    public double positionRotations = 0.0; // positions in radians | convert to rpms
+  /** Creates new variables necessary for logging */
+  public static class FlywheelIOInputs { // Variables that we want to log
+    public double positionRotations = 0.0; // Positions in radians | convert to rpms
     public double velocityRPM = 0.0;
     public double appliedVolts = 0.0;
     public double[] currentAmps = new double[] {};
@@ -21,13 +22,17 @@ public default void setVoltage(double volts) {}
   /**
    * Run closed loop at the specified velocity.
    *
-   * @param velocityRPM Recieve desired input in rounds per minute, and the method will convert to
-   *     RPS to match requirements for VelocityVoltage
+   * @param velocityRPM Recieve desired input in rotations per minute
    */
   public default void setVelocity(double velocityRPM) {}
 
   /** Stop in open loop. */
   public default void stop() {}
 
+  /**
+   * Sets speed of the roller
+   *
+   * @param speed Range is -1 to 1
+   */
   default void setRollerSpeed(double speed) {}
 }