diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 568fe9e..a097a2f 100644 --- 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)); } }