Adding Algae Mech to Robot State

.vscode/settings.json

@@ -57,5 +57,5 @@
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
   ],
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable"
 }

build.gradle

@@ -81,7 +81,7 @@ dependencies {
 
     implementation 'ch.qos.logback:logback-classic:1.3.5' //logging
     implementation("com.opencsv:opencsv:5.6")
-    implementation 'org.strykeforce:wallEYE:25.0.0'
+    implementation 'org.strykeforce:wallEYE:25.0.1'
     implementation 'net.jafama:jafama:2.3.2' //fastMath
 }
 

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

@@ -9,11 +9,17 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.algae.IntakeAlgaeCommand;
 import frc.robot.commands.algae.OpenLoopAlgaeCommand;
+import frc.robot.commands.algae.ProcessorAlgaeCommand;
+import frc.robot.commands.algae.ToggleHasAlgaeCommand;
+import frc.robot.commands.biscuit.HoldBiscuitCommand;
+import frc.robot.commands.biscuit.JogBiscuitCommand;
 import frc.robot.commands.coral.EnableEjectBeamCommand;
 import frc.robot.commands.coral.OpenLoopCoralCommand;
 import frc.robot.commands.drive.DriveTeleopCommand;
@@ -22,9 +28,19 @@
 import frc.robot.commands.elevator.JogElevatorCommand;
 import frc.robot.commands.elevator.SetElevatorPositionCommand;
 import frc.robot.commands.elevator.ZeroElevatorCommand;
-import frc.robot.commands.robotState.ScoreReefManualCommand;
+import frc.robot.commands.robotState.FloorAlgaeCommand;
+import frc.robot.commands.robotState.HPAlgaeCommand;
+import frc.robot.commands.robotState.InterruptAutoCommand;
+import frc.robot.commands.robotState.ReefCycleCommand;
+import frc.robot.commands.robotState.ScoreAlgaeCommand;
+import frc.robot.commands.robotState.SetScoreSideRightCommand;
 import frc.robot.commands.robotState.SetScoringLevelCommand;
 import frc.robot.commands.robotState.StowCommand;
+import frc.robot.commands.robotState.ToggleAlgaeHeightCommand;
+import frc.robot.commands.robotState.ToggleAutoCommand;
+import frc.robot.commands.robotState.ToggleGetAlgaeCommand;
+import frc.robot.commands.robotState.setScoreSideLeftCommand;
+import frc.robot.constants.BiscuitConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.RobotConstants;
 import frc.robot.controllers.FlyskyJoystick;
@@ -119,7 +135,7 @@ public RobotContainer() {
     ledIO = new LEDIO();
     ledSubsystem = new LEDSubsystem();
 
-    visionSubsystem = new VisionSubsystem();
+    visionSubsystem = new VisionSubsystem(driveSubsystem);
 
     tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem);
 
@@ -142,6 +158,8 @@ public RobotContainer() {
     configureTelemetry();
     configureDriverBindings();
     configureOperatorBindings();
+    // configureTestOperatorBindings();
+    configurePitDashboard();
   }
 
   private void configureTelemetry() {
@@ -150,6 +168,7 @@ private void configureTelemetry() {
     algaeSubsystem.registerWith(telemetryService);
     elevatorSubsystem.registerWith(telemetryService);
     funnelSubsystem.registerWith(telemetryService);
+    biscuitSubsystem.registerWith(telemetryService);
     telemetryService.start();
   }
 
@@ -158,13 +177,79 @@ private void configureDriverBindings() {
         new DriveTeleopCommand(
             () -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem));
 
-    // Reset Gyro Command
+    // Reset Gyro Command, stow, interrupt Auton, and zero elev
     new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem));
+    new JoystickButton(driveJoystick, Button.SWD.id)
+        .onTrue(
+            new StowCommand(
+                robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem))
+        .onFalse(
+            new StowCommand(
+                robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
+    new JoystickButton(driveJoystick, Button.SWA.id)
+        .onTrue(new InterruptAutoCommand(robotStateSubsystem))
+        .onFalse(new InterruptAutoCommand(robotStateSubsystem));
+    new JoystickButton(driveJoystick, Button.SWB_UP.id)
+        .onTrue(new ZeroElevatorCommand(elevatorSubsystem))
+        .onFalse(new ZeroElevatorCommand(elevatorSubsystem));
+    new JoystickButton(driveJoystick, Button.SWB_DWN.id)
+        .onTrue(new ZeroElevatorCommand(elevatorSubsystem))
+        .onFalse(new ZeroElevatorCommand(elevatorSubsystem));
+
+    // other stuff
     new JoystickButton(driveJoystick, Button.M_SWH.id)
-        .onTrue(new ScoreReefManualCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem));
+        .onTrue(
+            new ReefCycleCommand(
+                robotStateSubsystem,
+                elevatorSubsystem,
+                coralSubsystem,
+                biscuitSubsystem,
+                algaeSubsystem));
+    new JoystickButton(driveJoystick, Button.M_SWE.id)
+        .onTrue(
+            new ScoreAlgaeCommand(
+                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
+    new JoystickButton(driveJoystick, Button.SWF_UP.id)
+        .onTrue(
+            new FloorAlgaeCommand(
+                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
+        .onFalse(
+            new FloorAlgaeCommand(
+                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
+    new JoystickButton(driveJoystick, Button.SWF_DWN.id)
+        .onTrue(
+            new FloorAlgaeCommand(
+                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
+        .onFalse(
+            new FloorAlgaeCommand(
+                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
   }
 
   private void configureOperatorBindings() {
+    // Move biscuit
+    new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogBiscuitCommand(
+                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
+        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+    new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogBiscuitCommand(
+                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
+        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+
+    // Move elevator
+    new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogElevatorCommand(
+                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
+        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
+    new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogElevatorCommand(
+                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
+        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
+
     // Set Levels
     new Trigger(() -> xboxController.getLeftTriggerAxis() > RobotConstants.kTriggerDeadband)
         .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L1));
@@ -181,12 +266,56 @@ private void configureOperatorBindings() {
             new StowCommand(
                 robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
 
-    // zero elevator, slated for removal
+    // Algae
+    new JoystickButton(xboxController, XboxController.Button.kY.value)
+        .onTrue(new ToggleAlgaeHeightCommand(robotStateSubsystem));
     new JoystickButton(xboxController, XboxController.Button.kX.value)
-        .onTrue(new ZeroElevatorCommand(elevatorSubsystem));
+        .onTrue(new ToggleGetAlgaeCommand(robotStateSubsystem));
+    new JoystickButton(xboxController, XboxController.Button.kB.value)
+        .onTrue(
+            new HPAlgaeCommand(
+                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
+
+    // Scoring
+    new JoystickButton(xboxController, XboxController.Button.kA.value)
+        .onTrue(new ToggleAutoCommand(robotStateSubsystem));
+
+    new JoystickButton(xboxController, XboxController.Button.kRightStick.value)
+        .onTrue(new SetScoreSideRightCommand(robotStateSubsystem));
+    new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
+        .onTrue(new setScoreSideLeftCommand(robotStateSubsystem));
   }
 
   private void configureTestOperatorBindings() {
+    new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value)
+        .onTrue(new IntakeAlgaeCommand(algaeSubsystem));
+    new JoystickButton(xboxController, XboxController.Button.kRightBumper.value)
+        .onTrue(new ProcessorAlgaeCommand(algaeSubsystem));
+
+    // Move biscuit
+    new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogBiscuitCommand(
+                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
+        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+    new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogBiscuitCommand(
+                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
+        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+
+    // Move elevator
+    new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogElevatorCommand(
+                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
+        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
+    new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogElevatorCommand(
+                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
+        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
+
     // Stop Coral
     new JoystickButton(xboxController, XboxController.Button.kB.value)
         .onTrue(new OpenLoopCoralCommand(coralSubsystem, 0));
@@ -201,13 +330,25 @@ private void configureTestOperatorBindings() {
         .onTrue(new OpenLoopCoralCommand(coralSubsystem, 1))
         .onTrue(new EnableEjectBeamCommand(false, coralSubsystem));
 
-    // Move Elevator
+    // Move biscuit
     new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogBiscuitCommand(
+                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
+        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+    new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
+        .onTrue(
+            new JogBiscuitCommand(
+                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
+        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
+
+    // Move elevator
+    new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
         .onTrue(
             new JogElevatorCommand(
                 elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
         .onFalse(new HoldElevatorCommand(elevatorSubsystem));
-    new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
+    new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
         .onTrue(
             new JogElevatorCommand(
                 elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
@@ -243,6 +384,19 @@ private void configureTestOperatorBindings() {
             new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kL4CoralSetpoint));
   }
 
+  public void configurePitDashboard() {
+
+    Shuffleboard.getTab("Pit")
+        .add("Toggle Has Algae", new ToggleHasAlgaeCommand(algaeSubsystem))
+        .withPosition(3, 2)
+        .withSize(1, 1);
+
+    Shuffleboard.getTab("Pit")
+        .add("Zero Elevator", new ZeroElevatorCommand(elevatorSubsystem))
+        .withPosition(2, 1)
+        .withSize(1, 1);
+  }
+
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");
   }

src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java

@@ -0,0 +1,17 @@
+package frc.robot.commands.algae;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+
+public class IntakeAlgaeCommand extends InstantCommand {
+  private AlgaeSubsystem algaeSubsystem;
+
+  public IntakeAlgaeCommand(AlgaeSubsystem algaeSubsystem) {
+    this.algaeSubsystem = algaeSubsystem;
+  }
+
+  @Override
+  public void initialize() {
+    algaeSubsystem.intake();
+  }
+}

src/main/java/frc/robot/commands/algae/ProcessorAlgaeCommand.java

@@ -0,0 +1,17 @@
+package frc.robot.commands.algae;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+
+public class ProcessorAlgaeCommand extends InstantCommand {
+  private AlgaeSubsystem algaeSubsystem;
+
+  public ProcessorAlgaeCommand(AlgaeSubsystem algaeSubsystem) {
+    this.algaeSubsystem = algaeSubsystem;
+  }
+
+  @Override
+  public void initialize() {
+    algaeSubsystem.scoreProcessor();
+  }
+}

src/main/java/frc/robot/commands/algae/ToggleHasAlgaeCommand.java

@@ -0,0 +1,19 @@
+package frc.robot.commands.algae;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeStates;
+
+public class ToggleHasAlgaeCommand extends InstantCommand {
+  private AlgaeSubsystem algaeSubsystem;
+
+  public ToggleHasAlgaeCommand(AlgaeSubsystem algaeSubsystem) {
+    this.algaeSubsystem = algaeSubsystem;
+  }
+
+  @Override
+  public void initialize() {
+    algaeSubsystem.setState(
+        algaeSubsystem.getState() == AlgaeStates.EMPTY ? AlgaeStates.HAS_ALGAE : AlgaeStates.EMPTY);
+  }
+}

src/main/java/frc/robot/commands/biscuit/HoldBiscuitCommand.java

@@ -0,0 +1,18 @@
+package frc.robot.commands.biscuit;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+
+public class HoldBiscuitCommand extends InstantCommand {
+  private BiscuitSubsystem biscuitSubsystem;
+
+  public HoldBiscuitCommand(BiscuitSubsystem biscuitSubsystem) {
+    this.biscuitSubsystem = biscuitSubsystem;
+    addRequirements(biscuitSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    biscuitSubsystem.setPosition(biscuitSubsystem.getPosition());
+  }
+}

src/main/java/frc/robot/commands/biscuit/JogBiscuitCommand.java

@@ -0,0 +1,32 @@
+package frc.robot.commands.biscuit;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+
+public class JogBiscuitCommand extends Command {
+
+  private BiscuitSubsystem biscuitSubsystem;
+  private Angle positionChange;
+
+  public JogBiscuitCommand(BiscuitSubsystem biscuitSubsystem, Angle positionChange) {
+    this.biscuitSubsystem = biscuitSubsystem;
+    this.positionChange = positionChange;
+    addRequirements(biscuitSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange));
+  }
+
+  @Override
+  public void execute() {
+    biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange));
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,17 @@
+package frc.robot.commands.robotState;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+
+public class ClimbCommand extends InstantCommand {
+  RobotStateSubsystem robotState;
+
+  public ClimbCommand(RobotStateSubsystem robotState) {
+    this.robotState = robotState;
+  }
+
+  @Override
+  public void initialize() {
+    robotState.toClimb();
+  }
+}