Fixed ground pick command

Iron-Panthers · Sep 22, 2023 · 2b03776 · 2b03776
1 parent 98a04ff
commit 2b03776
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 7 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -197,9 +197,10 @@ public static final class Ports {
     }
 
     public static final class Setpoints {
-      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 ElevatorState STOWED = new ElevatorState(20, 0);
+      public static final ElevatorState HANDOFF = new ElevatorState(10, 10);
+      public static final ElevatorState SHELF_INTAKE = new ElevatorState(20, 20);
+      public static final ElevatorState GROUND_INTAKE = new Elevator(0, 0);
     }
 
     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
@@ -66,6 +66,7 @@
 import java.util.Optional;
 import java.util.function.DoubleSupplier;
 
+
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}

diff --git a/src/main/java/frc/robot/commands/GroundPickupCommand.java b/src/main/java/frc/robot/commands/GroundPickupCommand.java
@@ -23,13 +23,13 @@ public GroundPickupCommand(
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-        new IntakeModeCommand(intakeSubsystem, IntakeMode.INTAKE)
-            .deadlineWith(
-                new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.HANDOFF))
+        new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.GROUND_INTAKE)
+            .andThen(
+                new IntakeModeCommand(intakeSubsystem, IntakeMode.INTAKE))
             .andThen(
                 new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED)
                     .alongWith(new IntakeModeCommand(intakeSubsystem, IntakeMode.HOLD))));
-    // hold???
+
 
   }
 }