New Algae Removal Command

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.

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"));

src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefCmd.java → src/main/java/frc/robot/commands/robot/collectalgaereef/CollectAlgaeReefScoopCmd.java

@@ -1,59 +1,56 @@
 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 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;
 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 {
+
+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() ;
-    }
 }

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,15 +556,16 @@ 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 ;
 
             case CollectAlgaeReefKeep:
                 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 ;
         }

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) ;