Merge branch '35-create-make-determine-waypoint-command' of https://g…

…ithub.com/LakotaRobotics1038/2025RobotProject into 35-create-make-determine-waypoint-command

src/main/java/frc/robot/DriverJoystick.java

@@ -4,6 +4,7 @@
 import frc.robot.constants.IOConstants;
 import frc.robot.commands.DetermineWaypointCommand;
 import frc.robot.constants.AutoConstants;
+import frc.robot.constants.DriveWaypoints;
 import frc.robot.libraries.XboxController1038;
 import frc.robot.subsystems.DriveTrain;
 

src/main/java/frc/robot/OperatorPanel.java

@@ -0,0 +1,101 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.OperatorState.ScoringSide;
+import frc.robot.commands.AcquireAlgaeCommand;
+import frc.robot.commands.AcquireCoralCommand;
+import frc.robot.commands.DisposeAlgaeCommand;
+import frc.robot.commands.DisposeCoralCommand;
+import frc.robot.commands.SetAcquisitionPositionCommand;
+import frc.robot.commands.SetExtensionPositionCommand;
+import frc.robot.commands.SetShoulderPositionCommand;
+import frc.robot.commands.SetWristPositionCommand;
+import frc.robot.constants.IOConstants;
+import frc.robot.constants.ExtensionConstants.ExtensionSetpoints;
+import frc.robot.constants.ShoulderConstants.ShoulderSetpoints;
+import frc.robot.constants.WristConstants.WristSetpoints;
+import frc.robot.subsystems.Extension;
+import frc.robot.subsystems.Shoulder;
+import frc.robot.subsystems.Wrist;
+import frc.robot.utils.AcquisitionPositionSetpoint;
+
+public class OperatorPanel extends GenericHID {
+    private OperatorState operatorState;
+
+    private Extension extension;
+    private Wrist wrist;
+    private Shoulder shoulder;
+
+    public final JoystickButton acquireButton = new JoystickButton(this, IOConstants.kAcquireButtonNumber);
+    public final JoystickButton disposeButton = new JoystickButton(this, IOConstants.kDisposeButtonNumber);
+    public final JoystickButton storageButton = new JoystickButton(this, IOConstants.kStorageButtonNumber);
+    public final JoystickButton bargeButton = new JoystickButton(this, IOConstants.kBargeButtonNumber);
+    public final JoystickButton coralL1Button = new JoystickButton(this, IOConstants.kCoralL1ButtonNumber);
+    public final JoystickButton coralL2Button = new JoystickButton(this, IOConstants.kCoralL2ButtonNumber);
+    public final JoystickButton coralL3Button = new JoystickButton(this, IOConstants.kCoralL3ButtonNumber);
+    public final JoystickButton coralL4Button = new JoystickButton(this, IOConstants.kCoralL4ButtonNumber);
+    public final JoystickButton algaeL23Button = new JoystickButton(this, IOConstants.kAlgaeL23ButtonNumber);
+    public final JoystickButton algaeL34Button = new JoystickButton(this, IOConstants.kAlgaeL34ButtonNumber);
+    public final JoystickButton feederButton = new JoystickButton(this, IOConstants.kFeederButtonNumber);
+    public final JoystickButton processorButton = new JoystickButton(this, IOConstants.kProcesssorButtonNumber);
+    public final JoystickButton coralPosScoringSwitch = new JoystickButton(this,
+            IOConstants.kCoralPosScoringSwitchNumber);
+
+    private OperatorPanel() {
+        super(IOConstants.kOperatorPanelPort);
+        this.operatorState = OperatorState.getInstance();
+        operatorState.setLastInput(AcquisitionPositionSetpoint.Storage);
+        operatorState.setScoringSide(coralPosScoringSwitch.getAsBoolean() ? ScoringSide.LEFT : ScoringSide.RIGHT);
+
+        this.acquireButton.and(operatorState::isCoral).whileTrue(new AcquireCoralCommand());
+        this.acquireButton.and(operatorState::isAlgae).whileTrue(new AcquireAlgaeCommand());
+        this.disposeButton.and(operatorState::isCoral).whileTrue(new DisposeCoralCommand());
+        this.disposeButton.and(operatorState::isAlgae).whileTrue(new DisposeAlgaeCommand());
+        this.storageButton.toggleOnTrue(new SetAcquisitionPositionCommand(AcquisitionPositionSetpoint.Storage));
+        this.bargeButton.toggleOnTrue(new SetAcquisitionPositionCommand(AcquisitionPositionSetpoint.Barge));
+        this.coralL1Button.onTrue(
+                new InstantCommand(() -> operatorState.setLastInput(AcquisitionPositionSetpoint.L1Coral)));
+        this.coralL2Button
+                .onTrue(new InstantCommand(() -> operatorState.setLastInput(AcquisitionPositionSetpoint.L2Coral)));
+        this.coralL3Button
+                .onTrue(new InstantCommand(() -> operatorState.setLastInput(AcquisitionPositionSetpoint.L3Coral)));
+        this.coralL4Button
+                .onTrue(new InstantCommand(() -> operatorState.setLastInput(AcquisitionPositionSetpoint.L4Coral)));
+        this.algaeL23Button
+                .onTrue(new InstantCommand(() -> operatorState.setLastInput(AcquisitionPositionSetpoint.L23Algae)));
+        this.algaeL34Button
+                .onTrue(new InstantCommand(() -> operatorState.setLastInput(AcquisitionPositionSetpoint.L34Algae)));
+        this.processorButton
+                .onTrue(new InstantCommand(() -> operatorState.setLastInput(AcquisitionPositionSetpoint.Processor)));
+        this.feederButton.onTrue(
+                new InstantCommand(() -> operatorState.setLastInput(AcquisitionPositionSetpoint.FeederStation)));
+        this.coralPosScoringSwitch
+                .onTrue(new InstantCommand(() -> operatorState.setScoringSide(ScoringSide.LEFT)))
+                .onFalse(new InstantCommand(() -> operatorState.setScoringSide(ScoringSide.RIGHT)));
+    }
+
+    // Singleton Setup
+    private static OperatorPanel instance;
+
+    public static OperatorPanel getInstance() {
+        if (instance == null) {
+            System.out.println("Creating a new Operator");
+            instance = new OperatorPanel();
+        }
+        return instance;
+    }
+
+    public void enableDefaults() {
+        extension.setDefaultCommand(new SetExtensionPositionCommand(ExtensionSetpoints.Storage));
+        wrist.setDefaultCommand(new SetWristPositionCommand(WristSetpoints.Storage));
+        shoulder.setDefaultCommand(new SetShoulderPositionCommand(ShoulderSetpoints.Storage));
+    }
+
+    public void clearDefaults() {
+        extension.removeDefaultCommand();
+        wrist.removeDefaultCommand();
+        shoulder.removeDefaultCommand();
+    }
+}

src/main/java/frc/robot/OperatorState.java

@@ -0,0 +1,67 @@
+package frc.robot;
+
+import frc.robot.utils.AcquisitionPositionSetpoint;
+
+public class OperatorState {
+    private AcquisitionPositionSetpoint lastInput;
+    private ScoringSide scoringSide;
+
+    public enum ScoringSide {
+        LEFT, RIGHT;
+    }
+
+    private OperatorState() {
+        this.lastInput = AcquisitionPositionSetpoint.Storage;
+    }
+
+    private static OperatorState instance;
+
+    public static OperatorState getInstance() {
+        if (instance == null) {
+            instance = new OperatorState();
+        }
+
+        return instance;
+    }
+
+    public AcquisitionPositionSetpoint getLastInput() {
+        return this.lastInput;
+    }
+
+    public boolean isCoral() {
+        switch (getLastInput()) {
+            case L1Coral:
+            case L2Coral:
+            case L3Coral:
+            case L4Coral:
+            case FeederStation:
+                return true;
+            default:
+                return false;
+        }
+    }
+
+    public boolean isAlgae() {
+        switch (getLastInput()) {
+            case L23Algae:
+            case L34Algae:
+            case Processor:
+            case Barge:
+                return true;
+            default:
+                return false;
+        }
+    }
+
+    public void setLastInput(AcquisitionPositionSetpoint lastInput) {
+        this.lastInput = lastInput;
+    }
+
+    public void setScoringSide(ScoringSide scoringSide) {
+        this.scoringSide = scoringSide;
+    }
+
+    public ScoringSide getScoringSide() {
+        return this.scoringSide;
+    }
+}

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

@@ -30,11 +30,14 @@ public class Robot extends TimedRobot {
     // Subsystems
     private DriveTrain driveTrain = DriveTrain.getInstance();
 
+    // Human Interface Devices
+    private OperatorPanel operatorPanel = OperatorPanel.getInstance();
+
     @Override
     public void robotInit() {
         // Singleton instances that need to be created but not referenced
         DriverJoystick.getInstance();
-        OperatorJoystick.getInstance();
+        OperatorPanel.getInstance();
         Dashboard.getInstance();
 
         addPeriodic(swagLights::periodic, 0.25);
@@ -67,6 +70,7 @@ public void disabledExit() {
 
     @Override
     public void autonomousInit() {
+        operatorPanel.clearDefaults();
         driveTrain.zeroHeading();
         autonomousCommand = autonSelector.chooseAuton();
         // if (DriverStation.isFMSAttached()) {
@@ -96,6 +100,7 @@ public void autonomousExit() {
 
     @Override
     public void teleopInit() {
+        operatorPanel.enableDefaults();
         Dashboard.getInstance().clearTrajectory();
         driveTrain.configNeutralMode(SwerveConstants.kTeleopDrivingMotorNeutralMode);
     }

src/main/java/frc/robot/commands/AcquireCommand.java → src/main/java/frc/robot/commands/AcquireAlgaeCommand.java

@@ -3,22 +3,22 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Acquisition;
 
-public class AcquireCommand extends Command {
+public class AcquireAlgaeCommand extends Command {
 
     private Acquisition acquisition = Acquisition.getInstance();
 
-    public AcquireCommand() {
+    public AcquireAlgaeCommand() {
         this.addRequirements(acquisition);
     }
 
     @Override
     public void execute() {
-        acquisition.acquire();
+        acquisition.acquireAlgae();
     }
 
     @Override
     public boolean isFinished() {
-        return acquisition.getAlgaeSwitch() || acquisition.getBottomLaser();
+        return false;
     }
 
     @Override

src/main/java/frc/robot/commands/AcquireCoralCommand.java

@@ -0,0 +1,29 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Acquisition;
+
+public class AcquireCoralCommand extends Command {
+
+    private Acquisition acquisition = Acquisition.getInstance();
+
+    public AcquireCoralCommand() {
+        this.addRequirements(acquisition);
+    }
+
+    @Override
+    public void execute() {
+        acquisition.acquireCoral();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return acquisition.getBottomLaser();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        acquisition.stopAcquisition();
+    }
+
+}

src/main/java/frc/robot/commands/ClimbUpCommand.java

@@ -1,7 +1,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-
+import frc.robot.constants.ClimbConstants;
 import frc.robot.subsystems.Climb;
 
 public class ClimbUpCommand extends Command {
@@ -18,7 +18,7 @@ public void execute() {
 
     @Override
     public boolean isFinished() {
-        return false;
+        return climb.getPosition() >= ClimbConstants.maxClimbDistance;
     }
 
     @Override

src/main/java/frc/robot/commands/DisposeAlgaeCommand.java

@@ -0,0 +1,41 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Acquisition;
+
+public class DisposeAlgaeCommand extends Command {
+    private Acquisition acquisition = Acquisition.getInstance();
+    private double secondsToDispose;
+    private Timer timer = new Timer();
+
+    public DisposeAlgaeCommand() {
+        this(0.0);
+    }
+
+    public DisposeAlgaeCommand(double secondsToDispose) {
+        this.secondsToDispose = secondsToDispose;
+        super.addRequirements(acquisition);
+    }
+
+    @Override
+    public void execute() {
+        timer.restart();
+        acquisition.disposeAlgae();
+    }
+
+    @Override
+    public boolean isFinished() {
+        // if algae limit switch is not pressed and set time has passed stop the command
+        if (!acquisition.getAlgaeSwitch()) {
+            return secondsToDispose != 0 && timer.get() >= secondsToDispose;
+        }
+
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        acquisition.stopAcquisition();
+    }
+}

src/main/java/frc/robot/commands/DisposeCommand.java → src/main/java/frc/robot/commands/DisposeCoralCommand.java

@@ -4,35 +4,32 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Acquisition;
 
-public class DisposeCommand extends Command {
+public class DisposeCoralCommand extends Command {
     private Acquisition acquisition = Acquisition.getInstance();
     private double secondsToDispose;
     private Timer timer = new Timer();
 
-    public DisposeCommand() {
+    public DisposeCoralCommand() {
         this(0.0);
     }
 
-    public DisposeCommand(double secondsToDispose) {
+    public DisposeCoralCommand(double secondsToDispose) {
         this.secondsToDispose = secondsToDispose;
         super.addRequirements(acquisition);
     }
 
     @Override
     public void execute() {
         timer.restart();
-        acquisition.dispose();
+        acquisition.disposeCoral();
     }
 
     @Override
     public boolean isFinished() {
-        // if neither acquisition laser returns true, move on to checking the algae
-        // switch. command is finished if all three sensors return false.
+        // if neither acquisition laser returns true command is finished
 
         if (!(acquisition.getTopLaser() || acquisition.getBottomLaser())) {
-            if (!acquisition.getAlgaeSwitch()) {
-                return secondsToDispose == 0.0 ? false : timer.get() >= secondsToDispose;
-            }
+            return secondsToDispose != 0.0 && timer.get() >= secondsToDispose;
         }
 
         return false;