LEDs, Button Bindings, Arm + Elevator routines, bug fixes, and cleanup code

simgui.json

@@ -3,10 +3,11 @@
     "types": {
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/Autonomous": "String Chooser",
+      "/SmartDashboard/Visualizers/Arm": "Mechanism2d",
       "/SmartDashboard/Visualizers/Elevator": "Mechanism2d"
     },
     "windows": {
-      "/SmartDashboard/Visualizers/Elevator": {
+      "/SmartDashboard/Visualizers/Arm": {
         "window": {
           "visible": true
         }
@@ -19,11 +20,31 @@
         "Elevator": {
           "open": true
         },
+        "Froggy": {
+          "open": true
+        },
+        "Funnel": {
+          "open": true
+        },
+        "Shooter": {
+          "open": true
+        },
+        "Visualizers": {
+          "Arm": {
+            "open": true
+          },
+          "Elevator": {
+            "open": true
+          }
+        },
         "open": true
       }
     }
   },
   "NetworkTables Info": {
+    "Server": {
+      "open": true
+    },
     "visible": true
   }
 }

src/main/java/com/stuypulse/robot/Robot.java

@@ -40,7 +40,9 @@ public void robotPeriodic() {
     public void disabledInit() {}
 
     @Override
-    public void disabledPeriodic() {}
+    public void disabledPeriodic() {
+        // put your pregame LED check in here
+    }
 
     /***********************/
     /*** AUTONOMOUS MODE ***/

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -10,15 +10,29 @@
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
 
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
+import com.stuypulse.robot.commands.climb.*;
+import com.stuypulse.robot.commands.froggy.*;
 import com.stuypulse.robot.commands.funnel.FunnelDefaultCommand;
+import com.stuypulse.robot.commands.led.LedRainbow;
+import com.stuypulse.robot.commands.led.LedSolidColor;
+import com.stuypulse.robot.commands.routines.*;
+import com.stuypulse.robot.commands.routines.algae.*;
+import com.stuypulse.robot.commands.routines.front_side.*;
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.LED;
+import com.stuypulse.robot.subsystems.arm.Arm;
+import com.stuypulse.robot.subsystems.climb.Climb;
 import com.stuypulse.robot.subsystems.elevator.Elevator;
-import com.stuypulse.robot.subsystems.funnel.CoralFunnel;
+import com.stuypulse.robot.subsystems.froggy.Froggy;
+import com.stuypulse.robot.subsystems.funnel.Funnel;
+import com.stuypulse.robot.subsystems.led.LEDController;
 import com.stuypulse.robot.subsystems.lokishooter.LokiShooter;
 
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 public class RobotContainer {
 
@@ -27,35 +41,102 @@ public class RobotContainer {
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
     // Subsystem
-
-    private final CoralFunnel funnel = CoralFunnel.getInstance();
+    private final Funnel funnel = Funnel.getInstance();
     private final LokiShooter shooter = LokiShooter.getInstance();
+    private final Arm arm = Arm.getInstance();
     private final Elevator elevator = Elevator.getInstance();
+    private final Climb climb = Climb.getInstance();
+    private final Froggy froggy = Froggy.getInstance();
+    private final LEDController ledController = LEDController.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
 
     // Robot container
-
     public RobotContainer() {
         configureDefaultCommands();
         configureButtonBindings();
         configureAutons();
+
+        new Trigger(() -> froggy.isCoralStalling() || shooter.hasCoral())
+                .onTrue(new LedSolidColor(LED.HAS_CORAL_COLOR));
+
+        new Trigger(((FunnelDefaultCommand) funnel.getDefaultCommand())::isUnjamming)
+                .onTrue(new LedSolidColor(LED.UNJAM_COLOR));
+
+        new Trigger(
+                        () ->
+                                (Math.abs(
+                                                climb.getAngle().getDegrees()
+                                                        - Settings.Climb.OPEN_ANGLE.getDegrees())
+                                        <= Settings.Climb.ANGLE_TOLERANCE.getDegrees()))
+                .onTrue(new LedRainbow());
+
+        new Trigger(
+                        () ->
+                                (Math.abs(
+                                                climb.getAngle().getDegrees()
+                                                        - Settings.Climb.CLIMBED_ANGLE.getDegrees())
+                                        <= Settings.Climb.ANGLE_TOLERANCE.getDegrees()))
+                .onTrue(new LedSolidColor(LED.CLIMB_COLOR));
     }
 
     /****************/
-    /*** DEFAULTS ***/
+    /*** DEFAULT ***/
     /****************/
 
     private void configureDefaultCommands() {
         funnel.setDefaultCommand(new FunnelDefaultCommand());
     }
 
     /***************/
-    /*** BUTTONS ***/
+    /*** BUTTON ***/
     /***************/
 
-    private void configureButtonBindings() {}
+    private void configureButtonBindings() {
+
+        /* RIGHT SIDE BUTTONS */
+        // ADD ALIGNMENT TO ALL SCORING ROUTINES
+
+        // BOTTOM BUTTON -> LVL 2 FRONT
+        driver.getBottomButton().whileTrue(new ScoreL2Front()).onFalse(new MoveToFeed());
+
+        // RIGHT BUTTON -> LVL 3 FRONT
+        driver.getRightButton().whileTrue(new ScoreL3Front()).onFalse(new MoveToFeed());
+
+        // TOP BUTTON -> LVL 4 FRONT
+        driver.getTopButton().whileTrue(new ScoreL4Front()).onFalse(new MoveToFeedReverse());
+
+        // LEFT BUTTON -> BARGE SCORE
+        driver.getLeftButton().whileTrue(new ScoreBarge()).onFalse(new MoveToFeed());
+
+        // TOP RIGHT PADDLE -> CLIMB CLOSE
+        driver.getLeftMenuButton().onTrue(new ClimbStow());
+
+        // BOTTOM RIGHT PADDLE -> CLIMB OPEN
+        driver.getDPadRight().onTrue(new ClimbOpen());
+
+        // RIGHT MENU BUTTON -> CLIMB DRIVE
+        driver.getRightMenuButton().onTrue(new ClimbClimb());
+
+        // TOP LEFT PADDLE -> L3 REEF ALGAE INTAKE
+        driver.getDPadLeft().whileTrue(new AcquireAlgaeL3()).onFalse(new MoveToStow());
+
+        // BOTTOM LEFT PADDLE -> L2 REEF ALGAE INTAKE
+        driver.getDPadDown().whileTrue(new AcquireAlgaeL2()).onFalse(new MoveToStow());
+
+        // RIGHT BUMPER -> L1 SCORE
+        driver.getRightBumper().whileTrue(new FroggyScoreL1());
+
+        // RIGHT TRIGGER -> GROUND CORAL INTAKE
+        driver.getRightTriggerButton().whileTrue(new FroggyCoralGroundIntake());
+
+        // LEFT BUMPER -> PROCESSOR SCORE
+        driver.getLeftBumper().whileTrue(new FroggyProcessorScore());
+
+        // LEFT TRIGGER -> GROUND ALGAE INTAKE
+        driver.getLeftTriggerButton().whileTrue(new FroggyAlgaeGroundIntake());
+    }
 
     /**************/
     /*** AUTONS ***/

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToAngle.java → src/main/java/com/stuypulse/robot/commands/arm/ArmToAngle.java

@@ -6,16 +6,18 @@
 
 package com.stuypulse.robot.commands.arm;
 
+import com.stuypulse.robot.commands.led.LedSolidColor;
+import com.stuypulse.robot.constants.Settings.LED;
 import com.stuypulse.robot.subsystems.arm.Arm;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
-public class ArmMoveToAngle extends InstantCommand {
-    private final Arm arm;
-    private final Rotation2d angle;
+public class ArmToAngle extends InstantCommand {
+    protected final Arm arm;
+    protected final Rotation2d angle;
 
-    public ArmMoveToAngle(Rotation2d angle) {
+    public ArmToAngle(Rotation2d angle) {
         arm = Arm.getInstance();
         this.angle = angle;
     }
@@ -24,4 +26,14 @@ public ArmMoveToAngle(Rotation2d angle) {
     public void initialize() {
         arm.setTargetAngle(angle);
     }
+
+    @Override
+    public boolean isFinished() {
+        return arm.atTargetAngle();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        if (interrupted) new LedSolidColor(LED.ABORT_COLOR).schedule();
+    }
 }

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToFunnel.java → src/main/java/com/stuypulse/robot/commands/arm/ArmToFeed.java

@@ -8,13 +8,14 @@
 
 import com.stuypulse.robot.constants.Settings;
 
-public class ArmMoveToFunnel extends ArmMoveToAngle {
-    public ArmMoveToFunnel() {
+public class ArmToFeed extends ArmToAngle {
+    public ArmToFeed() {
         super(Settings.Arm.FUNNEL_ANGLE);
     }
 
     @Override
     public void initialize() {
+        arm.setRotateBoolean(false);
         super.initialize();
     }
 }

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL2Back.java → src/main/java/com/stuypulse/robot/commands/arm/ArmToFeedReverse.java

@@ -8,13 +8,14 @@
 
 import com.stuypulse.robot.constants.Settings;
 
-public class ArmMoveToL2Back extends ArmMoveToAngle {
-    public ArmMoveToL2Back() {
-        super(Settings.Arm.L2_ANGLE_BACK);
+public class ArmToFeedReverse extends ArmToAngle {
+    public ArmToFeedReverse() {
+        super(Settings.Arm.FUNNEL_ANGLE);
     }
 
     @Override
     public void initialize() {
+        arm.setRotateBoolean(true);
         super.initialize();
     }
 }

src/main/java/com/stuypulse/robot/commands/arm/ArmMoveToL3Back.java → src/main/java/com/stuypulse/robot/commands/arm/ArmToVertical.java

@@ -8,13 +8,14 @@
 
 import com.stuypulse.robot.constants.Settings;
 
-public class ArmMoveToL3Back extends ArmMoveToAngle {
-    public ArmMoveToL3Back() {
-        super(Settings.Arm.L3_ANGLE_BACK);
+public class ArmToVertical extends ArmToAngle {
+    public ArmToVertical() {
+        super(Settings.Arm.VERTICAL_ANGLE);
     }
 
     @Override
     public void initialize() {
+        arm.setRotateBoolean(false);
         super.initialize();
     }
 }

src/main/java/com/stuypulse/robot/commands/arm/algae/ArmToAlgaeL2.java

@@ -0,0 +1,22 @@
+/************************ PROJECT MARY *************************/
+/* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
+/* Use of this source code is governed by an MIT-style license */
+/* that can be found in the repository LICENSE file.           */
+/***************************************************************/
+
+package com.stuypulse.robot.commands.arm.algae;
+
+import com.stuypulse.robot.commands.arm.ArmToAngle;
+import com.stuypulse.robot.constants.Settings;
+
+public class ArmToAlgaeL2 extends ArmToAngle {
+    public ArmToAlgaeL2() {
+        super(Settings.Arm.ALGAE_L2_ANGLE);
+    }
+
+    @Override
+    public void initialize() {
+        arm.setRotateBoolean(false);
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/algae/ArmToAlgaeL3.java

@@ -0,0 +1,22 @@
+/************************ PROJECT MARY *************************/
+/* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
+/* Use of this source code is governed by an MIT-style license */
+/* that can be found in the repository LICENSE file.           */
+/***************************************************************/
+
+package com.stuypulse.robot.commands.arm.algae;
+
+import com.stuypulse.robot.commands.arm.ArmToAngle;
+import com.stuypulse.robot.constants.Settings;
+
+public class ArmToAlgaeL3 extends ArmToAngle {
+    public ArmToAlgaeL3() {
+        super(Settings.Arm.ALGAE_L3_ANGLE);
+    }
+
+    @Override
+    public void initialize() {
+        arm.setRotateBoolean(false);
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/algae/ArmToBarge.java

@@ -0,0 +1,22 @@
+/************************ PROJECT MARY *************************/
+/* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
+/* Use of this source code is governed by an MIT-style license */
+/* that can be found in the repository LICENSE file.           */
+/***************************************************************/
+
+package com.stuypulse.robot.commands.arm.algae;
+
+import com.stuypulse.robot.commands.arm.ArmToAngle;
+import com.stuypulse.robot.constants.Settings;
+
+public class ArmToBarge extends ArmToAngle {
+    public ArmToBarge() {
+        super(Settings.Arm.BARGE_ANGLE);
+    }
+
+    @Override
+    public void initialize() {
+        arm.setRotateBoolean(false);
+        super.initialize();
+    }
+}

src/main/java/com/stuypulse/robot/commands/arm/algae/ArmToStow.java

@@ -0,0 +1,22 @@
+/************************ PROJECT MARY *************************/
+/* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */
+/* Use of this source code is governed by an MIT-style license */
+/* that can be found in the repository LICENSE file.           */
+/***************************************************************/
+
+package com.stuypulse.robot.commands.arm.algae;
+
+import com.stuypulse.robot.commands.arm.ArmToAngle;
+import com.stuypulse.robot.constants.Settings;
+
+public class ArmToStow extends ArmToAngle {
+    public ArmToStow() {
+        super(Settings.Arm.STOW_ANGLE);
+    }
+
+    @Override
+    public void initialize() {
+        arm.setRotateBoolean(false);
+        super.initialize();
+    }
+}