Add more testing controls

frc868 · Feb 13, 2024 · cd5e1cd · cd5e1cd
1 parent 55eed20
commit cd5e1cd
Showing 1 changed file with 13 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java
@@ -5,6 +5,7 @@
 
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.Constants.Intake.IntakePosition;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
@@ -18,7 +19,7 @@ public class Controls {
     private static double speedMultiplier = 1.0;
 
     public static void configureDriverControl(int port, Drivetrain drivetrain, Intake intake, Shooter shooter,
-            ShooterTilt shooterTilt, Climber climber, NoteLift noteLift, LEDs leds) {
+            ShooterTilt shooterTilt, Climber climber, NoteLift noteLift) {
         CommandVirpilJoystick joystick = new CommandVirpilJoystick(port);
 
         drivetrain.setDefaultCommand(
@@ -57,7 +58,7 @@ public static void configureDriverControl(int port, Drivetrain drivetrain, Intak
     }
 
     public static void configureTestingControl(int port, Drivetrain drivetrain, Intake intake, Shooter shooter,
-            ShooterTilt shooterTilt, Climber climber, NoteLift noteLift, LEDs leds) {
+            ShooterTilt shooterTilt, Climber climber, NoteLift noteLift) {
         CommandXboxController controller = new CommandXboxController(port);
 
         controller.x().whileTrue(shooterTilt
@@ -67,6 +68,16 @@ public static void configureTestingControl(int port, Drivetrain drivetrain, Inta
         controller.a().whileTrue(intake
                 .setOverridenSpeedCommand(() -> controller.getRightTriggerAxis() - controller.getLeftTriggerAxis()));
         controller.b().whileTrue(intake.runRollersCommand());
+
+        controller.povLeft().whileTrue(climber
+                .setOverridenSpeedCommand(() -> controller.getRightTriggerAxis() - controller.getLeftTriggerAxis()));
+        controller.povRight().whileTrue(noteLift
+                .setOverridenSpeedCommand(() -> controller.getRightTriggerAxis() - controller.getLeftTriggerAxis()));
+
+        // controller.x().whileTrue(intake.sysIdQuasistatic(Direction.kForward));
+        // controller.y().whileTrue(intake.sysIdQuasistatic(Direction.kReverse));
+        // controller.a().whileTrue(intake.sysIdDynamic(Direction.kForward));
+        // controller.b().whileTrue(intake.sysIdDynamic(Direction.kReverse));
     }
 
 }