Piece selector fix

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

@@ -28,7 +28,6 @@
 import frc.robot.commands.Presets.StowPresetSequence;
 import frc.robot.commands.Presets.SubstationPickupPresetSequence;
 import frc.robot.subsystems.TelescopeSubsystem;
-import frc.robot.subsystems.CrocodileSubsystem.GamePiece;
 import frc.robot.subsystems.CrocodileSubsystem.WristPosition;
 import frc.robot.subsystems.Swerve.DrivebaseSubsystem;
 
@@ -139,23 +138,22 @@ private void configureButtonBindings() {
 
     zeroGyro.onTrue(new InstantCommand(() -> swerve.zeroGyro()));
 
-    mechController.a().onTrue(new PickupPresetSequence(telescope, rotator, endEffector, driverController.y(), () -> (endEffector.getGamePiece() == GamePiece.CONE ? true : false)));
-    mechController.y().onTrue(new StowPresetSequence(telescope, rotator, endEffector));
+    mechController.a().onTrue(new PickupPresetSequence(telescope, rotator, endEffector, driverController.y(), () -> endEffector.getGamePiece()));
+    mechController.y().onTrue(new StowPresetSequence(telescope, rotator, endEffector, () -> endEffector.getGamePiece()));
     mechController.x().onTrue(new HighPresetSequence(telescope, rotator, endEffector,
-        () -> (driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis())));
+        () -> (driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis()), () -> endEffector.getGamePiece()));
     mechController.b().onTrue(new MidPresetSequence(telescope, rotator, endEffector));
     mechController.povLeft().onTrue(new InstantCommand(() -> telescope.resetEncoder()));
     mechController.button(8).onTrue(new InstantCommand(() -> {
-      endEffector.setGamePiece(GamePiece.CONE);
+      endEffector.setGamePiece(true);
       candle.cone();
     }, endEffector));
     mechController.button(7).onTrue(new InstantCommand(() -> {
-      endEffector.setGamePiece(GamePiece.CUBE);
+      endEffector.setGamePiece(false);
       candle.cube();
     }, endEffector));
     mechController.povDown()
-        .onTrue(new SubstationPickupPresetSequence(telescope, rotator, endEffector, driverController.y()));
-    driverController.y().onTrue(new AutoIntakeCommand(endEffector, 0.5, driverController.y()));
+        .onTrue(new SubstationPickupPresetSequence(telescope, rotator, endEffector, driverController.y(), () -> endEffector.getGamePiece()));
   }
 
   public void teleopInit() {

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

@@ -6,46 +6,37 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.CrocodileSubsystem;
 import java.util.function.BooleanSupplier;
-import frc.robot.subsystems.CrocodileSubsystem.GamePiece;
 
 public class AutoIntakeCommand extends CommandBase {
     private final CrocodileSubsystem crocodileSubsystem;
     private final double speed;
     private final Timer timer;
     private final Debouncer debouncer;
     private final BooleanSupplier stopIntake;
-    private final GamePiece gamePiece;
+    private final BooleanSupplier gamePiece;
 
     private boolean wasCancelled = false;
-
+    //set stopIntake to null if you don't want to use it
     public AutoIntakeCommand(CrocodileSubsystem crocodileSubsystem, double speed,
-            BooleanSupplier stopIntake, GamePiece gamePiece) {
+            BooleanSupplier stopIntake, BooleanSupplier gamePiece) {
         this.crocodileSubsystem = crocodileSubsystem;
         this.speed = speed;
         debouncer = new Debouncer(0.5, DebounceType.kFalling);
         timer = new Timer();
         this.stopIntake = stopIntake;
-        crocodileSubsystem.setGamePiece(gamePiece);
-        this.gamePiece = crocodileSubsystem.getGamePiece();
+        this.gamePiece = gamePiece;
         addRequirements(crocodileSubsystem);
     }
-
-    // overload autointke and set stopIntake to null, use for auto
-    public AutoIntakeCommand(CrocodileSubsystem crocodileSubsystem, double speed,
-            GamePiece gamePiece) {
-        this(crocodileSubsystem, speed, null, gamePiece);
+    public AutoIntakeCommand(CrocodileSubsystem crocodileSubsystem, double speed, boolean gamePiece) {
+        this(crocodileSubsystem, speed, null, () -> gamePiece);
     }
 
-    // overload autointake and set gamepiece to getGamePiece
-    public AutoIntakeCommand(CrocodileSubsystem crocodileSubsystem,  double speed,
-            BooleanSupplier stopIntake) {
-        this(crocodileSubsystem, speed, stopIntake, crocodileSubsystem.getGamePiece());
-    }
+
 
     @Override
     public void initialize() {
         // TODO: check if this is the right negation
-        if (gamePiece == GamePiece.CONE) {
+        if (gamePiece.getAsBoolean()) {
             crocodileSubsystem.setIntakeSpeed(speed);
         } else {
             crocodileSubsystem.setIntakeSpeed(-speed);
@@ -57,7 +48,7 @@ public boolean isFinished() {
         // if outtaking, keep running motors until beam break hasn't been broken for 0.5
         // seconds
 
-        if ((speed < 0 && gamePiece == GamePiece.CONE) || (speed > 0 && gamePiece == GamePiece.CUBE)) {
+        if ((speed < 0 && gamePiece.getAsBoolean()) || (speed > 0 && !gamePiece.getAsBoolean())) {
             if (crocodileSubsystem.getBeamBreak()) {
                 timer.start();
             }

src/main/java/frc/robot/commands/Autos/PreloadParkCenter.java

@@ -42,8 +42,8 @@ public PreloadParkCenter(DrivebaseSubsystem driveBase, CrocodileSubsystem croc,
     this.defaultConfig = new TrajectoryConfig(SharedConstants.AutoConstants.k_MAX_SPEED_MPS,
         SharedConstants.AutoConstants.k_MAX_ACCEL_MPS_SQUARED);
     addCommands(
-        new HighPresetSequence(telescope, rotator, croc, null),
-        new HighPresetSequence(telescope, rotator, croc, null),
+        new HighPresetSequence(telescope, rotator, croc, null, () -> true),
+        new HighPresetSequence(telescope, rotator, croc, null, () -> true),
         new InstantCommand(()->croc.setIntakeSpeed(-1)),
         new WaitCommand(0.25),
         new InstantCommand(()->croc.setIntakeSpeed(0)),

src/main/java/frc/robot/commands/Autos/PreloadPlusOneLeft.java

@@ -42,17 +42,17 @@ public PreloadPlusOneLeft(DrivebaseSubsystem driveBase, CrocodileSubsystem croc,
     this.defaultConfig = new TrajectoryConfig(SharedConstants.AutoConstants.k_MAX_SPEED_MPS,
         SharedConstants.AutoConstants.k_MAX_ACCEL_MPS_SQUARED);
     addCommands(
-        new HighPresetSequence(telescope, rotator, croc, null),
+        new HighPresetSequence(telescope, rotator, croc, null, () -> true),
         new InstantCommand(()->croc.setIntakeSpeed(-1)),
         new WaitCommand(0.25),
         new InstantCommand(()->croc.setIntakeSpeed(0)),
         new ParallelCommandGroup(
-          new PickupPresetSequence(telescope, rotator, croc, null).withTimeout(3),
+          new PickupPresetSequence(telescope, rotator, croc, null, () -> true).withTimeout(3),
           new SwerveTrajectoryFollowCommand(driveBase, "preloadPlusOneLeft1", defaultConfig, true)
         ),
         new StowPresetSequence(telescope, rotator, croc),
         new SwerveTrajectoryFollowCommand(driveBase, "preloadPlusOneLeft2", defaultConfig),
-        new HighPresetSequence(telescope, rotator, croc, null),
+        new HighPresetSequence(telescope, rotator, croc, null, () -> true),
         new InstantCommand(()->croc.setIntakeSpeed(-1)),
         new WaitCommand(0.25),
         new StowPresetSequence(telescope, rotator, croc)

src/main/java/frc/robot/commands/Autos/PreloadPlusOneRight.java

@@ -42,17 +42,17 @@ public PreloadPlusOneRight(DrivebaseSubsystem driveBase, CrocodileSubsystem croc
     this.defaultConfig = new TrajectoryConfig(SharedConstants.AutoConstants.k_MAX_SPEED_MPS,
         SharedConstants.AutoConstants.k_MAX_ACCEL_MPS_SQUARED);
     addCommands(
-        new HighPresetSequence(telescope, rotator, croc, null),
+        new HighPresetSequence(telescope, rotator, croc, null, () -> true),
         new InstantCommand(()->croc.setIntakeSpeed(-1)),
         new WaitCommand(0.25),
         new InstantCommand(()->croc.setIntakeSpeed(0)),
         new ParallelCommandGroup(
-          new PickupPresetSequence(telescope, rotator, croc, null).withTimeout(3),
+          new PickupPresetSequence(telescope, rotator, croc, null, () -> true).withTimeout(3),
           new SwerveTrajectoryFollowCommand(driveBase, "preloadPlusOneRight1", defaultConfig, true)
         ),
         new StowPresetSequence(telescope, rotator, croc),
         new SwerveTrajectoryFollowCommand(driveBase, "preloadPlusOneRight2", defaultConfig),
-        new HighPresetSequence(telescope, rotator, croc, null),
+        new HighPresetSequence(telescope, rotator, croc, null, () -> true),
         new InstantCommand(()->croc.setIntakeSpeed(-1)),
         new WaitCommand(0.25),
         new StowPresetSequence(telescope, rotator, croc)

src/main/java/frc/robot/commands/DefaultCommands/CrocodileDefaultCommand.java

@@ -9,7 +9,6 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.CrocodileSubsystem;
-import frc.robot.subsystems.CrocodileSubsystem.GamePiece;
 
 public class CrocodileDefaultCommand extends CommandBase {
     private final CrocodileSubsystem subsystem;
@@ -47,7 +46,7 @@ public void execute() {
             subsystem.setRunPID(true);
         }        
         //TODO: check if this is the right negation
-        if(subsystem.getGamePiece() == CrocodileSubsystem.GamePiece.CONE){
+        if(subsystem.getGamePiece()){
             subsystem.setIntakeSpeed(trigger.getAsDouble());
         }
         else{

src/main/java/frc/robot/commands/Presets/HighPresetSequence.java

@@ -1,28 +1,30 @@
 package frc.robot.commands.Presets;
 
+import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.RotatorToPosition;
 import frc.robot.commands.TelescopeToPosition;
 import frc.robot.subsystems.CrocodileSubsystem;
 import frc.robot.subsystems.RotatorSubsystem;
 import frc.robot.subsystems.TelescopeSubsystem;
-import frc.robot.subsystems.CrocodileSubsystem.GamePiece;
 import frc.robot.subsystems.CrocodileSubsystem.WristPosition;
 import frc.robot.subsystems.RotatorSubsystem.RotatorPosition;
 import frc.robot.subsystems.TelescopeSubsystem.TelescopePosition;
 
 public class HighPresetSequence extends SequentialCommandGroup {
     private DoubleSupplier intake;
     private double multiplier = 1; 
-    public HighPresetSequence(TelescopeSubsystem telescope, RotatorSubsystem rotator, CrocodileSubsystem crocodile, DoubleSupplier intake) {
+    public HighPresetSequence(TelescopeSubsystem telescope, RotatorSubsystem rotator, CrocodileSubsystem crocodile, DoubleSupplier intake, BooleanSupplier gamePiece) {
         this.intake = intake;
         addRequirements(rotator,telescope,crocodile);
 
-        if(crocodile.getGamePiece() == GamePiece.CUBE){
+        if(!gamePiece.getAsBoolean()){
             multiplier = -1;
         }
         addCommands(
@@ -32,5 +34,6 @@ public HighPresetSequence(TelescopeSubsystem telescope, RotatorSubsystem rotator
             crocodile.setWristToPositionCommand(WristPosition.CONE_SCORE).withTimeout(2)
 
         );
+        deadlineWith(new RunCommand(() -> crocodile.setIntakeSpeed(MathUtil.clamp((0.25 + intake.getAsDouble()), -1, 1) * multiplier)));
     }
 }

src/main/java/frc/robot/commands/Presets/PickupPresetSequence.java

@@ -2,15 +2,13 @@
 
 import java.util.function.BooleanSupplier;
 
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.AutoIntakeCommand;
 import frc.robot.commands.RotatorToPosition;
 import frc.robot.commands.TelescopeToPosition;
 import frc.robot.subsystems.CrocodileSubsystem;
 import frc.robot.subsystems.RotatorSubsystem;
 import frc.robot.subsystems.TelescopeSubsystem;
-import frc.robot.subsystems.CrocodileSubsystem.GamePiece;
 import frc.robot.subsystems.CrocodileSubsystem.WristPosition;
 import frc.robot.subsystems.RotatorSubsystem.RotatorPosition;
 import frc.robot.subsystems.TelescopeSubsystem.TelescopePosition;
@@ -28,15 +26,15 @@ public PickupPresetSequence(TelescopeSubsystem telescope, RotatorSubsystem rotat
         else {
             addCommands(crocodile.setWristToPositionCommand(WristPosition.CUBE_PICKUP).withTimeout(2));
         }
-        addCommands(new AutoIntakeCommand(crocodile, 1, button));
+        addCommands(new AutoIntakeCommand(crocodile, 1, button, gamePieice));
     }
-    public PickupPresetSequence(TelescopeSubsystem telescope, RotatorSubsystem rotator, CrocodileSubsystem crocodile, BooleanSupplier button) {
+    public PickupPresetSequence(TelescopeSubsystem telescope, RotatorSubsystem rotator, CrocodileSubsystem crocodile, BooleanSupplier button, boolean gamePieice) {
         addRequirements(telescope,rotator,crocodile);
         addCommands(
             new TelescopeToPosition(telescope, TelescopePosition.GROUND_PICKUP).withTimeout(2),
             new RotatorToPosition(rotator, telescope, RotatorPosition.GROUND_PICKUP).withTimeout(2)
         );
         addCommands(crocodile.setWristToPositionCommand(WristPosition.CONE_PICKUP).withTimeout(2));
-        addCommands(new AutoIntakeCommand(crocodile, 1, button));
+        addCommands(new AutoIntakeCommand(crocodile, 1, gamePieice));
     }
 }

src/main/java/frc/robot/commands/Presets/StowPresetSequence.java

@@ -3,18 +3,28 @@
 import java.util.function.BooleanSupplier;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.AutoIntakeCommand;
 import frc.robot.commands.RotatorToPosition;
 import frc.robot.commands.TelescopeToPosition;
 import frc.robot.subsystems.CrocodileSubsystem;
 import frc.robot.subsystems.RotatorSubsystem;
 import frc.robot.subsystems.TelescopeSubsystem;
-import frc.robot.subsystems.CrocodileSubsystem.GamePiece;
 import frc.robot.subsystems.CrocodileSubsystem.WristPosition;
 import frc.robot.subsystems.TelescopeSubsystem.TelescopePosition;
 
 public class StowPresetSequence extends SequentialCommandGroup {
+    public StowPresetSequence(TelescopeSubsystem telescope, RotatorSubsystem rotator, CrocodileSubsystem crocodile, BooleanSupplier gamePiece) {
+        addRequirements(telescope,rotator,crocodile);
+        double multiplier = gamePiece.getAsBoolean() == true ? 1 : -1;
+        addCommands(
+            new TelescopeToPosition(telescope, TelescopePosition.RETRACTED).withTimeout(2),
+            new RotatorToPosition(rotator, telescope, 90).withTimeout(2),
+            crocodile.setWristToPositionCommand(WristPosition.CONE_SCORE).withTimeout(2)
+        );
+        deadlineWith(new RunCommand(() -> crocodile.setIntakeSpeed(0.25 * multiplier)));
+    }
     public StowPresetSequence(TelescopeSubsystem telescope, RotatorSubsystem rotator, CrocodileSubsystem crocodile) {
         addRequirements(telescope,rotator,crocodile);
         addCommands(

src/main/java/frc/robot/commands/Presets/SubstationPickupPresetSequence.java

@@ -10,19 +10,18 @@
 import frc.robot.subsystems.CrocodileSubsystem;
 import frc.robot.subsystems.RotatorSubsystem;
 import frc.robot.subsystems.TelescopeSubsystem;
-import frc.robot.subsystems.CrocodileSubsystem.GamePiece;
 import frc.robot.subsystems.CrocodileSubsystem.WristPosition;
 import frc.robot.subsystems.RotatorSubsystem.RotatorPosition;
 import frc.robot.subsystems.TelescopeSubsystem.TelescopePosition;
 
 public class SubstationPickupPresetSequence extends SequentialCommandGroup {
     public SubstationPickupPresetSequence(TelescopeSubsystem telescope, RotatorSubsystem rotator,
-            CrocodileSubsystem crocodile, BooleanSupplier button) {
+            CrocodileSubsystem crocodile, BooleanSupplier button, BooleanSupplier gamePiece) {
         addRequirements(telescope, rotator, crocodile);
         addCommands(
                 new RotatorToPosition(rotator, telescope, RotatorPosition.SHELF_PICKUP).withTimeout(2),
                 new TelescopeToPosition(telescope, TelescopePosition.SHELF_PICKUP).withTimeout(2),
                 crocodile.setWristToPositionCommand(WristPosition.CONE_SHELF).withTimeout(2),
-                new AutoIntakeCommand(crocodile, 1, button));
+                new AutoIntakeCommand(crocodile, 1, button, gamePiece));
     }
 }

src/main/java/frc/robot/subsystems/CrocodileSubsystem.java

@@ -19,7 +19,7 @@ public class CrocodileSubsystem extends SubsystemBase {
     private final PIDController wristPID;
     private final DutyCycleEncoder wristEncoder;
     private final DigitalInput beamBreak;
-    private GamePiece gamePiece = GamePiece.CONE;
+    private boolean gamePiece = true;
     private final double WRIST_kP = 0.015;
     private final double WRIST_kI = 0.0;
     private final double WRIST_kD = 0.00075;
@@ -30,19 +30,6 @@ public class CrocodileSubsystem extends SubsystemBase {
 
     private boolean runPID = true;
 
-    public enum GamePiece {
-        CONE("CONE"), 
-        CUBE("CUBE");
-        private final String name;
-
-        private GamePiece(String name) {
-            this.name = name;
-        }
-        String getData(){
-            return name;
-        }
-    }
-
     public enum WristPosition {
         CONE_PICKUP(160),
         CUBE_PICKUP(115),
@@ -125,11 +112,11 @@ public boolean getBeamBreak() {
         return beamBreak.get();
     }
 
-    public GamePiece getGamePiece() {
+    public boolean getGamePiece() {
         return gamePiece;
     }
 
-    public void setGamePiece(GamePiece gamePiece) {
+    public void setGamePiece(boolean gamePiece) {
         this.gamePiece = gamePiece;
     }
 
@@ -141,7 +128,7 @@ public void setRunPID(boolean set) {
     public void periodic() {
         if (runPID) setWristSpeed(MathUtil.clamp(wristPID.calculate(getWristPosition()), -SPEED_LIMIT, SPEED_LIMIT));
         SmartDashboard.putBoolean("Beam Break Status", !getBeamBreak());
-        SmartDashboard.putString("Current Piece Selection", gamePiece.getData());
+        SmartDashboard.putString("Game Piece", getGamePiece() ? "Cone" : "Cube");
         // System.out.println(getBeamBreak() + " " + getWristPosition());
     }
 }