the kitty

src/main/java/frc/robot/RobotContainer.java

@@ -20,9 +20,8 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.commands.drive.HoldStance;
 import frc.robot.subsystems.drive.Drive;
-import frc.robot.subsystems.drive.DriveIO;
-import frc.robot.subsystems.drive.DriveIOSim;
 import frc.robot.subsystems.drive.DriveIOSparkMax;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -34,7 +33,7 @@
  */
 public class RobotContainer {
   // Subsystems
-  private final Drive drive;
+  public static final Drive drive = new Drive(new DriveIOSparkMax());
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -44,22 +43,22 @@ public class RobotContainer {
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
-    switch (Constants.currentMode) {
-      case REAL:
-        // Real robot, instantiate hardware IO implementations
-        drive = new Drive(new DriveIOSparkMax());
-        break;
+    // switch (Constants.currentMode) {
+    //   case REAL:
+    //     // Real robot, instantiate hardware IO implementations
+    //     drive = new Drive(new DriveIOSparkMax());
+    //     break;
 
-      case SIM:
-        // Sim robot, instantiate physics sim IO implementations
-        drive = new Drive(new DriveIOSim());
-        break;
+    //   case SIM:
+    //     // Sim robot, instantiate physics sim IO implementations
+    //     drive = new Drive(new DriveIOSim());
+    //     break;
 
-      default:
-        // Replayed robot, disable IO implementations
-        drive = new Drive(new DriveIO() {});
-        break;
-    }
+    //   default:
+    //     // Replayed robot, disable IO implementations
+    //     drive = new Drive(new DriveIO() {});
+    //     break;
+    // }
 
     // Set up auto routines
     autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
@@ -90,6 +89,7 @@ private void configureButtonBindings() {
     drive.setDefaultCommand(
         Commands.run(
             () -> drive.driveArcade(-controller.getLeftY(), -controller.getRightX()), drive));
+    controller.a().whileTrue(new HoldStance());
   }
 
   /**

src/main/java/frc/robot/commands/drive/HoldStance.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+
+public class HoldStance extends Command {
+  /** Creates a new HoldStance. */
+  PIDController leftPID = new PIDController(40, 0, 0);
+
+  PIDController rightPID = new PIDController(40, 0, 0);
+
+  public HoldStance() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    double leftSetpoint = RobotContainer.drive.getLeftPositionMeters();
+    double rightSetpoint = RobotContainer.drive.getRightPositionMeters();
+
+    leftPID.setSetpoint(leftSetpoint);
+    rightPID.setSetpoint(rightSetpoint);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    RobotContainer.drive.driveVolts(
+        leftPID.calculate(RobotContainer.drive.getLeftPositionMeters()),
+        rightPID.calculate(RobotContainer.drive.getRightPositionMeters()));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}