Revert "fix for #24"

This reverts commit ec3ea78.
DeepBlueRobotics · Mar 6, 2024 · 571b732 · 571b732
1 parent 519029c
commit 571b732
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 2 deletions.
diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -58,10 +58,17 @@ private void setDefaultCommands() {
     // () -> ProcessedAxisValue(driverController, Axis.kRightX)),
     // () -> driverController.getRawButton(OI.Driver.slowDriveButton)
     // ));
+
     arm.setDefaultCommand(new ArmTeleop(arm, () -> inputProcessing(getStickValue(manipulatorController, Axis.kLeftY))));
   }
 
   private void setBindingsDriver() {
+    new JoystickButton(driverController, Button.kX.value)
+        .whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    new JoystickButton(driverController, Button.kY.value)
+        .whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    new JoystickButton(driverController, Button.kB.value).whileTrue(arm.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    new JoystickButton(driverController, Button.kA.value).whileTrue(arm.sysIdDynamic(SysIdRoutine.Direction.kReverse));
     // 4 cardinal directions on arrowpad
     // slowmode toggle on trigger
     // 3 cardinal directions on letterpad

diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
@@ -46,8 +46,6 @@ public void initialize() {
   public void execute() {
     // use trapazoid math and controllerMoveArm method from arm subsytem to apply
     // voltage to the motor
-    if (Constants.OI.JOY_THRESH == 0)
-      return;
     double speeds = getRequestedSpeeds();
     double currTime = Timer.getFPGATimestamp();
     double deltaT = currTime - lastTime;