fixed logic errors on selection for algae / coral wristevator setpoint

Team334 · Jan 21, 2025 · de2b4a0 · de2b4a0
1 parent 6dddc92
commit de2b4a0
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 9 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -1,12 +1,7 @@
 {
-  "FMS": {
-    "window": {
-      "visible": false
-    }
-  },
   "System Joysticks": {
     "window": {
-      "visible": false
+      "enabled": false
     }
   },
   "keyboardJoysticks": [
@@ -101,6 +96,10 @@
   "robotJoysticks": [
     {
       "guid": "Keyboard0"
+    },
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
     }
   ]
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -205,12 +205,23 @@ private void configureOperatorBindings() {
     _operatorController.rightStick().whileTrue(_wristevator.setSetpoint(HOME));
 
     _operatorController.a().whileTrue(_wristevator.setSetpoint(L1));
+
     _operatorController
         .b()
-        .whileTrue(_wristevator.setSetpoint(getCurrentPiece() == Piece.CORAL ? L2 : LOWER_ALGAE));
+        .whileTrue(
+            either(
+                _wristevator.setSetpoint(L2),
+                _wristevator.setSetpoint(LOWER_ALGAE),
+                () -> getCurrentPiece() == Piece.CORAL));
+
     _operatorController
         .y()
-        .whileTrue(_wristevator.setSetpoint(getCurrentPiece() == Piece.CORAL ? L3 : UPPER_ALGAE));
+        .whileTrue(
+            either(
+                _wristevator.setSetpoint(L3),
+                _wristevator.setSetpoint(UPPER_ALGAE),
+                () -> getCurrentPiece() == Piece.CORAL));
+
     _operatorController.x().whileTrue(_wristevator.setSetpoint(L4));
 
     _operatorController
@@ -235,6 +246,11 @@ private Command rumbleControllers(double rumble, double seconds) {
           _driverController.getHID().setRumble(RumbleType.kBothRumble, rumble);
           _operatorController.getHID().setRumble(RumbleType.kBothRumble, rumble);
         })
+        .finallyDo(
+            () -> {
+              _driverController.getHID().setRumble(RumbleType.kBothRumble, 0);
+              _operatorController.getHID().setRumble(RumbleType.kBothRumble, 0);
+            })
         .withTimeout(seconds);
   }
 

diff --git a/src/main/java/frc/robot/subsystems/Wristevator.java b/src/main/java/frc/robot/subsystems/Wristevator.java
@@ -3,6 +3,7 @@
 import static edu.wpi.first.units.Units.*;
 
 import com.ctre.phoenix6.hardware.TalonFX;
+import dev.doglog.DogLog;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
@@ -101,7 +102,9 @@ public boolean homeSwitch() {
 
   /** Set the wristevator to a setpoint. */
   public Command setSetpoint(WristevatorSetpoint setpoint) {
-    return run(() -> {}).withName("Set Setpoint: " + setpoint.toString());
+    return run(() -> {})
+        .beforeStarting(() -> DogLog.log("Wristevator/Setpoint", setpoint.toString()))
+        .withName("Set Setpoint: " + setpoint.toString());
   }
 
   /** Control the elevator and wrist speeds individually. */