Merge branch 'main' into tag-align

Author: mwitcpalek

src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java

@@ -43,7 +43,7 @@ public boolean isFinished() {
     } else {
       return robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE
           || robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE
-          || !robotStateSubsystem.isBargeSafe;
+          || !robotStateSubsystem.getIsBargeSafe();
     }
   }
 }

src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java

@@ -25,7 +25,7 @@ public ScoreReefManualCommand(
   public void initialize() {
     startingRobotState = robotStateSubsystem.getState();
     startingElevatorFinished = elevatorSubsystem.isFinished();
-    robotStateSubsystem.setIsAuto(false);
+    robotStateSubsystem.setIsAutoPlacing(false);
     robotStateSubsystem.setGetAlgaeOnCycle(false);
     robotStateSubsystem.toPrepCoral();
   }

src/main/java/frc/robot/commands/robotState/ToggleGetAlgaeCommand.java

@@ -12,6 +12,6 @@ public ToggleGetAlgaeCommand(RobotStateSubsystem robotStateSubsystem) {
 
   @Override
   public void initialize() {
-    robotStateSubsystem.ToggleGetAlgaeOnCycle();
+    robotStateSubsystem.toggleGetAlgaeOnCycle();
   }
 }

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

@@ -28,8 +28,6 @@ public class BiscuitConstants {
   public static final double kTicksPerRot = 160;
   public static final int talonID = 25;
   public static final double kCloseEnough = 0.05;
-  public static final Angle kMaxFwd = Rotations.of(51.04735 + 5);
-  public static final Angle kMaxRev = Rotations.of(-12.3489 - 5);
   public static final double kSafeToStowUpper = 40;
   public static final double kSafeToStowLower = -5;
 
@@ -43,8 +41,8 @@ public class BiscuitConstants {
   public static final Angle kL2AlgaeSetpoint = Rotations.of(24.104);
   public static final Angle kL3AlgaeSetpoint = Rotations.of(24.562);
 
-  public static final Angle kL2AlgaeRemovalSetpoint = kL2AlgaeSetpoint;
-  public static final Angle kL3AlgaeRemovalSetpoint = kL3AlgaeSetpoint;
+  public static final Angle kL2AlgaeRemovalSetpoint = kStowSetpoint;
+  public static final Angle kL3AlgaeRemovalSetpoint = kStowSetpoint;
 
   // Coral score
   public static final Angle kL1CoralSetpoint = kStowSetpoint;
@@ -66,6 +64,10 @@ public class BiscuitConstants {
   public static final double kJogAmountUp = 10;
   public static final double kJogAmountDown = -10;
 
+  // Soft Limits
+  public static final Angle kMaxFwd = kMicAlgaeSetpoint.plus(Rotations.of(5));
+  public static final Angle kMaxRev = kBargeBackwardSetpoint.minus(Rotations.of(5));
+
   // Disables the TalonFXS by setting its voltage to zero.
   public static VoltageConfigs disableTalon() {
     VoltageConfigs voltage =

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

@@ -25,12 +25,11 @@ public class ElevatorConstants {
   public static final double kMaxRev = 2;
   public static final int kZeroMultiple =
       0; // some constant to multiply, add by to turn the analog input into a position
-  public static final double kZeroSpeed = -.05;
+  public static final double kZeroSpeed = -0.05;
   public static final double kZeroVolts = -0.5;
   public static final int kZeroCounter = 2;
-  public static final double kZeroedThreshhold = .025;
+  public static final double kZeroedThreshhold = 0.025;
 
-  public static final int heightAnalogID = 0;
   public static final int kFxIDMain = 20;
   public static final int kFxIDFollow = 21;
 
@@ -74,7 +73,6 @@ public static TalonFXConfiguration getBothFXConfig() {
     CurrentLimitsConfigs current =
         new CurrentLimitsConfigs()
             .withStatorCurrentLimitEnable(false)
-            // .withStatorCurrentLimit(20)
             .withSupplyCurrentLimitEnable(true)
             .withSupplyCurrentLimit(40)
             .withSupplyCurrentLowerLimit(10)
@@ -129,7 +127,7 @@ public static TalonFXConfiguration getBothFXConfig() {
 
   public static CurrentLimitsConfigs getZeroingCurrentLimitsConfigs() {
     CurrentLimitsConfigs current =
-        new CurrentLimitsConfigs() // TODO actually have correct limits for zeroing
+        new CurrentLimitsConfigs()
             .withStatorCurrentLimitEnable(false)
             .withStatorCurrentLimit(20)
             .withSupplyCurrentLimit(10)

src/main/java/frc/robot/subsystems/elevator/ElevatorIOFX.java

@@ -12,7 +12,6 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.wpilibj.AnalogInput;
 import frc.robot.constants.ElevatorConstants;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
@@ -30,7 +29,6 @@ public class ElevatorIOFX implements ElevatorIO {
   TalonFXConfigurator configuratorBack;
   StatusSignal<Angle> currPosition;
   StatusSignal<AngularVelocity> currVelocity;
-  public AnalogInput heightAnalogInput = new AnalogInput(ElevatorConstants.heightAnalogID);
   private MotionMagicVoltage positionRequestMain =
       new MotionMagicVoltage(0).withEnableFOC(false).withSlot(0);
   private Follower positionRequestFollow = new Follower(ElevatorConstants.kFxIDMain, false);

src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java

@@ -90,7 +90,7 @@ public void stopMotor() {
     setPercent(0.0);
   }
 
-  public void ClearCoral() {
+  public void clearCoral() {
     curState = FunnelState.HasNotSeenCoral;
   }
 }

src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java

@@ -90,6 +90,7 @@ public void setStartNode(Character startNode) {
   public void startPathHandler() {
     nodeNames.add(0, startNode);
     isHandling = true;
+    robotStateSubsystem.setIsAutoPlacing(false);
     curState = PathStates.DRIVE_FETCH;
   }
 
@@ -189,6 +190,7 @@ public void killPathHandler() {
     isHandling = false;
     curState = PathStates.DONE;
     runningPath = false;
+    robotStateSubsystem.setIsAutoPlacing(true);
     timer.stop();
     timer.reset();
   }