make separate cone and cube setpoints

src/main/java/frc/robot/Constants.java

@@ -208,9 +208,18 @@ public static final class Setpoints {
           new ElevatorState(MIN_EXTENSION_INCHES, 75);
       public static final ElevatorState GROUND_INTAKE_CUBE =
           new ElevatorState(MIN_EXTENSION_INCHES, 85);
-      public static final ElevatorState SCORE_HIGH = new ElevatorState(MAX_EXTENSION_INCHES, 20);
-      public static final ElevatorState SCORE_MID = new ElevatorState(MAX_EXTENSION_INCHES / 2, 40);
-      public static final ElevatorState SCORE_LOW = new ElevatorState(MIN_EXTENSION_INCHES, 60);
+      public static final ElevatorState SCORE_HIGH_CONE =
+          new ElevatorState(MAX_EXTENSION_INCHES, 20);
+      public static final ElevatorState SCORE_HIGH_CUBE =
+          new ElevatorState(MAX_EXTENSION_INCHES, 20);
+      public static final ElevatorState SCORE_MID_CONE =
+          new ElevatorState(MAX_EXTENSION_INCHES / 2, 40);
+      public static final ElevatorState SCORE_MID_CUBE =
+          new ElevatorState(MAX_EXTENSION_INCHES / 2, 40);
+      public static final ElevatorState SCORE_LOW_CONE =
+          new ElevatorState(MIN_EXTENSION_INCHES, 60);
+      public static final ElevatorState SCORE_LOW_CUBE =
+          new ElevatorState(MIN_EXTENSION_INCHES, 60);
     }
 
     public static final double MAX_EXTENSION_INCHES = 54;
@@ -261,27 +270,27 @@ public static final class Ports {
       Map.of(
           NodeType.CONE.atHeight(Height.HIGH),
           List.of(
-              new ScoreStep(Elevator.Setpoints.SCORE_HIGH).canWaitHere(),
-              new ScoreStep(Elevator.Setpoints.SCORE_HIGH, IntakeSubsystem.Modes.OUTTAKE)),
+              new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CONE).canWaitHere(),
+              new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CONE, IntakeSubsystem.Modes.OUTTAKE)),
           NodeType.CONE.atHeight(Height.MID),
           List.of(
-              new ScoreStep(Elevator.Setpoints.SCORE_MID).canWaitHere(),
-              new ScoreStep(Elevator.Setpoints.SCORE_MID, IntakeSubsystem.Modes.OUTTAKE)),
+              new ScoreStep(Elevator.Setpoints.SCORE_MID_CONE).canWaitHere(),
+              new ScoreStep(Elevator.Setpoints.SCORE_MID_CONE, IntakeSubsystem.Modes.OUTTAKE)),
           NodeType.CONE.atHeight(Height.LOW),
           List.of(
-              new ScoreStep(Elevator.Setpoints.SCORE_LOW).canWaitHere(),
+              new ScoreStep(Elevator.Setpoints.SCORE_LOW_CONE).canWaitHere(),
               new ScoreStep(IntakeSubsystem.Modes.OUTTAKE)),
           NodeType.CUBE.atHeight(Height.HIGH),
           List.of(
-              new ScoreStep(Elevator.Setpoints.SCORE_HIGH).canWaitHere(),
-              new ScoreStep(Elevator.Setpoints.SCORE_HIGH, IntakeSubsystem.Modes.OUTTAKE)),
+              new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CUBE).canWaitHere(),
+              new ScoreStep(Elevator.Setpoints.SCORE_HIGH_CUBE, IntakeSubsystem.Modes.OUTTAKE)),
           NodeType.CUBE.atHeight(Height.MID),
           List.of(
-              new ScoreStep(Elevator.Setpoints.SCORE_MID).canWaitHere(),
-              new ScoreStep(Elevator.Setpoints.SCORE_MID, IntakeSubsystem.Modes.OUTTAKE)),
+              new ScoreStep(Elevator.Setpoints.SCORE_MID_CUBE).canWaitHere(),
+              new ScoreStep(Elevator.Setpoints.SCORE_MID_CUBE, IntakeSubsystem.Modes.OUTTAKE)),
           NodeType.CUBE.atHeight(Height.LOW),
           List.of(
-              new ScoreStep(Elevator.Setpoints.SCORE_LOW).canWaitHere(),
+              new ScoreStep(Elevator.Setpoints.SCORE_LOW_CUBE).canWaitHere(),
               new ScoreStep(IntakeSubsystem.Modes.OUTTAKE)));
 
   public static final class Vision {

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

@@ -124,7 +124,7 @@ public RobotContainer() {
             drivebaseSubsystem,
             translationXSupplier,
             translationYSupplier,
-            anthony.rightBumper(),
+            // anthony.rightBumper(),
             anthony.leftBumper()));
 
     // // FIXME: This error is here to kind of guide you...
@@ -304,8 +304,11 @@ private void configureButtonBindings() {
         .povUp()
         .onTrue(
             new ElevatorPositionCommand(
-                elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))
-        .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, anthony.rightStick()));
+                elevatorSubsystem,
+                anthony.rightBumper().getAsBoolean()
+                    ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE
+                    : Constants.Elevator.Setpoints.GROUND_INTAKE_CONE))
+        .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, anthony.rightBumper()));
 
     // reset
     jacobLayer
@@ -323,21 +326,11 @@ private void configureButtonBindings() {
 
     anthony
         .povDown()
-        .onTrue(
-            new GroundPickupCommand(
-                intakeSubsystem,
-                elevatorSubsystem,
-                () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE,
-                jacob.rightStick()));
+        .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.rightStick()));
 
     jacob
         .a()
-        .onTrue(
-            new GroundPickupCommand(
-                intakeSubsystem,
-                elevatorSubsystem,
-                () -> jacob.getHID().getPOV() == 180 ? Modes.INTAKE : Modes.INTAKE,
-                jacob.rightStick()));
+        .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.rightStick()));
 
     jacobLayer
         .off(jacob.povUp())
@@ -355,7 +348,10 @@ private void configureButtonBindings() {
             new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, jacob.rightStick())
                 .alongWith(
                     new ElevatorPositionCommand(
-                        elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE)))
+                        elevatorSubsystem,
+                        jacob.rightStick().getAsBoolean()
+                            ? Constants.Elevator.Setpoints.SHELF_INTAKE_CUBE
+                            : Constants.Elevator.Setpoints.SHELF_INTAKE_CONE)))
         .onFalse(
             new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)
                 .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF)));

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

@@ -23,21 +23,21 @@ public class DefaultDriveCommand extends CommandBase {
   private final DoubleSupplier translationYSupplier;
 
   private final BooleanSupplier isRobotRelativeForwardSupplier;
-  private final BooleanSupplier isRobotRelativeBackwardSupplier;
+  // private final BooleanSupplier isRobotRelativeBackwardSupplier;
 
   /** Creates a new DefaultDriveCommand. */
   public DefaultDriveCommand(
       DrivebaseSubsystem drivebaseSubsystem,
       DoubleSupplier translationXSupplier,
       DoubleSupplier translationYSupplier,
-      BooleanSupplier isRobotRelativeForwardSupplier,
-      BooleanSupplier isRobotRelativeBackwardSupplier) {
+      BooleanSupplier isRobotRelativeForwardSupplier) {
+    // BooleanSupplier isRobotRelativeBackwardSupplier) {
 
     this.drivebaseSubsystem = drivebaseSubsystem;
     this.translationXSupplier = translationXSupplier;
     this.translationYSupplier = translationYSupplier;
     this.isRobotRelativeForwardSupplier = isRobotRelativeForwardSupplier;
-    this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier;
+    // this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier;
 
     addRequirements(drivebaseSubsystem);
   }
@@ -48,12 +48,12 @@ public void execute() {
     double x = translationXSupplier.getAsDouble();
     double y = translationYSupplier.getAsDouble();
     Boolean forwardRelative = isRobotRelativeForwardSupplier.getAsBoolean();
-    Boolean backwardRelative = isRobotRelativeBackwardSupplier.getAsBoolean();
+    // Boolean backwardRelative = isRobotRelativeBackwardSupplier.getAsBoolean();
 
     drivebaseSubsystem.drive(
         DrivebaseSubsystem.produceChassisSpeeds(
             forwardRelative,
-            backwardRelative,
+            // backwardRelative,
             x,
             y,
             0,

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

@@ -11,7 +11,6 @@
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.Modes;
 import java.util.function.BooleanSupplier;
-import java.util.function.Supplier;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
@@ -25,10 +24,8 @@ public class GroundPickupCommand extends SequentialCommandGroup {
   public GroundPickupCommand(
       IntakeSubsystem intakeSubsystem,
       ElevatorSubsystem elevatorSubsystem,
-      Supplier<Modes> modeSupplier,
       BooleanSupplier isCube) {
     // Add your commands in the addCommands() call, e.g.
-    // addCommands(new FooCommand(), new BarCommand());
     this.isCube = isCube;
     this.elevatorSubsystem = elevatorSubsystem;
 

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

@@ -22,23 +22,23 @@ public class RotateVelocityDriveCommand extends CommandBase {
   private final DoubleSupplier rotationSupplier;
 
   private final BooleanSupplier isRobotRelativeForwardSupplier;
-  private final BooleanSupplier isRobotRelativeBackwardSupplier;
+  // private final BooleanSupplier isRobotRelativeBackwardSupplier;
 
   /** Creates a new RotateVelocityDriveCommand. */
   public RotateVelocityDriveCommand(
       DrivebaseSubsystem drivebaseSubsystem,
       DoubleSupplier translationXSupplier,
       DoubleSupplier translationYSupplier,
       DoubleSupplier rotationSupplier,
-      BooleanSupplier isRobotRelativeForwardSupplier,
-      BooleanSupplier isRobotRelativeBackwardSupplier) {
+      BooleanSupplier isRobotRelativeForwardSupplier) {
+    // BooleanSupplier isRobotRelativeBackwardSupplier) {
 
     this.drivebaseSubsystem = drivebaseSubsystem;
     this.translationXSupplier = translationXSupplier;
     this.translationYSupplier = translationYSupplier;
     this.rotationSupplier = rotationSupplier;
     this.isRobotRelativeForwardSupplier = isRobotRelativeForwardSupplier;
-    this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier;
+    // this.isRobotRelativeBackwardSupplier = isRobotRelativeBackwardSupplier;
 
     addRequirements(drivebaseSubsystem);
   }
@@ -59,7 +59,7 @@ public void execute() {
     drivebaseSubsystem.drive(
         DrivebaseSubsystem.produceChassisSpeeds(
             isRobotRelativeForwardSupplier.getAsBoolean(),
-            isRobotRelativeBackwardSupplier.getAsBoolean(),
+            // isRobotRelativeBackwardSupplier.getAsBoolean(),
             x,
             y,
             rot,

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

@@ -557,13 +557,13 @@ public void periodic() {
 
   public static ChassisSpeeds produceChassisSpeeds(
       boolean isRobotRelativeForward,
-      boolean isRobotRelativeBackward,
+      // boolean isRobotRelativeBackward,
       double x,
       double y,
       double rotationVelocity,
       Rotation2d currentGyroAngle) {
     if (isRobotRelativeForward) return new ChassisSpeeds(x, y, rotationVelocity);
-    if (isRobotRelativeBackward) return new ChassisSpeeds(-x, -y, rotationVelocity);
+    // if (isRobotRelativeBackward) return new ChassisSpeeds(-x, -y, rotationVelocity);
     return ChassisSpeeds.fromFieldRelativeSpeeds(x, y, rotationVelocity, currentGyroAngle);
   }
 }