Calibrated setpoints

Iron-Panthers · Nov 10, 2023 · bd079c1 · bd079c1
1 parent c57326f
commit bd079c1
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 15 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -207,19 +207,15 @@ public static final class Setpoints {
       public static final ElevatorState GROUND_INTAKE_CONE =
           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_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);
+          new ElevatorState(MIN_EXTENSION_INCHES, 90);
+      public static final ElevatorState SCORE_HIGH_CONE = new ElevatorState(43, 52.5);
+      public static final ElevatorState SCORE_HIGH_CUBE = new ElevatorState(53.8, 74.5);
+      public static final ElevatorState SCORE_MID_CONE = new ElevatorState(22.5, 35.5);
+      public static final ElevatorState SCORE_MID_CUBE = new ElevatorState(30, 74.5);
       public static final ElevatorState SCORE_LOW_CONE =
-          new ElevatorState(MIN_EXTENSION_INCHES, 60);
+          new ElevatorState(MIN_EXTENSION_INCHES, 55);
       public static final ElevatorState SCORE_LOW_CUBE =
-          new ElevatorState(MIN_EXTENSION_INCHES, 60);
+          new ElevatorState(MIN_EXTENSION_INCHES, 48);
     }
 
     public static final double MAX_EXTENSION_INCHES = 54;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -324,11 +324,27 @@ private void configureButtonBindings() {
 
     anthony
         .povDown()
-        .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, anthony.rightBumper()));
+        .onTrue(
+            new GroundPickupCommand(
+                intakeSubsystem,
+                elevatorSubsystem,
+                () ->
+                    anthony.rightBumper().getAsBoolean()
+                        ? Elevator.Setpoints.GROUND_INTAKE_CUBE
+                        : Elevator.Setpoints.GROUND_INTAKE_CONE,
+                anthony.rightBumper()));
 
     jacob
         .a()
-        .onTrue(new GroundPickupCommand(intakeSubsystem, elevatorSubsystem, jacob.leftBumper()));
+        .onTrue(
+            new GroundPickupCommand(
+                intakeSubsystem,
+                elevatorSubsystem,
+                () ->
+                    jacob.leftBumper().getAsBoolean()
+                        ? Elevator.Setpoints.GROUND_INTAKE_CUBE
+                        : Elevator.Setpoints.GROUND_INTAKE_CONE,
+                jacob.leftBumper()));
 
     jacobLayer
         .off(jacob.povUp())

diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.Constants.Elevator;
 import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorState;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.Modes;
 import java.util.function.BooleanSupplier;
@@ -21,16 +22,20 @@ public class GroundPickupCommand extends SequentialCommandGroup {
 
   private ElevatorSubsystem elevatorSubsystem;
 
+  private Supplier<ElevatorState> setpoint;
+
   public GroundPickupCommand(
       IntakeSubsystem intakeSubsystem,
       ElevatorSubsystem elevatorSubsystem,
+      Supplier<ElevatorState> setpoint,
       BooleanSupplier isCube) {
     // Add your commands in the addCommands() call, e.g.
     this.isCube = isCube;
     this.elevatorSubsystem = elevatorSubsystem;
+    this.setpoint = setpoint;
 
     addCommands(
-        determineIntakeType()
+        new ElevatorPositionCommand(elevatorSubsystem, setpoint.get())
             .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, isCube))
             .andThen(new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED)));
   }

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -52,7 +52,7 @@ public IntakeSubsystem() {
     shuffleboard.addString("Current mode", () -> currentIntakeMode.toString());
     shuffleboard.addDouble("filter output", () -> filterOutput);
     shuffleboard.addDouble("motor output", intakeMotor::getMotorOutputPercent);
-    shuffleboard.addBoolean("is cone", () -> isCube);
+    shuffleboard.addBoolean("is cube intake", () -> isCube);
     // shuffleboard.addDouble("coneToFInches", this::getConeToFInches);
     // shuffleboard.addDouble("cubeToFInches", this::getCubeToFInches);
   }