Added Constants

Iron-Panthers · Sep 18, 2023 · 98036d5 · 98036d5
1 parent fdfc21f
commit 98036d5
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 31 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -195,6 +195,12 @@ public static final class Ports {
       public static final int WRIST_MOTOR_PORT = CAN.at(0, "wrist motor port");
     }
 
+    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 double MAX_HEIGHT = 10;
     public static final double MIN_HEIGHT = 0;
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -303,10 +303,10 @@ private void configureButtonBindings() {
                 drivebaseSubsystem, intakeSubsystem, EngageCommand.EngageDirection.GO_BACKWARD));
 
     // outtake states
-    
-    jasonLayer
-        .off(jason.leftTrigger())
-        .whileTrue(new ForceOuttakeSubsystemModeCommand(intakeSubsystem, IntakeMode.INTAKE));
+
+    // jasonLayer
+    //     .off(jason.leftTrigger())
+    //     .whileTrue(new ForceOuttakeSubsystemModeCommand(intakeSubsystem, IntakeMode.INTAKE));
     jasonLayer
         .off(jason.rightTrigger())
         .onTrue(new IntakeModeCommand(intakeSubsystem, IntakeMode.OUTTAKE));
@@ -328,32 +328,33 @@ private void configureButtonBindings() {
         // FIXME: This error is here to kind of guide you...
         .onTrue(
             new ElevatorPositionCommand(
-                elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))
-        .whileTrue(
-            new ForceOuttakeSubsystemModeCommand(
-                intakeSubsystem, IntakeSubsystem.IntakeMode.INTAKE));
+                elevatorSubsystem, 0, Constants.Elevator.Setpoints.SHELF_INTAKE));
+    //     .whileTrue(
+    //         new ForceOuttakeSubsystemModeCommand(
+    //             intakeSubsystem, IntakeSubsystem.IntakeMode.INTAKE));
 
     // Reset arm position
     jasonLayer
         .off(jason.y())
         // FIXME: This error is here to kind of guide you...
-        .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED))
+        .onTrue(
+            new ElevatorPositionCommand(elevatorSubsystem, 0, Constants.Elevator.Setpoints.STOWED))
         .onTrue(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.HOLD));
     jason.start().onTrue(new SetZeroModeCommand(elevatorSubsystem));
 
-    jasonLayer
-        .off(jason.a())
-        .onTrue(
-            new GroundPickupCommand(
-                intakeSubsystem,
-                outtakeSubsystem,
-                elevatorSubsystem,
-                () ->
-                    jason.getHID().getPOV() == 180
-                        ? IntakeSubsystem.IntakeMode.INTAKE_LOW
-                        : IntakeSubsystem.IntakeMode.INTAKE));
-
-    jason.start().onTrue(new ZeroIntakeModeCommand(intakeSubsystem));
+    // jasonLayer
+    //     .off(jason.a())
+    //     .onTrue(
+    //         new GroundPickupCommand(
+    //             intakeSubsystem,
+    //             outtakeSubsystem,
+    //             elevatorSubsystem,
+    //             () ->
+    //                 jason.getHID().getPOV() == 180
+    //                     ? IntakeSubsystem.IntakeMode.INTAKE_LOW
+    //                     : IntakeSubsystem.IntakeMode.INTAKE));
+
+    // jason.start().onTrue(new ZeroIntakeModeCommand(intakeSubsystem));
 
     jason
         .back()
@@ -362,13 +363,11 @@ private void configureButtonBindings() {
                 // FIXME: This error is here to kind of guide you...
                 .alongWith(
                     new ElevatorPositionCommand(
-                        elevatorSubsystem, Constants.Elevator.Setpoints.HANDOFF))
-                .alongWith(
-                    new ForceOuttakeSubsystemModeCommand(
-                        outtakeSubsystem, IntakeSubsystem.IntakeMode.OFF)))
+                        elevatorSubsystem, 0, Constants.Elevator.Setpoints.HANDOFF))
+                .alongWith(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.OFF)))
         .onFalse(
             // FIXME: This error is here to kind of guide you...
-            new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)
+            new ElevatorPositionCommand(elevatorSubsystem, 0, Constants.Elevator.Setpoints.STOWED)
                 .alongWith(
                     new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.HOLD)));
 
@@ -450,15 +449,15 @@ private void setupAutonomousCommands() {
 
     final List<ScoreStep> drivingCubeOuttake =
         List.of(
-            new ScoreStep(new ArmState(35, Arm.Setpoints.Extensions.MIN_EXTENSION)).canWaitHere(),
+            new ScoreStep(new ElevatorState(35, Constants.Elevator.MIN_HEIGHT)).canWaitHere(),
             new ScoreStep(IntakeMode.OUTTAKE));
     final boolean[] intakeLow = {false};
     final Map<String, Command> eventMap =
         Map.of(
             "stow arm",
             "zero everything",
             (new SetZeroModeCommand(elevatorSubsystem))
-                .alongWith(new ZeroIntakeModeCommand(intakeSubsystem)),
+                .alongWith(new IntakeModeCommand(intakeSubsystem, IntakeMode.HOLD)),
             "intake",
             new GroundPickupCommand(
                 intakeSubsystem,
@@ -507,11 +506,14 @@ public void end(boolean interrupted) {
             new ScoreCommand(
                     intakeSubsystem, elevatorSubsystem, drivingCubeOuttake.subList(1, 2), 1)
                 .andThen(
-                    new ElevatorPositionCommand(elevatorSubsystem, Arm.Setpoints.STOWED)
+                    new ElevatorPositionCommand(
+                            elevatorSubsystem, 0, Constants.Elevator.Setpoints.STOWED)
                         .andThen(new IntakeModeCommand(intakeSubsystem, IntakeMode.OFF))),
             "armbat preload",
             new ElevatorPositionCommand(elevatorSubsystem, 30, 0)
-                .andThen(new ElevatorPositionCommand(elevatorSubsystem, Arm.Setpoints.STOWED)));
+                .andThen(
+                    new ElevatorPositionCommand(
+                        elevatorSubsystem, 0, Constants.Elevator.Setpoints.STOWED)));
 
     autoSelector.setDefaultOption(
         "N1 1Cone + 2Cube Low Mobility Engage",