New superstructure barge logic

src/main/java/frc/robot/Controls.java

@@ -730,7 +730,7 @@ private void configureElevatorLEDBindings() {
     }
 
     s.elevatorLEDSubsystem.setDefaultCommand(
-        s.elevatorLEDSubsystem.showScoringMode(() -> soloScoringMode));
+        s.elevatorLEDSubsystem.showScoringMode(() -> soloScoringMode, intakeMode));
 
     if (s.elevatorSubsystem != null) {
       Trigger hasBeenZeroed = new Trigger(s.elevatorSubsystem::getHasBeenZeroed);
@@ -925,7 +925,15 @@ private void configureSoloControllerBindings() {
                   Command scoreCommand;
                   switch (soloScoringMode) {
                     case CORAL_IN_CLAW -> {
-                      scoreCommand = getSoloCoralBranchHeightCommand();
+                      scoreCommand =
+                          getSoloCoralBranchHeightCommand()
+                              .until(
+                                  () ->
+                                      soloController.a().getAsBoolean()
+                                          || soloController.b().getAsBoolean()
+                                          || soloController.x().getAsBoolean()
+                                          || soloController.y().getAsBoolean());
+                      ;
                     }
                     case ALGAE_IN_CLAW -> {
                       Command bargeScoreCommand =
@@ -970,7 +978,13 @@ private void configureSoloControllerBindings() {
                     () -> {
                       Command scoreCommand =
                           switch (soloScoringMode) {
-                            case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand();
+                            case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand()
+                                .until(
+                                    () ->
+                                        soloController.a().getAsBoolean()
+                                            || soloController.b().getAsBoolean()
+                                            || soloController.x().getAsBoolean()
+                                            || soloController.y().getAsBoolean());
                             case ALGAE_IN_CLAW -> Commands.sequence(
                                     superStructure.algaeProcessorScore(
                                         soloController.rightBumper()),

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

@@ -6,8 +6,10 @@
 
 import au.grapplerobotics.CanBridge;
 import com.pathplanner.lib.commands.FollowPathCommand;
+import edu.wpi.first.net.WebServer;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.RobotBase;
@@ -37,7 +39,6 @@ public static Robot getInstance() {
   private final RobotType robotType;
   public final Controls controls;
   public final Subsystems subsystems;
-
   public final Sensors sensors;
   public final SuperStructure superStructure;
   private final PowerDistribution PDH;
@@ -107,6 +108,7 @@ protected Robot() {
       FollowPathCommand.warmupCommand().schedule();
     }
     MatchTab.create(sensors, subsystems);
+    WebServer.start(5800, Filesystem.getDeployDirectory().getPath());
   }
 
   @Override

src/main/java/frc/robot/generated/CompTunerConstants.java

@@ -55,7 +55,7 @@ public class CompTunerConstants {
 
   // The stator current at which the wheels start to slip;
   // This needs to be tuned to your individual robot
-  private static final Current kSlipCurrent = Amps.of(80.0);
+  private static final Current kSlipCurrent = Amps.of(60.0);
 
   // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
   // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.

src/main/java/frc/robot/sensors/ArmSensor.java

@@ -16,8 +16,8 @@ public class ArmSensor {
 
   private final LaserCan mainSensor;
   // VALUES ARE IN METERS
-  private static final double TROUGH_LOWER_LIMIT = 0.10;
-  private static final double TROUGH_UPPER_LIMIT = 0.20;
+  private static final double TROUGH_LOWER_LIMIT = 0.12;
+  private static final double TROUGH_UPPER_LIMIT = 0.22;
   private static final double CLAW_LOWER_LIMIT = 0.01;
   private static final double CLAW_UPPER_LIMIT = 0.09;
 

src/main/java/frc/robot/sensors/ElevatorLight.java

@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Hardware;
+import frc.robot.util.ScoringMode;
 import frc.robot.util.SoloScoringMode;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
@@ -82,15 +83,19 @@ public Command animate(LEDPattern animation, String name) {
         .withName("Animate" + name);
   }
 
-  public Command showScoringMode(Supplier<SoloScoringMode> scoringMode) {
+  public Command showScoringMode(Supplier<SoloScoringMode> scoringMode, ScoringMode intakingMode) {
     return run(() -> {
           SoloScoringMode currentMode = scoringMode.get();
           if (currentMode == SoloScoringMode.ALGAE_IN_CLAW) {
             updateLEDs(LEDPattern.solid(Color.kTeal));
           } else if (currentMode == SoloScoringMode.CORAL_IN_CLAW) {
             updateLEDs(LEDPattern.solid(Color.kWhite));
           } else if (currentMode == SoloScoringMode.NO_GAME_PIECE) {
-            updateLEDs(LEDPattern.solid(Color.kDeepPink));
+            if (intakingMode == ScoringMode.CORAL) {
+              updateLEDs(LEDPattern.solid(Color.kDarkOliveGreen));
+            } else if (intakingMode == ScoringMode.ALGAE) {
+              updateLEDs(LEDPattern.solid(Color.kChocolate));
+            }
           }
         })
         .withName("Animate Scoring Mode");

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

@@ -32,7 +32,7 @@
 
 public class ArmPivot extends SubsystemBase {
   // Presets
-  private final double ARMPIVOT_KP = 50; // previously 38.5
+  private final double ARMPIVOT_KP = 38.5; // previously 50
   private final double ARMPIVOT_KI = 0;
   private final double ARMPIVOT_KD = 0;
   private final double ARMPIVOT_KS = 0.1;
@@ -195,7 +195,7 @@ public void factoryDefaults() {
     talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake;
 
     // enabling current limits
-    talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 80;
+    talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 40;
     talonFXConfiguration.CurrentLimits.StatorCurrentLimitEnable = true;
     talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 20;
     talonFXConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true;

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

@@ -25,7 +25,7 @@ public class GroundArm extends SubsystemBase {
   private final double ARMPIVOT_KV = 4;
   private final double ARMPIVOT_KG = 0.048;
   private final double ARMPIVOT_KA = 0;
-  public static final double STOWED_POSITION = 0.45;
+  public static final double STOWED_POSITION = 0.46;
   public static final double UP_POSITION =
       0.27; // untested - should be somewhere in between stowed and ground
   public static final double GROUND_POSITION = -0.050;

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

@@ -48,7 +48,7 @@ public void configMotors() {
     configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake;
     cfg.apply(configuration);
     // enabling stator current limits
-    currentLimits.StatorCurrentLimit = 40; // subject to change
+    currentLimits.StatorCurrentLimit = 100; // subject to change
     currentLimits.StatorCurrentLimitEnable = true;
     currentLimits.SupplyCurrentLimit = 20; // subject to change
     currentLimits.SupplyCurrentLimitEnable = true;