Changed ElevatorPositionCommand

Iron-Panthers · Sep 19, 2023 · 8b3f4f8 · 8b3f4f8
1 parent 386b1c1
commit 8b3f4f8
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 23 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -195,9 +195,9 @@ public static final class Ports {
     }
 
     public static final class Setpoints {
-      public static final double STOWED = 10.0;
-      public static final double HANDOFF = 5.0;
-      public static final double SHELF_INTAKE = 20.0;
+      public static final ElevatorState STOWED = ElevatorState(0, 0);
+      public static final ElevatorState HANDOFF = ElevatorState(10, 10);
+      public static final ElevatorState SHELF_INTAKE = ElevatorState(20, 20);
     }
 
     public static final double MAX_HEIGHT = 10;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -39,7 +39,6 @@
 import frc.robot.commands.ScoreCommand;
 import frc.robot.commands.ScoreCommand.ScoreStep;
 import frc.robot.commands.VibrateHIDCommand;
-import frc.robot.commands.WristManualCommand;
 import frc.robot.subsystems.CANWatchdogSubsystem;
 import frc.robot.subsystems.DrivebaseSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
@@ -185,10 +184,6 @@ public void containerMatchStarting() {
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
-    // FIXME the left and right triggers are already in use, either change or delete these button
-    // bindings
-    jason.leftBumper().onTrue(new WristManualCommand(elevatorSubsystem, 0));
-    jason.rightBumper().onTrue(new WristManualCommand(elevatorSubsystem, 20));
     jason
         .y()
         .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.MAX_HEIGHT, 0));
@@ -315,8 +310,7 @@ private void configureButtonBindings() {
         // FIXME: This error is here to kind of guide you...
         .onTrue(
             new ElevatorPositionCommand(
-                // FIXME make elevator position setpoints in constants using an ElevatorState record
-                elevatorSubsystem, 0, Constants.Elevator.Setpoints.SHELF_INTAKE));
+                elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE));
     //     .whileTrue(
     //         new ForceOuttakeSubsystemModeCommand(
     //             intakeSubsystem, IntakeSubsystem.IntakeMode.INTAKE));
@@ -415,7 +409,7 @@ private void setupAutonomousCommands() {
 
     final List<ScoreStep> drivingCubeOuttake =
         List.of(
-            new ScoreStep(new ElevatorState(35, Constants.Elevator.MIN_HEIGHT)).canWaitHere(),
+            new ScoreStep(ElevatorState(35, Constants.Elevator.MIN_HEIGHT)).canWaitHere(),
             new ScoreStep(IntakeMode.OUTTAKE));
     final boolean[] intakeLow = {false};
     final Map<String, Command> eventMap =
@@ -462,13 +456,13 @@ public void end(boolean interrupted) {
                     intakeSubsystem, elevatorSubsystem, drivingCubeOuttake.subList(1, 2), 1)
                 .andThen(
                     new ElevatorPositionCommand(
-                            elevatorSubsystem, 0, Constants.Elevator.Setpoints.STOWED)
+                            elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)
                         .andThen(new IntakeModeCommand(intakeSubsystem, IntakeMode.OFF))),
             "armbat preload",
             new ElevatorPositionCommand(elevatorSubsystem, 30, 0)
                 .andThen(
                     new ElevatorPositionCommand(
-                        elevatorSubsystem, 0, Constants.Elevator.Setpoints.STOWED)));
+                        elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)));
   }
 
   /**

diff --git a/src/main/java/frc/robot/commands/ElevatorPositionCommand.java b/src/main/java/frc/robot/commands/ElevatorPositionCommand.java
@@ -6,29 +6,37 @@
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorState;
 
 public class ElevatorPositionCommand extends CommandBase {
-  private final ElevatorSubsystem ElevatorSubsystem;
-  private final double targetHeight;
-  private final double targetAngle; // FIXME rename to "targetAngle"
+  private final ElevatorSubsystem elevatorSubsystem;
+  private double targetHeight;
+  private double targetAngle;
 
-  /** Creates a new ArmPositionCommand. */
+  /** Creates a new ElevatorPositionCommand. */
   public ElevatorPositionCommand(
-      ElevatorSubsystem elevatorSubsystem, double targetHeight, double targetAngle) {
-    // FIXME make a second constructor that requires an ElevatorState instead of two doubles for
-    // height and angle
-    this.ElevatorSubsystem = elevatorSubsystem;
+      ElevatorSubsystem subsystem, double targetHeight, double targetAngle) {
+    this.elevatorSubsystem = subsystem;
     this.targetHeight = targetHeight;
     this.targetAngle = targetAngle;
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(elevatorSubsystem);
   }
 
+  public ElevatorPositionCommand(ElevatorSubsystem elevatorSubsystem, ElevatorState elevatorState) {
+    // height and angle
+    this.elevatorSubsystem = elevatorSubsystem;
+    targetHeight = elevatorState.height();
+    targetAngle = elevatorState.angle();
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(elevatorSubsystem);
+  }
+
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    ElevatorSubsystem.setTargetHeight(targetHeight);
-    ElevatorSubsystem.setTargetAngle(targetAngle);
+    elevatorSubsystem.setTargetHeight(targetHeight);
+    elevatorSubsystem.setTargetAngle(targetAngle);
   }
 
   // Called every time the scheduler runs while the command is scheduled.