predictive wrist pose instead of reactive

TitaniumTitans · Feb 28, 2024 · 38d6b6e · 38d6b6e
1 parent dd37c62
commit 38d6b6e
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
@@ -122,10 +122,10 @@ public Command setArmDesiredPose(double armPose, double wristPose) {
     return run(() -> {
       m_desiredArmPoseDegs = armPose;
 
-      double wristGap = m_inputs.wristPositionDegs + m_inputs.armPositionDegs;
-      if (wristGap < ArmConstants.WRIST_ARM_GAP.getValue()) {
-        double underGap = ArmConstants.WRIST_ARM_GAP.getValue() - wristGap;
-        m_desiredWristPoseDegs = m_inputs.wristPositionDegs + underGap;
+      double estWristGap = armPose + wristPose;
+      if (estWristGap < ArmConstants.WRIST_ARM_GAP.getValue()) {
+        double underGap = ArmConstants.WRIST_ARM_GAP.getValue() - estWristGap;
+        m_desiredWristPoseDegs = wristPose + underGap;
       } else {
         m_desiredWristPoseDegs = wristPose;
       }

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java
@@ -59,7 +59,7 @@ public ShooterIOKraken() {
     m_indexer = new TalonFX(ShooterConstants.INDEXER_ID, canbus);
 
     m_tof = new TimeOfFlight(28);
-    m_tof.setRangingMode(TimeOfFlight.RangingMode.Short, 10);
+    m_tof.setRangingMode(TimeOfFlight.RangingMode.Short, 25);
 
     // general motor configs
     TalonFXConfiguration shooterConfig = new TalonFXConfiguration();