tweak elevator safety, add autoPlace LEDs

Author: mwitcpalek

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

@@ -4,7 +4,7 @@ public class RobotStateConstants {
   public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
   public static final double kAlgaeRetreatDistance = 0;
 
-  public static final double kBlueBargeSafeX = 7.6;
+  public static final double kBlueBargeSafeX = 7; // 7.6
   public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX;
 
   public static final double kCoralEjectTimer = 0.25;

src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java

@@ -599,6 +599,7 @@ public void toScoreAlgae() {
         if (isBargeSafe) {
           driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier);
           if (isAutoPlacing) {
+            setAutoPlacingLed(true);
             driveSubsystem.setIgnoreSticks(true);
             bargeAlignSubsystem.startBargeAlign(allianceColor);
             setState(RobotStates.BARGE_ALIGN);
@@ -958,10 +959,12 @@ public void periodic() {
         } else if (!algaeSubsystem.hasAlgae()
             && scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer)
             && isEjectingAlgae) {
+          setAutoPlacingLed(false);
           driveSubsystem.setIgnoreSticks(false);
           isEjectingAlgae = false;
           toStowSafe();
         } else if (algaeHeight != currentAlgaeHeight) {
+          setAutoPlacingLed(false);
           toScoreAlgae();
         }
       }