From 2c8b0dc44578c53f4258ed5f22d18a8ebb598462 Mon Sep 17 00:00:00 2001 From: Pranav Kausik Date: Sat, 11 Oct 2025 12:57:41 -0700 Subject: [PATCH 1/4] Worked on algae scoop cmd. Removes algae without collecting. Tested and running --- src/main/java/frc/robot/Constants.java | 2 +- .../CollectAlgaeReefScoopCmd.java | 175 ++++++++++++++++++ .../subsystems/brain/BrainSubsystem.java | 4 +- .../manipulator/ManipulatorConstants.java | 4 +- 4 files changed, 181 insertions(+), 4 deletions(-) create mode 100755 src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 831a373..8a5c712 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -104,7 +104,7 @@ public static class ReefConstants { /** * How far to back up from the ALGAE scoring pose. */ - public static final Distance backupDistanceAlgae = Inches.of(20); + public static final Distance backupDistanceAlgae = Inches.of(40); /** * The distance from the center of the ROBOT to the ARM. diff --git a/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java b/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java new file mode 100755 index 0000000..3163ba0 --- /dev/null +++ b/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java @@ -0,0 +1,175 @@ +package frc.robot.commands.robot.collectalgaereef; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.Milliseconds; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; + +import java.util.Optional; + +import org.xerosw.math.XeroMath; +import org.xerosw.util.XeroSequenceCmd; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.RobotContainer; +import frc.robot.Constants.ReefLevel; +import frc.robot.commands.drive.DriveCommands; +import frc.robot.commands.robot.CommandConstants; + +import frc.robot.subsystems.brain.BrainSubsystem; +import frc.robot.subsystems.brain.GamePiece; +import frc.robot.subsystems.brain.SetHoldingCmd; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.grabber.GrabberSubsystem; +import frc.robot.subsystems.grabber.commands.RunGrabberVoltsCmd; +import frc.robot.subsystems.manipulator.ManipulatorConstants; +import frc.robot.subsystems.manipulator.ManipulatorSubsystem; +import frc.robot.subsystems.manipulator.commands.GoToCmd; +import frc.robot.subsystems.manipulator.commands.GoToCmdDirect; +import frc.robot.util.ReefFaceInfo; +import frc.robot.util.ReefUtil; + + +public class CollectAlgaeReefScoopCmd extends XeroSequenceCmd { + private BrainSubsystem brain_ ; + private ManipulatorSubsystem manipulator_; + private GrabberSubsystem grabber_; + private ReefLevel height_ ; + private Drive db_ ; + private boolean eject_ ; + private boolean quick_ ; + + public CollectAlgaeReefScoopCmd(BrainSubsystem brain, Drive db, ManipulatorSubsystem manipulator, GrabberSubsystem grabber, ReefLevel height, boolean eject, boolean quick) { + super("CollectAlgaeReefScoopCmd") ; + brain_ = brain ; + db_ = db ; + manipulator_ = manipulator; + grabber_ = grabber; + height_ = height ; + eject_ = eject ; + quick_ = quick ; + } + + @Override + public void initSequence(SequentialCommandGroup seq) { + ReefLevel level = height_ ; + + Angle angle ; + Distance height ; + + if (height_ == ReefLevel.AskBrain) { + level = brain_.algaeLevel() ; + } + + if (level == ReefLevel.L2 || level == ReefLevel.L1) { + angle = ManipulatorConstants.Arm.Positions.kAlgaeReefCollectNewL2 ; + height = ManipulatorConstants.Elevator.Positions.kAlgaeReefCollectNewL2 ; + } else if (level == ReefLevel.L3 || level == ReefLevel.L4) { + angle = ManipulatorConstants.Arm.Positions.kAlgaeReefCollectNewL3 ; + height = ManipulatorConstants.Elevator.Positions.kAlgaeReefCollectNewL3 ; + } + else { + // + // Bad height value, so we just return and have an empty sequence which + // does nothing. + // + return ; + } + + Optional reefFace = ReefUtil.getTargetedReefFace(db_.getPose()); + if (reefFace.isEmpty()) + return ; + + Alliance a = DriverStation.getAlliance().get() ; + + // + // This is about the actual collect operation + // + Pose2d bup = reefFace.get().getAlgaeBackupPose() ; + Pose2d buprot ; + + if (eject_) { + Rotation2d rot ; + + if (a == Alliance.Red) { + if (XeroMath.normalizeAngleDegrees(bup.getRotation().getDegrees() - 180.0) < 5.0) { + rot = Rotation2d.fromDegrees(90.0) ; + } + else { + rot = Rotation2d.fromDegrees(180.0) ; + } + } + else { + if (XeroMath.normalizeAngleDegrees(bup.getRotation().getDegrees()) < 5.0) { + rot = Rotation2d.fromDegrees(90.0) ; + } + else { + rot = Rotation2d.fromDegrees(0.0) ; + } + } + buprot = new Pose2d(bup.getTranslation(), rot) ; + } + else { + buprot = bup ; + } + + seq.addCommands( + new GoToCmdDirect(manipulator_, height, angle), + RobotContainer.getInstance().gamepad().setLockCommand(true), + //grabber_.setVoltageCommand(Volts.of(-6.0)), + DriveCommands.simplePathCommand(db_, reefFace.get().getAlgaeCollectPose(), bup, + MetersPerSecond.of(2.0), + CommandConstants.ReefDrive.kMaxDriveAcceleration), + new GoToCmdDirect(manipulator_, height.plus(Meters.of(0.05)), angle.minus(Degrees.of(40))), + DriveCommands.simplePathCommand(db_, buprot, bup, + MetersPerSecond.of(3.0), + MetersPerSecondPerSecond.of(3.0))); + + + Command postseq ; + + if (!quick_) { + postseq = Commands.sequence( + RobotContainer.getInstance().gamepad().setLockCommand(false), + new SetHoldingCmd(brain_, GamePiece.ALGAE_HIGH), + new GoToCmdDirect(manipulator_, manipulator_.getElevatorPosition(), + ManipulatorConstants.Arm.Positions.kAlgaeReefHold), + new GentleLowerElevator(manipulator_, ManipulatorConstants.Elevator.Positions.kAlgaeReefHold)) ; + } + else { + postseq = Commands.sequence( + RobotContainer.getInstance().gamepad().setLockCommand(false)) ; + } + + if (eject_) { + seq.addCommands(new RunGrabberVoltsCmd(grabber_, Milliseconds.of(500))) ; + } + else { + seq.addCommands( + new ConditionalCommand( + postseq, + Commands.sequence( + RobotContainer.getInstance().gamepad().setLockCommand(false), + grabber_.setVoltageCommand(Volts.zero()), + new GoToCmdDirect(manipulator_, height, angle) + ), + this::hasAlgae)) ; + } + seq.addCommands(RobotContainer.getInstance().gamepad().setLockCommand(false)) ; + } + + private boolean hasAlgae() { + return grabber_.hasAlgae() ; + } +} diff --git a/src/main/java/frc/robot/subsystems/brain/BrainSubsystem.java b/src/main/java/frc/robot/subsystems/brain/BrainSubsystem.java index 3838e60..d6ea411 100644 --- a/src/main/java/frc/robot/subsystems/brain/BrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/brain/BrainSubsystem.java @@ -27,6 +27,7 @@ import frc.robot.util.ReefUtil; import frc.robot.commands.robot.NullCmd ; import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefCmd; +import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefScoopCmd; import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefGotoCmd; import frc.robot.commands.robot.collectcoral.CollectCoralCmd; import frc.robot.commands.robot.placecoral.PlaceCoralCmd; @@ -556,7 +557,8 @@ private RobotActionCommandList getRobotActionCommand(RobotAction action, ReefLev list.add(new CollectAlgaeReefGotoCmd(this, m_, ReefLevel.AskBrain)) ; conds.add(null) ; - list.add(new CollectAlgaeReefCmd(this, db_, m_, g_, ReefLevel.AskBrain, true, false)) ; + //list.add(new CollectAlgaeReefCmd(this, db_, m_, g_, ReefLevel.AskBrain, true, false)) ; + list.add(new CollectAlgaeReefScoopCmd(this, db_, m_, g_, ReefLevel.AskBrain, false, false)) ; conds.add(() -> { return ReefUtil.getTargetedReefFace(db_.getPose()).isPresent() ; }) ; break ; diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java index a0c2190..86c8cf1 100755 --- a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java +++ b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java @@ -218,8 +218,8 @@ public class Positions { public static final Distance kAlgaeReefCollectL3 = Centimeters.of(78.0) ; public static final Distance kAlgaeReefCollectL2 = Centimeters.of(40.0) ; - public static final Distance kAlgaeReefCollectNewL3 = Centimeters.of(72.0) ; - public static final Distance kAlgaeReefCollectNewL2 = Centimeters.of(32.0) ; + public static final Distance kAlgaeReefCollectNewL3 = Centimeters.of(70.0) ; + public static final Distance kAlgaeReefCollectNewL2 = Centimeters.of(37.0) ; public static final Distance kAlgaeReefCollectNewPos2L3 = Centimeters.of(75.0) ; public static final Distance kAlgaeReefCollectNewPos2L2 = Centimeters.of(37.0) ; From 5598a375e26d32f62165cb211654f19d13112fd8 Mon Sep 17 00:00:00 2001 From: Pranav Kausik Date: Tue, 14 Oct 2025 20:00:00 -0700 Subject: [PATCH 2/4] Updated code with timeout to prevent algae getting stuck --- .../collectalgaereef/CollectAlgaeReefScoopCmd.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java b/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java index 3163ba0..47f17a1 100755 --- a/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java +++ b/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java @@ -9,6 +9,7 @@ import java.util.Optional; +import org.littletonrobotics.junction.Logger; import org.xerosw.math.XeroMath; import org.xerosw.util.XeroSequenceCmd; @@ -131,12 +132,18 @@ public void initSequence(SequentialCommandGroup seq) { DriveCommands.simplePathCommand(db_, reefFace.get().getAlgaeCollectPose(), bup, MetersPerSecond.of(2.0), CommandConstants.ReefDrive.kMaxDriveAcceleration), - new GoToCmdDirect(manipulator_, height.plus(Meters.of(0.05)), angle.minus(Degrees.of(40))), + new GoToCmdDirect( + manipulator_, + height.plus(Meters.of(0.05)), + angle.minus(Degrees.of(40)) + ).raceWith( + Commands.waitTime(Milliseconds.of(300)) + .finallyDo(() -> System.out.println("Removal Deadline Hit")) + ), DriveCommands.simplePathCommand(db_, buprot, bup, MetersPerSecond.of(3.0), MetersPerSecondPerSecond.of(3.0))); - Command postseq ; if (!quick_) { From 196aa803ea3c782be46941a84c0ab69f11465bce Mon Sep 17 00:00:00 2001 From: Pranav Kausik Date: Tue, 14 Oct 2025 21:14:53 -0700 Subject: [PATCH 3/4] Merged CollectAlgaeReefCmd with CollectAlgaeReefScoopCmd. Made normal algae collect more accurate using similar code from new scoop removal method. Deprecated old CollectAlgaeReef files. Needs cleanup --- .../frc/robot/commands/auto/AutoCommands.java | 14 +- .../collectalgaereef/CollectAlgaeReefCmd.java | 171 ------------------ .../CollectAlgaeReefScoopCmd.java | 79 ++------ .../subsystems/brain/BrainSubsystem.java | 21 +-- .../grabber/commands/CollectAlgaeCmd.java | 37 ---- .../grabber/commands/CollectAlgaeNewCmd.java | 35 ---- 6 files changed, 37 insertions(+), 320 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefCmd.java delete mode 100644 src/main/java/frc/robot/subsystems/grabber/commands/CollectAlgaeCmd.java delete mode 100644 src/main/java/frc/robot/subsystems/grabber/commands/CollectAlgaeNewCmd.java diff --git a/src/main/java/frc/robot/commands/auto/AutoCommands.java b/src/main/java/frc/robot/commands/auto/AutoCommands.java index e6014f8..eef09cb 100644 --- a/src/main/java/frc/robot/commands/auto/AutoCommands.java +++ b/src/main/java/frc/robot/commands/auto/AutoCommands.java @@ -1,15 +1,15 @@ package frc.robot.commands.auto; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Milliseconds; +import static edu.wpi.first.units.Units.Seconds; + import java.util.Optional; import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; - -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Milliseconds; -import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -21,7 +21,7 @@ import frc.robot.commands.misc.StateCmd; import frc.robot.commands.robot.WaitForCoralInRobot; import frc.robot.commands.robot.algaenet.AlgaeNetWhileMovingCmd; -import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefCmd; +import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefScoopCmd; import frc.robot.commands.robot.collectalgaereef.GentleLowerElevator; import frc.robot.commands.robot.collectcoral.CollectCoralCmd; import frc.robot.commands.robot.placecoral.PlaceCoralCmd; @@ -415,7 +415,7 @@ public static AutoModeBaseCmd oneCoralOneAlgaeProcessorAuto(BrainSubsystem brain addToSequence(seq, driveSub.stopCmd()); addToSequence(seq, logState(modename, "Collect Algae")); - addToSequence(seq, new CollectAlgaeReefCmd(brainSub, driveSub, manipSub, grabberSub, ReefLevel.L2, false, true)); + addToSequence(seq, new CollectAlgaeReefScoopCmd(brainSub, driveSub, manipSub, grabberSub, ReefLevel.L2, false, true)); addToSequence(seq, logState(modename, "Drive Processor")); addToSequence(seq, @@ -460,7 +460,7 @@ public static AutoModeBaseCmd oneCoralOneAlgaeBargeAuto(BrainSubsystem brainSub, addToSequence(seq, driveSub.stopCmd()); addToSequence(seq, logState(modename, "Collect Algae")); - addToSequence(seq, new CollectAlgaeReefCmd(brainSub, driveSub, manipSub, grabberSub, ReefLevel.L2, false, false)); + addToSequence(seq, new CollectAlgaeReefScoopCmd(brainSub, driveSub, manipSub, grabberSub, ReefLevel.L2, false, false)); addToSequence(seq, logState(modename, "Drive Barge")); addToSequence(seq, DriveCommands.followPathCommand("BargeBarge")); diff --git a/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefCmd.java b/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefCmd.java deleted file mode 100644 index 3e9acc2..0000000 --- a/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefCmd.java +++ /dev/null @@ -1,171 +0,0 @@ -package frc.robot.commands.robot.collectalgaereef; - -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.Milliseconds; -import static edu.wpi.first.units.Units.Volts; - -import java.util.Optional; - -import org.xerosw.math.XeroMath; -import org.xerosw.util.XeroSequenceCmd; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.RobotContainer; -import frc.robot.Constants.ReefLevel; -import frc.robot.commands.drive.DriveCommands; -import frc.robot.commands.robot.CommandConstants; -import frc.robot.subsystems.brain.BrainSubsystem; -import frc.robot.subsystems.brain.GamePiece; -import frc.robot.subsystems.brain.SetHoldingCmd; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.grabber.GrabberSubsystem; -import frc.robot.subsystems.grabber.commands.CollectAlgaeNewCmd; -import frc.robot.subsystems.grabber.commands.RunGrabberVoltsCmd; -import frc.robot.subsystems.manipulator.ManipulatorConstants; -import frc.robot.subsystems.manipulator.ManipulatorSubsystem; -import frc.robot.subsystems.manipulator.commands.GoToCmdDirect; -import frc.robot.util.ReefFaceInfo; -import frc.robot.util.ReefUtil; - -public class CollectAlgaeReefCmd extends XeroSequenceCmd { - private BrainSubsystem brain_ ; - private ManipulatorSubsystem manipulator_; - private GrabberSubsystem grabber_; - private ReefLevel height_ ; - private Drive db_ ; - private boolean eject_ ; - private boolean quick_ ; - - public CollectAlgaeReefCmd(BrainSubsystem brain, Drive db, ManipulatorSubsystem manipulator, GrabberSubsystem grabber, ReefLevel height, boolean eject, boolean quick) { - super("CollectAlgaeReefCmd") ; - brain_ = brain ; - db_ = db ; - manipulator_ = manipulator; - grabber_ = grabber; - height_ = height ; - eject_ = eject ; - quick_ = quick ; - } - - @Override - public void initSequence(SequentialCommandGroup seq) { - ReefLevel level = height_ ; - - Angle angle ; - Distance height ; - - if (height_ == ReefLevel.AskBrain) { - level = brain_.algaeLevel() ; - } - - if (level == ReefLevel.L2 || level == ReefLevel.L1) { - angle = ManipulatorConstants.Arm.Positions.kAlgaeReefCollectNewL2 ; - height = ManipulatorConstants.Elevator.Positions.kAlgaeReefCollectNewL2 ; - } else if (level == ReefLevel.L3 || level == ReefLevel.L4) { - angle = ManipulatorConstants.Arm.Positions.kAlgaeReefCollectNewL3 ; - height = ManipulatorConstants.Elevator.Positions.kAlgaeReefCollectNewL3 ; - } - else { - // - // Bad height value, so we just return and have an empty sequence which - // does nothing. - // - return ; - } - - Optional reefFace = ReefUtil.getTargetedReefFace(db_.getPose()); - if (reefFace.isEmpty()) - return ; - - Alliance a = DriverStation.getAlliance().get() ; - - // - // This is about the actual collect operation - // - Pose2d bup = reefFace.get().getAlgaeBackupPose() ; - Pose2d buprot ; - - if (eject_) { - Rotation2d rot ; - - if (a == Alliance.Red) { - if (XeroMath.normalizeAngleDegrees(bup.getRotation().getDegrees() - 180.0) < 5.0) { - rot = Rotation2d.fromDegrees(90.0) ; - } - else { - rot = Rotation2d.fromDegrees(180.0) ; - } - } - else { - if (XeroMath.normalizeAngleDegrees(bup.getRotation().getDegrees()) < 5.0) { - rot = Rotation2d.fromDegrees(90.0) ; - } - else { - rot = Rotation2d.fromDegrees(0.0) ; - } - } - buprot = new Pose2d(bup.getTranslation(), rot) ; - } - else { - buprot = bup ; - } - - seq.addCommands( - new GoToCmdDirect(manipulator_, height, angle), - RobotContainer.getInstance().gamepad().setLockCommand(true), - grabber_.setVoltageCommand(Volts.of(-6.0)), - DriveCommands.simplePathCommand(db_, reefFace.get().getAlgaeCollectPose(), bup, - MetersPerSecond.of(2.0), - CommandConstants.ReefDrive.kMaxDriveAcceleration), - new CollectAlgaeNewCmd(grabber_, manipulator_, level), - DriveCommands.simplePathCommand(db_, buprot, bup, - MetersPerSecond.of(3.0), - MetersPerSecondPerSecond.of(3.0))) ; - - Command postseq ; - - if (!quick_) { - postseq = Commands.sequence( - RobotContainer.getInstance().gamepad().setLockCommand(false), - new SetHoldingCmd(brain_, GamePiece.ALGAE_HIGH), - new GoToCmdDirect(manipulator_, manipulator_.getElevatorPosition(), - ManipulatorConstants.Arm.Positions.kAlgaeReefHold), - new GentleLowerElevator(manipulator_, ManipulatorConstants.Elevator.Positions.kAlgaeReefHold)) ; - } - else { - postseq = Commands.sequence( - RobotContainer.getInstance().gamepad().setLockCommand(false), - new SetHoldingCmd(brain_, GamePiece.ALGAE_HIGH)) ; - } - - if (eject_) { - seq.addCommands(new RunGrabberVoltsCmd(grabber_, Milliseconds.of(500))) ; - } - else { - seq.addCommands( - new ConditionalCommand( - postseq, - Commands.sequence( - RobotContainer.getInstance().gamepad().setLockCommand(false), - grabber_.setVoltageCommand(Volts.zero()), - new GoToCmdDirect(manipulator_, height, angle) - ), - this::hasAlgae)) ; - } - seq.addCommands(RobotContainer.getInstance().gamepad().setLockCommand(false)) ; - } - - private boolean hasAlgae() { - return grabber_.hasAlgae() ; - } -} diff --git a/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java b/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java index 47f17a1..9e29fdc 100755 --- a/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java +++ b/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java @@ -1,33 +1,27 @@ package frc.robot.commands.robot.collectalgaereef; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; import static edu.wpi.first.units.Units.Milliseconds; import static edu.wpi.first.units.Units.Volts; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Meters; import java.util.Optional; -import org.littletonrobotics.junction.Logger; -import org.xerosw.math.XeroMath; import org.xerosw.util.XeroSequenceCmd; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.RobotContainer; import frc.robot.Constants.ReefLevel; +import frc.robot.RobotContainer; import frc.robot.commands.drive.DriveCommands; import frc.robot.commands.robot.CommandConstants; - import frc.robot.subsystems.brain.BrainSubsystem; import frc.robot.subsystems.brain.GamePiece; import frc.robot.subsystems.brain.SetHoldingCmd; @@ -36,7 +30,6 @@ import frc.robot.subsystems.grabber.commands.RunGrabberVoltsCmd; import frc.robot.subsystems.manipulator.ManipulatorConstants; import frc.robot.subsystems.manipulator.ManipulatorSubsystem; -import frc.robot.subsystems.manipulator.commands.GoToCmd; import frc.robot.subsystems.manipulator.commands.GoToCmdDirect; import frc.robot.util.ReefFaceInfo; import frc.robot.util.ReefUtil; @@ -48,17 +41,17 @@ public class CollectAlgaeReefScoopCmd extends XeroSequenceCmd { private GrabberSubsystem grabber_; private ReefLevel height_ ; private Drive db_ ; - private boolean eject_ ; + private boolean collect_ ; private boolean quick_ ; - public CollectAlgaeReefScoopCmd(BrainSubsystem brain, Drive db, ManipulatorSubsystem manipulator, GrabberSubsystem grabber, ReefLevel height, boolean eject, boolean quick) { + public CollectAlgaeReefScoopCmd(BrainSubsystem brain, Drive db, ManipulatorSubsystem manipulator, GrabberSubsystem grabber, ReefLevel height, boolean collect, boolean quick) { super("CollectAlgaeReefScoopCmd") ; brain_ = brain ; db_ = db ; manipulator_ = manipulator; grabber_ = grabber; height_ = height ; - eject_ = eject ; + collect_ = collect ; quick_ = quick ; } @@ -92,44 +85,16 @@ public void initSequence(SequentialCommandGroup seq) { if (reefFace.isEmpty()) return ; - Alliance a = DriverStation.getAlliance().get() ; - // // This is about the actual collect operation // - Pose2d bup = reefFace.get().getAlgaeBackupPose() ; - Pose2d buprot ; - - if (eject_) { - Rotation2d rot ; - - if (a == Alliance.Red) { - if (XeroMath.normalizeAngleDegrees(bup.getRotation().getDegrees() - 180.0) < 5.0) { - rot = Rotation2d.fromDegrees(90.0) ; - } - else { - rot = Rotation2d.fromDegrees(180.0) ; - } - } - else { - if (XeroMath.normalizeAngleDegrees(bup.getRotation().getDegrees()) < 5.0) { - rot = Rotation2d.fromDegrees(90.0) ; - } - else { - rot = Rotation2d.fromDegrees(0.0) ; - } - } - buprot = new Pose2d(bup.getTranslation(), rot) ; - } - else { - buprot = bup ; - } + Pose2d backupPose = reefFace.get().getAlgaeBackupPose() ; seq.addCommands( new GoToCmdDirect(manipulator_, height, angle), RobotContainer.getInstance().gamepad().setLockCommand(true), - //grabber_.setVoltageCommand(Volts.of(-6.0)), - DriveCommands.simplePathCommand(db_, reefFace.get().getAlgaeCollectPose(), bup, + collect_ ? grabber_.setVoltageCommand(Volts.of(-6.0)) : Commands.none(), + DriveCommands.simplePathCommand(db_, reefFace.get().getAlgaeCollectPose(), backupPose, MetersPerSecond.of(2.0), CommandConstants.ReefDrive.kMaxDriveAcceleration), new GoToCmdDirect( @@ -140,7 +105,7 @@ public void initSequence(SequentialCommandGroup seq) { Commands.waitTime(Milliseconds.of(300)) .finallyDo(() -> System.out.println("Removal Deadline Hit")) ), - DriveCommands.simplePathCommand(db_, buprot, bup, + DriveCommands.simplePathCommand(db_, backupPose, backupPose, MetersPerSecond.of(3.0), MetersPerSecondPerSecond.of(3.0))); @@ -159,20 +124,16 @@ public void initSequence(SequentialCommandGroup seq) { RobotContainer.getInstance().gamepad().setLockCommand(false)) ; } - if (eject_) { - seq.addCommands(new RunGrabberVoltsCmd(grabber_, Milliseconds.of(500))) ; - } - else { - seq.addCommands( - new ConditionalCommand( - postseq, - Commands.sequence( - RobotContainer.getInstance().gamepad().setLockCommand(false), - grabber_.setVoltageCommand(Volts.zero()), - new GoToCmdDirect(manipulator_, height, angle) - ), - this::hasAlgae)) ; - } + + seq.addCommands( + new ConditionalCommand( + postseq, + Commands.sequence( + RobotContainer.getInstance().gamepad().setLockCommand(false), + grabber_.setVoltageCommand(Volts.zero()), + new GoToCmdDirect(manipulator_, height, angle) + ), + this::hasAlgae)) ; seq.addCommands(RobotContainer.getInstance().gamepad().setLockCommand(false)) ; } diff --git a/src/main/java/frc/robot/subsystems/brain/BrainSubsystem.java b/src/main/java/frc/robot/subsystems/brain/BrainSubsystem.java index d6ea411..5ae6e47 100644 --- a/src/main/java/frc/robot/subsystems/brain/BrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/brain/BrainSubsystem.java @@ -13,27 +13,26 @@ import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ReefLevel; +import frc.robot.RobotContainer; +import frc.robot.commands.robot.NullCmd ; +import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefGotoCmd; +import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefScoopCmd; +import frc.robot.commands.robot.collectcoral.CollectCoralCmd; +import frc.robot.commands.robot.placecoral.PlaceCoralCmd; +import frc.robot.commands.robot.scorealgae.ScoreAlgaeAfter; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.funnel.FunnelSubsystem; import frc.robot.subsystems.grabber.GrabberSubsystem; -import frc.robot.subsystems.manipulator.ManipulatorSubsystem; import frc.robot.subsystems.manipulator.ManipulatorConstants; +import frc.robot.subsystems.manipulator.ManipulatorSubsystem; import frc.robot.subsystems.oi.CoralSide; import frc.robot.subsystems.oi.OIConstants.LEDState; import frc.robot.subsystems.oi.OIConstants.OILed; import frc.robot.subsystems.oi.OISubsystem; import frc.robot.util.ReefFaceInfo; import frc.robot.util.ReefUtil; -import frc.robot.commands.robot.NullCmd ; -import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefCmd; -import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefScoopCmd; -import frc.robot.commands.robot.collectalgaereef.CollectAlgaeReefGotoCmd; -import frc.robot.commands.robot.collectcoral.CollectCoralCmd; -import frc.robot.commands.robot.placecoral.PlaceCoralCmd; -import frc.robot.commands.robot.scorealgae.ScoreAlgaeAfter; -import frc.robot.Constants.ReefLevel; -import frc.robot.RobotContainer; public class BrainSubsystem extends SubsystemBase { // The currently executing action, can be null if nothing is being executed @@ -566,7 +565,7 @@ private RobotActionCommandList getRobotActionCommand(RobotAction action, ReefLev list.add(new CollectAlgaeReefGotoCmd(this, m_, ReefLevel.AskBrain)) ; conds.add(null) ; - list.add(new CollectAlgaeReefCmd(this, db_, m_, g_, ReefLevel.AskBrain, false, false)) ; + list.add(new CollectAlgaeReefScoopCmd(this, db_, m_, g_, ReefLevel.AskBrain, true, false)) ; conds.add(() -> { return ReefUtil.getTargetedReefFace(db_.getPose()).isPresent() ; }) ; break ; } diff --git a/src/main/java/frc/robot/subsystems/grabber/commands/CollectAlgaeCmd.java b/src/main/java/frc/robot/subsystems/grabber/commands/CollectAlgaeCmd.java deleted file mode 100644 index 4613019..0000000 --- a/src/main/java/frc/robot/subsystems/grabber/commands/CollectAlgaeCmd.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc.robot.subsystems.grabber.commands; - -import static edu.wpi.first.units.Units.Milliseconds; -import static edu.wpi.first.units.Units.Volts; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.subsystems.grabber.GrabberConstants; -import frc.robot.subsystems.grabber.GrabberSubsystem; - -public class CollectAlgaeCmd extends SequentialCommandGroup { - - public CollectAlgaeCmd(GrabberSubsystem grabber) { - addCommands( - grabber.setVoltageCommand(Volts.of(-6.0)), - logState("Waiting"), - Commands.waitUntil(this::hasAlgae), - logState("Grabbed"), - new WaitCommand(Milliseconds.of(500)), - grabber.setVoltageCommand(Volts.of(GrabberConstants.Grabber.kHoldingVoltage)) - ); - } - - private boolean hasAlgae() { - return false ; - } - - private Command logState(String state) { - return Commands.runOnce(() -> { - Logger.recordOutput("Commands/AlgaeCollect/State", state); - }); - } -} diff --git a/src/main/java/frc/robot/subsystems/grabber/commands/CollectAlgaeNewCmd.java b/src/main/java/frc/robot/subsystems/grabber/commands/CollectAlgaeNewCmd.java deleted file mode 100644 index 579d726..0000000 --- a/src/main/java/frc/robot/subsystems/grabber/commands/CollectAlgaeNewCmd.java +++ /dev/null @@ -1,35 +0,0 @@ -package frc.robot.subsystems.grabber.commands; - -import static edu.wpi.first.units.Units.Milliseconds; -import static edu.wpi.first.units.Units.Volts; - -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants.ReefLevel; -import frc.robot.subsystems.grabber.GrabberConstants; -import frc.robot.subsystems.grabber.GrabberSubsystem; -import frc.robot.subsystems.manipulator.ManipulatorConstants; -import frc.robot.subsystems.manipulator.ManipulatorSubsystem; -import frc.robot.subsystems.manipulator.commands.GoToCmdDirect; - -public class CollectAlgaeNewCmd extends SequentialCommandGroup { - - public CollectAlgaeNewCmd(GrabberSubsystem grabber, ManipulatorSubsystem manip, ReefLevel level) { - Distance height = (level == ReefLevel.L1 || level == ReefLevel.L2) ? - ManipulatorConstants.Elevator.Positions.kAlgaeReefCollectNewPos2L2 : - ManipulatorConstants.Elevator.Positions.kAlgaeReefCollectNewPos2L3 ; - - Angle angle = (level == ReefLevel.L1 || level == ReefLevel.L2) ? - ManipulatorConstants.Arm.Positions.kAlgaeReefCollectNewPos2L2 : - ManipulatorConstants.Arm.Positions.kAlgaeReefCollectNewPos2L3 ; - - addCommands( - grabber.setVoltageCommand(Volts.of(-6.0)), - new GoToCmdDirect(manip, height, angle), - new WaitCommand(Milliseconds.of(250)), - grabber.setVoltageCommand(Volts.of(GrabberConstants.Grabber.kHoldingVoltage)) - ); - } -} From 365c02646f5f701883b118862afc10545dc0db3f Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 15 Oct 2025 11:25:17 -0700 Subject: [PATCH 4/4] Fix Brain Behavior --- .../collectalgaereef/CollectAlgaeReefScoopCmd.java | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java b/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java index 9e29fdc..f250822 100755 --- a/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java +++ b/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java @@ -27,7 +27,6 @@ import frc.robot.subsystems.brain.SetHoldingCmd; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.grabber.GrabberSubsystem; -import frc.robot.subsystems.grabber.commands.RunGrabberVoltsCmd; import frc.robot.subsystems.manipulator.ManipulatorConstants; import frc.robot.subsystems.manipulator.ManipulatorSubsystem; import frc.robot.subsystems.manipulator.commands.GoToCmdDirect; @@ -120,8 +119,8 @@ public void initSequence(SequentialCommandGroup seq) { new GentleLowerElevator(manipulator_, ManipulatorConstants.Elevator.Positions.kAlgaeReefHold)) ; } else { - postseq = Commands.sequence( - RobotContainer.getInstance().gamepad().setLockCommand(false)) ; + postseq = RobotContainer.getInstance().gamepad().setLockCommand(false) + .alongWith(collect_ ? new SetHoldingCmd(brain_, GamePiece.ALGAE_HIGH) : Commands.none()); } @@ -133,11 +132,7 @@ public void initSequence(SequentialCommandGroup seq) { grabber_.setVoltageCommand(Volts.zero()), new GoToCmdDirect(manipulator_, height, angle) ), - this::hasAlgae)) ; + grabber_::hasAlgae)); seq.addCommands(RobotContainer.getInstance().gamepad().setLockCommand(false)) ; } - - private boolean hasAlgae() { - return grabber_.hasAlgae() ; - } }