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;