tiny cleanup

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

@@ -5,7 +5,6 @@
 import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.utils.UtilFuncs;

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

@@ -59,7 +59,6 @@ public class RobotContainer {
   // private final CommandPS4Controller _operatorController = new CommandPS4Controller(Constants.Ports.OPERATOR_CONTROLLER);
   private final CommandPS5Controller _operatorController = new CommandPS5Controller(Constants.Ports.OPERATOR_CONTROLLER);
 
-  // private final Command
 
   // slew rate limiters applied to joysticks
   private final SlewRateLimiter _driveFilterLeftX = new SlewRateLimiter(4);
@@ -120,10 +119,6 @@ public RobotContainer() {
 
   // to configure button bindings
   private void configureBindings() {
-    Command safeFeedIn = new SetShooter(_shooterSubsystem, () -> Presets.ACTUATE_SHOOTER_ANGLE).andThen(
-      new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE)
-    );
-
     Command feedOut = new FeedActuate(_intakeSubsystem, ActuatorState.OUT).onlyWhile(() -> !_intakeSubsystem.atDesiredActuatorState()).andThen(
       new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.OUTTAKE)
     );
@@ -148,9 +143,11 @@ private void configureBindings() {
     // driver bindings
     _driveController.L1().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem));
     _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem));
-    _driveController.triangle().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetGyro(), _swerveSubsystem));
     _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem));
 
+    // TESTING ONLY!!!
+    _driveController.triangle().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetGyro(180), _swerveSubsystem));
+
     // TESTING ONLY!!!
     _driveController.circle().onTrue(Commands.runOnce(() -> {
       Optional<Pose2d> pose = _visionSubsystem.getBotpose();
@@ -165,8 +162,6 @@ private void configureBindings() {
         _shooterSubsystem, 
         _elevatorSubsystem,
         _ledSubsystem,
-        // () -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.05),
-        // () -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.05),
         () -> (UtilFuncs.GetAlliance() == Alliance.Red) ? 0 : 180,
         () -> Presets.CLOSE_SHOOTER_ANGLE, 
         () -> Presets.CLOSE_ELEVATOR_HEIGHT

src/main/java/frc/robot/commands/auto/AutoAim.java

@@ -64,7 +64,7 @@ private AutoAim(
   }
 
   /** 
-   * Creates a new AutoAim2. <strong>(CALCULATED TELEOP)</strong>
+   * Creates a new AutoAim2. <strong>(FOR CALCULATED TELEOP)</strong>
    * 
    * This command will auto-aim to calculated setpoints repeatedly.
    */
@@ -80,7 +80,7 @@ public AutoAim(
   }
 
   /** 
-   * Creates a new AutoAim2. <strong>(PRESET TELEOP)</strong>
+   * Creates a new AutoAim2. <strong>(FOR PRESET TELEOP)</strong>
    * 
    * This command will auto-aim to preset setpoints repeatedly.
    */
@@ -99,7 +99,7 @@ public AutoAim(
   }
 
   /** 
-   * Creates a new AutoAim2. <strong>(CALCULATED AUTON)</strong>
+   * Creates a new AutoAim2. <strong>(FOR CALCULATED AUTON)</strong>
    * 
    * This command will auto-aim to calculated setpoints once.
    */
@@ -113,7 +113,7 @@ public AutoAim(
   }
 
   /** 
-   * Creates a new AutoAim2. <strong>(PRESET AUTON)</strong>
+   * Creates a new AutoAim2. <strong>(FOR PRESET AUTON)</strong>
    * 
    * This command will auto-aim to preset setpoints once.
    */

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

@@ -32,7 +32,7 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    _shooter.driveAngle(_angleSpeed.getAsDouble() * Constants.Speeds.SHOOTER_ANGLE_MAX_SPEED); // TODO: find speed coeff
+    _shooter.driveAngle(_angleSpeed.getAsDouble() * Constants.Speeds.SHOOTER_ANGLE_MAX_SPEED);
 
   }
 

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

@@ -3,8 +3,6 @@
 
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
-import javax.sound.sampled.Port;
-
 import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.CANSparkMax;
@@ -15,18 +13,14 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
-import frc.robot.Constants.Encoders;
 import frc.robot.Constants.FeedForward;
 import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.PID;
 import frc.robot.Constants.Physical;
-import frc.robot.Constants.Ports;
 import frc.robot.Constants.Presets;
 import frc.robot.Constants.Speeds;
 import frc.robot.utils.UtilFuncs;
@@ -46,20 +40,17 @@ public class ShooterSubsystem extends SubsystemBase {
   private final TalonFX _angleMotor = new TalonFX(Constants.CAN.SHOOTER_ANGLE);
 
   private final RelativeEncoder _leftEncoder = _leftMotor.getEncoder();
-  // private final DutyCycleEncoder _angleEncoderAbsolute = new DutyCycleEncoder(Ports.ANGLE_ENCODER);
+  private final Encoder _revShooterEncoder = new Encoder(1, 2, false, Encoder.EncodingType.k2X);
 
   private final ArmFeedforward _angleFeed = new ArmFeedforward(0, FeedForward.SHOOTER_ANGLE_KG, 0);
   private final PIDController _angleController = new PIDController(PID.SHOOTER_ANGLE_KP, 0, 0);
 
   private final Debouncer _beamDebouncer = new Debouncer(0.3, DebounceType.kRising);
-  private final Encoder _incremental = new Encoder(1, 2, false, Encoder.EncodingType.k2X);
 
   private double _shooterTrim = 1.55;
 
   private boolean _holdNote = false;
 
-  private final boolean ABSOLUTE_RESET = false;
-
   /** Represents the state of the shooter's flywheels (speaker shoot, amp, nothing). */
   public enum ShooterState {
     SHOOT,
@@ -76,15 +67,9 @@ public ShooterSubsystem() {
 
     TalonFXConfig.configureFalcon(_angleMotor, true);
 
-    _incremental.setDistancePerPulse((360.0 / Physical.SHOOTER_ENCODER_ANGLE_GEAR_RATIO)/8192.0);
-    _incremental.reset();
-
-    // for resetting absolute angle
-    // _angleEncoder.reset();
-    // SmartDashboard.putNumber("ENC OFFSET", _angleEncoder.getPositionOffset());
+    _revShooterEncoder.setDistancePerPulse((360.0 / Physical.SHOOTER_ENCODER_ANGLE_GEAR_RATIO)/8192.0);
+    _revShooterEncoder.reset();
 
-    // _angleEncoderAbsolute.setDutyCycleRange(1.0/1024.0, 1023.0/1024.0);
-    // _angleEncoderAbsolute.setDistancePerRotation(360 / Physical.SHOOTER_ENCODER_ANGLE_GEAR_RATIO);
     resetAngle();
 
     // soft limits
@@ -111,24 +96,16 @@ public void periodic() {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("SHOOTER SETPOINT", _angleController.getSetpoint());
     SmartDashboard.putNumber("SHOOTER ANGLE", getAngle());
-    // SmartDashboard.putNumber("SHOOTER ABSOLUTE ENCODER", _angleEncoderAbsolute.getDistance() - 80);
     SmartDashboard.putNumber("SHOOTER PERCENT OUTPUT", _leftMotor.get());
-    SmartDashboard.putNumber("SHOOTER INCREMENTAL", _incremental.getDistance());
+    SmartDashboard.putNumber("SHOOTER INCREMENTAL ENCODER", _revShooterEncoder.getDistance());
     SmartDashboard.putBoolean("SHOOTER REVVED", false);
 
     _shooterTrim = SmartDashboard.getNumber("SHOOTER TRIM", _shooterTrim);
   }
 
   // for resetting the shooter's angle
   private void resetAngle() {
-    double resetAngle;
-
-    if (ABSOLUTE_RESET) {
-      // resetAngle = _angleEncoderAbsolute.getDistance() - 80;
-    } else {
-      resetAngle = 0;
-    }
-
+    double resetAngle = 0;
     _angleMotor.setPosition(resetAngle * Physical.SHOOTER_ANGLE_GEAR_RATIO / 360);
   }
 

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

@@ -290,11 +290,11 @@ public SwerveModuleState[] getStates() {
     return states;
   }
 
-  /** Resets the pose estimator's heading of the drive to 0. */
-  public void resetGyro() {
+  /** Resets the pose estimator's heading of the drive. */
+  public void resetGyro(double heading) {
     Pose2d current_pose = getPose();
     Pose2d new_pose = new Pose2d(current_pose.getTranslation().getX(), current_pose.getTranslation().getY(),
-        Rotation2d.fromDegrees(180));
+        Rotation2d.fromDegrees(heading));
 
     resetPose(new_pose);
   }

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

@@ -8,14 +8,11 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants;
 import frc.robot.Constants.PID;
-import frc.robot.Constants.Physical;
 import frc.robot.utils.configs.TalonFXConfig;
 
 /**

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

@@ -4,7 +4,6 @@
 import java.util.function.Supplier;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -19,6 +18,9 @@ public final class UtilFuncs {
   private static AprilTagFieldLayout _field;
   private static Supplier<Translation2d> _shotVectorSupplier;
 
+  private static Pose3d SPEAKER_RED_POSE;
+  private static Pose3d SPEAKER_BLUE_POSE;
+
   /**
    * Loads the AprilTag field.
    */
@@ -30,6 +32,9 @@ public static void LoadField() {
     }
 
     _field.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
+
+    SPEAKER_RED_POSE = _field.getTagPose(FieldConstants.SPEAKER_TAG_RED).get();
+    SPEAKER_BLUE_POSE = _field.getTagPose(FieldConstants.SPEAKER_TAG_BLUE).get();
   }
 
   /**
@@ -41,9 +46,9 @@ public static Pose3d GetSpeakerPose() {
     Pose3d pose;
 
     if (GetAlliance() == Alliance.Red) {
-      pose =  _field.getTagPose(FieldConstants.SPEAKER_TAG_RED).get();
+      pose =  SPEAKER_RED_POSE;
     } else {
-      pose = _field.getTagPose(FieldConstants.SPEAKER_TAG_BLUE).get();
+      pose = SPEAKER_BLUE_POSE;
     }
 
     return new Pose3d(

src/main/java/frc/robot/utils/configs/NeoConfig.java

@@ -30,8 +30,6 @@ public static void configureNeo(CANSparkMax neo, boolean invert) {
     neo.enableSoftLimit(SoftLimitDirection.kForward, false);
     neo.enableSoftLimit(SoftLimitDirection.kReverse, false);
 
-    // neo.setSmartCurrentLimit(50); // TODO: Find
-
     neo.setInverted(invert);
   }
 

src/main/java/frc/robot/utils/configs/TalonFXConfig.java

@@ -25,7 +25,6 @@ public class TalonFXConfig {
    * @return The configuration object applied to the Falcon.
    */
   public static TalonFXConfiguration configureFalcon(TalonFX falcon, boolean invert) {
-    // TODO: will prob need to add the code to zero encoder
     TalonFXConfiguration config = new TalonFXConfiguration();
 
     falcon.getConfigurator().DefaultTimeoutSeconds = Constants.CAN.CAN_TIMEOUT;