a

diff --git a/src/main/java/frc/robot/commands/gpm/ClimbCommand.java b/src/main/java/frc/robot/commands/gpm/ClimbCommand.java
new file mode 100644 (file)
index 0000000..932fab9
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/ClimbCommand.java
@@ -0,0 +1,26 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.PS5Controller;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.drive_comm.DriveToPose;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.Climb.LinearClimb;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+public class ClimbCommand extends SequentialCommandGroup{
+
+    public ClimbCommand(LinearClimb climb, Drivetrain drive, PS5Controller controller){
+        addCommands(
+            new ParallelCommandGroup(
+                new InstantCommand(() -> climb.goUp()),
+                new DriveToPose(drive, () -> FieldConstants.getClimbLocation())
+            ),
+            new InstantCommand(() -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 1.0))
+        );
+    }
+
+
+}

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index ba1f454d55e35a33cb7d614e0abf2108da888e38..7feba79cbe132eceeab3840d711e8263a7be65bf 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -2,6 +2,8 @@ package frc.robot.constants;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -16,6 +18,20 @@ public class FieldConstants {
   /**Apriltag layout for 2026 REBUILT */
   public static final AprilTagFieldLayout field = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
 
+  // TODO: Find this
+  /**The coordinate of the climb position */
+  public static final Pose2d BLUE_CLIMB_LOCATION = new Pose2d(0, 0, new Rotation2d());
+  public static final Pose2d RED_CLIMB_LOCATION = new Pose2d(0, 0, new Rotation2d());
+
+  public static final Pose2d getClimbLocation(){
+    if (Robot.getAlliance() == Alliance.Blue){
+      return BLUE_CLIMB_LOCATION;
+    }
+    else{
+      return RED_CLIMB_LOCATION;
+    }
+  }
+
   /** Location of hub target */
   public static final Translation3d HUB_BLUE =
       new Translation3d(Units.inchesToMeters(156.8), 4.035, Units.inchesToMeters(72));