diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index a01a923..7870ed2 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -76,10 +76,27 @@ public static final class TurretConstants { // Geometry public static final Transform3d CHASSIS_TO_TURRET_BASE = new Transform3d(Inches.of(-4.000), Inches.of(6.500), Inches.of(16.331), Rotation3d.kZero); - public static final Rotation2d ABS_ENCODER_OFFSET = new Rotation2d(5.157); - public static final Rotation2d MECHANISM_OFFSET = Rotation2d.kZero; - public static final double UPPER_LIMIT_RAD = Units.degreesToRadians(270); - public static final double LOWER_LIMIT_RAD = Units.degreesToRadians(45); + // Turret angle convention: 0 / 2*pi rad = forward, increasing CCW (viewed from above), + // matching standard Rotation2d/WPILib handedness. All constants below are w/r/t forward. + + // Calibration value chosen so the absolute encoder reads 0 / 2*pi when the turret is + // physically facing forward. To reclock after a mechanical rebuild or encoder reseat: point + // the turret forward by hand, read the raw absolute encoder value (with this offset backed + // out, i.e. temporarily zeroed), and set ABS_ENCODER_OFFSET to that raw reading. + public static final Rotation2d ABS_ENCODER_OFFSET = new Rotation2d(5.157 + 1.267); + + // Soft limits of the mechanical range, measured CCW from forward (e.g. LOWER=-280 means the + // turret can travel 280 deg clockwise of forward). UPPER_LIMIT_RAD - LOWER_LIMIT_RAD must + // stay under 360 deg, or the wrap math below (CENTER_RAD, and its use in TurretIOSpark / + // TurretIOSimSpark) can no longer place every reachable angle in a single unambiguous branch. + public static final double UPPER_LIMIT_RAD = Units.degreesToRadians(30); + public static final double LOWER_LIMIT_RAD = Units.degreesToRadians(-280); + + // Center of the operating range. Used to pick the modulus window ([CENTER-pi, CENTER+pi)) + // that setpoints and the relative-encoder seed get wrapped into, so the branch cut always + // falls in the unreachable gap directly opposite the range rather than inside it. Derived + // automatically from the limits above - do not hand-edit when reclocking. + public static final double CENTER_RAD = (LOWER_LIMIT_RAD + UPPER_LIMIT_RAD) / 2.0; public static final double MARGIN_RAD = Units.degreesToRadians(5); // Position controller diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java index db1f0b9..1577ec7 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java @@ -78,7 +78,9 @@ public TurretIOSimSpark() { LinearSystemId.createDCMotorSystem(GEARBOX, TURRET_MOI_KG_M2, MOTOR_REDUCTION), GEARBOX); - turnSim.setState(2.0 * Math.PI - MECHANISM_OFFSET.getRadians(), 0); + double seedPosition = + MathUtil.inputModulus(2.0 * Math.PI, CENTER_RAD - Math.PI, CENTER_RAD + Math.PI); + turnSim.setState(seedPosition, 0); turnSparkSim.setPosition(turnSim.getAngularPositionRad()); } @@ -93,13 +95,13 @@ public void updateInputs(TurretIOInputs inputs) { // Update inputs inputs.motorControllerConnected = true; - inputs.relativePosition = new Rotation2d(turnSparkSim.getPosition()).plus(MECHANISM_OFFSET); + inputs.relativePosition = new Rotation2d(turnSparkSim.getPosition()); inputs.velocityRadPerSec = turnSparkSim.getVelocity(); inputs.appliedVolts = turnSparkSim.getAppliedOutput() * turnSparkSim.getBusVoltage(); inputs.currentAmps = Math.abs(turnSparkSim.getMotorCurrent()); inputs.absoluteEncoderConnected = true; - inputs.absolutePosition = new Rotation2d(turnSparkSim.getPosition()).plus(MECHANISM_OFFSET); + inputs.absolutePosition = new Rotation2d(turnSparkSim.getPosition()); inputs.oversaturation = oversaturation; inputs.oversaturationLessMargin = oversaturationLessMargin; @@ -115,8 +117,7 @@ public void setOpenLoop(Voltage volts) { @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = - MathUtil.inputModulus( - rotation.getRadians() - MECHANISM_OFFSET.getRadians(), 0.0, 2.0 * Math.PI); + MathUtil.inputModulus(rotation.getRadians(), CENTER_RAD - Math.PI, CENTER_RAD + Math.PI); double clampedSetpoint = MathUtil.clamp(setpoint, LOWER_LIMIT_RAD, UPPER_LIMIT_RAD); double clampedSetpointWithMargin = MathUtil.clamp(setpoint, LOWER_LIMIT_RAD + MARGIN_RAD, UPPER_LIMIT_RAD - MARGIN_RAD); diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index 48760c2..1416520 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -54,7 +54,7 @@ public TurretIOSpark() { new DutyCycleEncoder( new DigitalInput(DIOPorts.TURRET_ABS_ENCODER), 2 * Math.PI, - ABS_ENCODER_OFFSET.getRadians() + MECHANISM_OFFSET.getRadians()); + ABS_ENCODER_OFFSET.getRadians()); var turnConfig = new SparkMaxConfig(); @@ -98,12 +98,14 @@ public TurretIOSpark() { @Override public void updateInputs(TurretIOInputs inputs) { if (!relativeEncoderSeeded && inputs.absoluteEncoderConnected) { - turnSparkEncoder.setPosition(absoluteEncoder.get()); + double seedPosition = + MathUtil.inputModulus(absoluteEncoder.get(), CENTER_RAD - Math.PI, CENTER_RAD + Math.PI); + turnSparkEncoder.setPosition(seedPosition); relativeEncoderSeeded = true; } // Read from cached values (non-blocking) - updated by SparkOdometryThread - inputs.relativePosition = new Rotation2d(sparkInputs.getPosition()).plus(MECHANISM_OFFSET); + inputs.relativePosition = new Rotation2d(sparkInputs.getPosition()); inputs.velocityRadPerSec = sparkInputs.getVelocity(); inputs.appliedVolts = sparkInputs.getAppliedVolts(); inputs.currentAmps = sparkInputs.getOutputCurrent(); @@ -129,8 +131,7 @@ public void setOpenLoop(Voltage volts) { @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = - MathUtil.inputModulus( - rotation.getRadians() - MECHANISM_OFFSET.getRadians(), 0.0, 2 * Math.PI); + MathUtil.inputModulus(rotation.getRadians(), CENTER_RAD - Math.PI, CENTER_RAD + Math.PI); double clampedSetpoint = MathUtil.clamp(setpoint, LOWER_LIMIT_RAD, UPPER_LIMIT_RAD); double clampedSetpointWithMargin = MathUtil.clamp(setpoint, LOWER_LIMIT_RAD + MARGIN_RAD, UPPER_LIMIT_RAD - MARGIN_RAD);