Algae Commands

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

@@ -12,14 +12,11 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.trigon.robot.commands.CommandConstants;
-import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands;
-import frc.trigon.robot.commands.commandfactories.CoralEjectionCommands;
-import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands;
-import frc.trigon.robot.commands.commandfactories.GeneralCommands;
+import frc.trigon.robot.commands.commandfactories.*;
+import frc.trigon.robot.constants.AutonomousConstants;
 import frc.trigon.robot.constants.CameraConstants;
 import frc.trigon.robot.constants.LEDConstants;
 import frc.trigon.robot.constants.OperatorConstants;
-import frc.trigon.robot.constants.PathPlannerConstants;
 import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
 import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
 import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler;
@@ -95,20 +92,25 @@ private void bindDefaultCommands() {
     private void initializeGeneralSystems() {
         Flippable.init();
         LEDConstants.init();
-        PathPlannerConstants.init();
+        AutonomousConstants.init();
     }
 
     private void bindControllerCommands() {
         OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND);
-        OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);
+//        OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);
         OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand());
 
-        OperatorConstants.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()))));
-        OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand());
+        OperatorConstants.FLOOR_ALGAE_COLLECTION_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getFloorAlgaeCollectionCommand());
+        OperatorConstants.REEF_ALGAE_COLLECTION_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getReefAlgaeCollectionCommand());
 
-        OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand());
+        OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand());
         OperatorConstants.SCORE_CORAL_LEFT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false));
         OperatorConstants.SCORE_CORAL_RIGHT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true));
+        OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand());
+
+        OperatorConstants.SPAWN_CORAL_IN_SIMULATION_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()))));
+        OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE));
+        OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(() -> AlgaeManipulationCommands.toggleLollipopCollection()));
     }
 
     private void configureSysIDBindings(MotorSubsystem subsystem) {

src/main/java/frc/trigon/robot/commands/CommandConstants.java

@@ -7,8 +7,8 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
+import frc.trigon.robot.constants.AutonomousConstants;
 import frc.trigon.robot.constants.OperatorConstants;
-import frc.trigon.robot.constants.PathPlannerConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
 import lib.commands.CameraPositionCalculationCommand;
 import lib.commands.WheelRadiusCharacterizationCommand;
@@ -45,7 +45,7 @@ public class CommandConstants {
                     () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX())
             ),
             WHEEL_RADIUS_CHARACTERIZATION_COMMAND = new WheelRadiusCharacterizationCommand(
-                    PathPlannerConstants.ROBOT_CONFIG.moduleLocations,
+                    AutonomousConstants.ROBOT_CONFIG.moduleLocations,
                     RobotContainer.SWERVE::getDriveWheelPositionsRadians,
                     () -> RobotContainer.SWERVE.getHeading().getRadians(),
                     (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond), null),

src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java

@@ -6,7 +6,7 @@
 import edu.wpi.first.wpilibj2.command.*;
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
-import frc.trigon.robot.constants.PathPlannerConstants;
+import frc.trigon.robot.constants.AutonomousConstants;
 import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
 import lib.utilities.flippable.FlippableRotation2d;
@@ -44,24 +44,24 @@ public static Translation2d calculateDistanceFromTrackedGamePiece() {
         final Translation2d robotToGamePiece = robotPose.getTranslation().minus(trackedObjectPositionOnField);
         var distanceFromTrackedGamePiece = robotToGamePiece.rotateBy(robotPose.getRotation().unaryMinus());
         Logger.recordOutput("GamePieceAutoDrive/DistanceFromTrackedGamePiece", distanceFromTrackedGamePiece);
-        Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position);
+        Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position);
         return distanceFromTrackedGamePiece;
     }
 
     public static Command getDriveToGamePieceCommand(Supplier<Translation2d> distanceFromTrackedGamePiece) {
         return new SequentialCommandGroup(
-                new InstantCommand(() -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)),
+                new InstantCommand(() -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)),
                 SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
-                        () -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()),
-                        () -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()),
+                        () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()),
+                        () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()),
                         CoralAutoDriveCommand::calculateTargetAngle
                 )
         );
     }
 
     public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrackedGamePiece) {
         return distanceFromTrackedGamePiece != null &&
-                (distanceFromTrackedGamePiece.getNorm() > PathPlannerConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open
+                (distanceFromTrackedGamePiece.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open
     }
 
     public static FlippableRotation2d calculateTargetAngle() {

src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java

@@ -0,0 +1,218 @@
+package frc.trigon.robot.commands.commandfactories;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.*;
+import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.commands.CommandConstants;
+import frc.trigon.robot.commands.commandclasses.WaitUntilChangeCommand;
+import frc.trigon.robot.constants.AutonomousConstants;
+import frc.trigon.robot.constants.FieldConstants;
+import frc.trigon.robot.constants.LEDConstants;
+import frc.trigon.robot.constants.OperatorConstants;
+import frc.trigon.robot.subsystems.arm.ArmCommands;
+import frc.trigon.robot.subsystems.arm.ArmConstants;
+import frc.trigon.robot.subsystems.elevator.ElevatorCommands;
+import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
+import frc.trigon.robot.subsystems.swerve.SwerveCommands;
+import lib.hardware.RobotHardwareStats;
+import lib.hardware.misc.leds.LEDCommands;
+import lib.utilities.flippable.FlippablePose2d;
+import lib.utilities.flippable.FlippableRotation2d;
+import lib.utilities.flippable.FlippableTranslation2d;
+
+import java.util.List;
+import java.util.Map;
+
+public class AlgaeManipulationCommands {
+    private static boolean
+            IS_HOLDING_ALGAE = false,
+            SHOULD_COLLECT_FROM_LOLLIPOP = false;
+    private static final PIDController ALIGN_TO_REEF_Y_CONTROLLER =
+            RobotHardwareStats.isSimulation() ?
+                    new PIDController(3, 0, 0) :
+                    new PIDController(3, 0, 0);
+
+    public static boolean isHoldingAlgae() {
+        return IS_HOLDING_ALGAE;
+    }
+
+    public static void toggleLollipopCollection() {
+        SHOULD_COLLECT_FROM_LOLLIPOP = !SHOULD_COLLECT_FROM_LOLLIPOP;
+    }
+
+    public static Command getFloorAlgaeCollectionCommand() {
+        return new SequentialCommandGroup(
+                GeneralCommands.getResetFlipArmOverrideCommand(),
+                new InstantCommand(() -> IS_HOLDING_ALGAE = true),
+                CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece),
+                getInitiateReefAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece),
+                new InstantCommand(() -> getScoreAlgaeCommand().schedule()).alongWith(getAlgaeCollectionConfirmationCommand())
+        ).finallyDo(() -> IS_HOLDING_ALGAE = false);
+    }
+
+    public static Command getReefAlgaeCollectionCommand() {
+        return new SequentialCommandGroup(
+                GeneralCommands.getResetFlipArmOverrideCommand(),
+                new InstantCommand(() -> IS_HOLDING_ALGAE = true),
+                CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece),
+                getInitiateReefAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece),
+                getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand())
+        ).alongWith(getAlignToReefCommand()).finallyDo(() -> IS_HOLDING_ALGAE = false);
+    }
+
+    private static Command getAlignToReefCommand() {
+        return new SequentialCommandGroup(
+                SwerveCommands.getDriveToPoseCommand(
+                        AlgaeManipulationCommands::calculateClosestAlgaeCollectionPose,
+                        AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS
+                ),
+                SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
+                        () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()),
+                        () -> ALIGN_TO_REEF_Y_CONTROLLER.calculate(calculateReefRelativeYOffset()),
+                        () -> calculateClosestAlgaeCollectionPose().getRotation()
+                )
+        ).raceWith(
+                new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.ARM::hasGamePiece)),
+                new WaitUntilCommand(OperatorConstants.STOP_REEF_ALGAE_ALIGN_TRIGGER)
+        ).onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy();
+    }
+
+    private static Command getScoreAlgaeCommand() {
+        return new SelectCommand<>(
+                Map.of(
+                        0, getHoldAlgaeCommand(),
+                        1, getScoreInNetCommand(),
+                        2, getScoreInProcessorCommand()
+                ),
+                AlgaeManipulationCommands::getAlgaeScoreMethodSelector
+        ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly().until(() -> !RobotContainer.ARM.hasGamePiece() && !isScoreAlgaeButtonPressed());
+    }
+
+    private static Command getHoldAlgaeCommand() {
+        return new ParallelCommandGroup(
+                ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.HOLD_ALGAE),
+                ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_WITH_ALGAE)
+        );
+    }
+
+    private static Command getScoreInNetCommand() {
+        return new ParallelRaceGroup(
+                ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_NET),
+                getArmScoringSequenceCommand(ArmConstants.ArmState.SCORE_NET, shouldReverseNetScore()),
+                SwerveCommands.getClosedLoopFieldRelativeDriveCommand(
+                        () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()),
+                        () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()),
+                        () -> new FlippableRotation2d(shouldReverseNetScore() ? Rotation2d.kZero : Rotation2d.k180deg, true)
+                ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY)
+        );
+    }
+
+    private static Command getScoreInProcessorCommand() {
+        return new ParallelRaceGroup(
+                ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_PROCESSOR),
+                getArmScoringSequenceCommand(ArmConstants.ArmState.SCORE_PROCESSOR, false),
+                SwerveCommands.getDriveToPoseCommand(
+                                () -> FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE,
+                                AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS
+                        ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE))
+                        .asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY)
+        );
+    }
+
+    private static Command getArmScoringSequenceCommand(ArmConstants.ArmState scoreState, boolean shouldReverseScore) {
+        return new SequentialCommandGroup(
+                ArmCommands.getPrepareForStateCommand(scoreState, shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER),
+                ArmCommands.getSetTargetStateCommand(scoreState, shouldReverseScore)
+        );
+    }
+
+    private static int getAlgaeScoreMethodSelector() {
+        if (OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean())
+            return 1;
+        if (OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean())
+            return 2;
+        return 0;
+    }
+
+    private static boolean shouldReverseNetScore() {//TODO: Implement
+        return false;
+    }
+
+    private static Command getInitiateFloorAlgaeCollectionCommand() {
+        return new ConditionalCommand(
+                getCollectAlgaeFromLollipopSequenceCommand(),
+                getCollectAlgaeFromFloorSequenceCommand(),
+                () -> SHOULD_COLLECT_FROM_LOLLIPOP
+        ).raceWith(
+                new WaitUntilChangeCommand<>(() -> SHOULD_COLLECT_FROM_LOLLIPOP)
+        ).repeatedly();
+    }
+
+    private static Command getCollectAlgaeFromLollipopSequenceCommand() {
+        return new ParallelCommandGroup(
+                GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP),
+                ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP)
+        );
+    }
+
+    private static Command getCollectAlgaeFromFloorSequenceCommand() {
+        return new ParallelCommandGroup(
+                GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_FLOOR),
+                ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_GROUND)
+        );
+    }
+
+    private static Command getInitiateReefAlgaeCollectionCommand() {
+        return new ParallelCommandGroup(
+                ArmCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER.getArmAlgaeCollectionState(), CoralPlacingCommands.shouldReverseScore()),
+                ElevatorCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER.getElevatorAlgaeCollectionState())
+        ).raceWith(
+                new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getArmAlgaeCollectionState),
+                new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getElevatorAlgaeCollectionState)
+        ).repeatedly();
+    }
+
+    private static Command getAlgaeCollectionConfirmationCommand() {
+        return new ParallelCommandGroup(
+                new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)),
+                LEDCommands.getAnimateCommand(LEDConstants.ALGAE_COLLECTION_CONFIRMATION_ANIMATION_SETTINGS)
+        );
+    }
+
+    private static FlippablePose2d calculateClosestAlgaeCollectionPose() {
+        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        final Translation2d reefCenterPosition = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get();
+        final Rotation2d[] reefClockAngles = FieldConstants.REEF_CLOCK_ANGLES;
+        final Transform2d reefCenterToBranchScoringPose = new Transform2d(FieldConstants.REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS, 0, new Rotation2d());
+        final List<Pose2d> scoringPoses = new java.util.ArrayList<>(List.of());
+
+        for (Rotation2d reefClockAngle : reefClockAngles) {
+            final Pose2d reefCenterAtTargetRotation = new Pose2d(reefCenterPosition, reefClockAngle);
+            scoringPoses.add(reefCenterAtTargetRotation.transformBy(reefCenterToBranchScoringPose));
+        }
+
+        final Pose2d closestScoringPose = robotPose.nearest(scoringPoses);
+        return new FlippablePose2d(closestScoringPose.getTranslation(), closestScoringPose.getRotation().plus(CoralPlacingCommands.shouldReverseScore() ? Rotation2d.k180deg : new Rotation2d()).getRadians(), false);
+    }
+
+    private static boolean isScoreAlgaeButtonPressed() {
+        return OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() ||
+                OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean();
+    }
+
+    private static double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPower) {
+        final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle();
+        final double xValue = CommandConstants.calculateDriveStickAxisValue(xPower);
+        final double yValue = CommandConstants.calculateDriveStickAxisValue(yPower);
+
+        return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin());
+    }
+
+    private static double calculateReefRelativeYOffset() {//TODO: Implement
+        return 0;
+    }
+}