diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 831a3738..8a5c7125 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/auto/AutoCommands.java b/src/main/java/frc/robot/commands/auto/AutoCommands.java index e6014f80..eef09cbe 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/CollectAlgaeReefScoopCmd.java old mode 100644 new mode 100755 similarity index 59% rename from src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefCmd.java rename to src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java index 3e9acc2d..f250822d --- a/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefCmd.java +++ b/src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java @@ -1,5 +1,7 @@ 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; @@ -7,21 +9,17 @@ 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.RobotContainer; import frc.robot.commands.drive.DriveCommands; import frc.robot.commands.robot.CommandConstants; import frc.robot.subsystems.brain.BrainSubsystem; @@ -29,31 +27,30 @@ 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 { + +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 collect_ ; private boolean quick_ ; - public CollectAlgaeReefCmd(BrainSubsystem brain, Drive db, ManipulatorSubsystem manipulator, GrabberSubsystem grabber, ReefLevel height, boolean eject, boolean quick) { - super("CollectAlgaeReefCmd") ; + 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 ; } @@ -87,50 +84,29 @@ 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 CollectAlgaeNewCmd(grabber_, manipulator_, level), - DriveCommands.simplePathCommand(db_, buprot, bup, + 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_, backupPose, backupPose, MetersPerSecond.of(3.0), - MetersPerSecondPerSecond.of(3.0))) ; + MetersPerSecondPerSecond.of(3.0))); Command postseq ; @@ -143,29 +119,20 @@ public void initSequence(SequentialCommandGroup seq) { new GentleLowerElevator(manipulator_, ManipulatorConstants.Elevator.Positions.kAlgaeReefHold)) ; } else { - postseq = Commands.sequence( - RobotContainer.getInstance().gamepad().setLockCommand(false), - new SetHoldingCmd(brain_, GamePiece.ALGAE_HIGH)) ; + postseq = RobotContainer.getInstance().gamepad().setLockCommand(false) + .alongWith(collect_ ? new SetHoldingCmd(brain_, GamePiece.ALGAE_HIGH) : Commands.none()); } - 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) + ), + grabber_::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 3838e60e..5ae6e476 100644 --- a/src/main/java/frc/robot/subsystems/brain/BrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/brain/BrainSubsystem.java @@ -13,26 +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.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 @@ -556,7 +556,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 ; @@ -564,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 46130197..00000000 --- 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 579d7262..00000000 --- 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)) - ); - } -}