From 650bd2a39d5cc8456e420754632af79021d0df11 Mon Sep 17 00:00:00 2001 From: Patrick Hurley Date: Sun, 5 Apr 2026 21:42:46 -0400 Subject: [PATCH] Pivot motor: add gravity compensation and auto-zero on enable kG was set to 0 and never applied to motor config, so only the P-term fought gravity. Also start pivotZeroed=false so zeroing runs on first enable, and default toward stow for safety. Co-Authored-By: Claude Opus 4.6 (1M context) --- src/main/java/frc/robot/subsystems/Collector.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Collector.java b/src/main/java/frc/robot/subsystems/Collector.java index fdce0ba9..49dd8755 100644 --- a/src/main/java/frc/robot/subsystems/Collector.java +++ b/src/main/java/frc/robot/subsystems/Collector.java @@ -69,7 +69,10 @@ public class CollectorConstants { public static final double PIVOT_KI = 0d; // temp public static final double PIVOT_KD = 0d; // temp public static final double PIVOT_KS = 0; // temp - public static final double PIVOT_KG = 0d; // temp + // Gravity compensation voltage. Negative = upward force (same convention + // as Hood kG). START LOW and increase until the arm holds position without + // sagging. If the arm drifts up, make the value less negative (closer to 0). + public static final double PIVOT_KG = RobotMap.IS_OASIS ? -0.15d : -0.15d; // tune me! // pivot @@ -118,12 +121,12 @@ public class CollectorConstants { private MechanismLigament2d ligament; private Angle targetPivotPosition; private PositionVoltage positionPID; - private boolean pivotZeroed = true; + private boolean pivotZeroed = false; // false = will auto-zero on first enable private final Timer zeroingTimer = new Timer(); private boolean pivotActive = false; private BooleanEntry requestZeroingDeploy; private BooleanEntry requestZeroingStow; - private boolean stowZero = false; + private boolean stowZero = true; // true = zero toward stow (retracted) hard stop on startup private DCMotor gearbox; @@ -153,6 +156,7 @@ public Collector() { pivotConfig.Slot0.kI = CollectorConstants.PIVOT_KI; pivotConfig.Slot0.kD = CollectorConstants.PIVOT_KD; pivotConfig.Slot0.kS = CollectorConstants.PIVOT_KS; + pivotConfig.Slot0.kG = CollectorConstants.PIVOT_KG; pivotConfig.Feedback.SensorToMechanismRatio = CollectorConstants.ENCODER_TO_MECHANISM_RATIO; pivotConfig.CurrentLimits.StatorCurrentLimitEnable = CollectorConstants.PIVOT_SUPPLY_LIMIT_ENABLE;