From 7d791664d05c944842b8d0695427725b38a6fb56 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sat, 28 Feb 2026 19:12:58 -0500 Subject: [PATCH 1/3] precalculate upper and lower limits --- .../java/frc/robot/subsystems/launcher/Launcher.java | 2 +- .../robot/subsystems/launcher/LauncherConstants.java | 6 ++++-- .../robot/subsystems/launcher/TurretIOSimSpark.java | 12 ++++++------ .../frc/robot/subsystems/launcher/TurretIOSpark.java | 12 ++++++------ 4 files changed, 17 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index ca7f833..0e3e51f 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -84,7 +84,7 @@ public Launcher( flywheelDisconnectedAlert = new Alert("Disconnected flywheel motor", AlertType.kError); hoodDisconnectedAlert = new Alert("Disconnected hood motor", AlertType.kError); - headingController.setTolerance(margin.in(Radians)); + headingController.setTolerance(marginRad); } @Override diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 16e0358..34aaac2 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -71,8 +72,9 @@ public static final class TurretConstants { new Transform3d(Inches.of(-2.271), Inches.of(-4.959), Inches.of(16.331), Rotation3d.kZero); public static final Rotation2d absEncoderOffset = new Rotation2d(5.157); public static final Rotation2d mechanismOffset = Rotation2d.kZero; - public static final Angle rangeOfMotion = Degrees.of(180); - public static final Angle margin = Degrees.of(5); + public static final double upperLimitRad = Units.degreesToRadians(270); + public static final double lowerLimitRad = Units.degreesToRadians(45); + public static final double marginRad = Units.degreesToRadians(5); // Position controller public static final double kPReal = 0.5; diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java index 0c630d3..75eab04 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java @@ -55,9 +55,9 @@ public TurretIOSimSpark() { turnConfig .softLimit - .forwardSoftLimit(Math.PI + rangeOfMotion.div(2).in(Radians)) + .forwardSoftLimit(upperLimitRad) .forwardSoftLimitEnabled(true) - .reverseSoftLimit(Math.PI - rangeOfMotion.div(2).in(Radians)) + .reverseSoftLimit(lowerLimitRad) .reverseSoftLimitEnabled(true); turnConfig @@ -118,13 +118,13 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double clampedSetpoint = MathUtil.clamp( setpoint, - Math.PI - rangeOfMotion.div(2).in(Radians), - Math.PI + rangeOfMotion.div(2).in(Radians)); + upperLimitRad, + lowerLimitRad); double clampedSetpointWithMargin = MathUtil.clamp( setpoint, - Math.PI - rangeOfMotion.div(2).in(Radians) + margin.in(Radians), - Math.PI + rangeOfMotion.div(2).in(Radians) - margin.in(Radians)); + upperLimitRad - marginRad, + lowerLimitRad + marginRad); oversaturation = setpoint - clampedSetpoint; oversaturationLessMargin = setpoint - clampedSetpointWithMargin; double feedforwardVolts = diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index e3f1b0c..ccee678 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -71,9 +71,9 @@ public TurretIOSpark() { turnConfig .softLimit - .forwardSoftLimit(Math.PI + rangeOfMotion.div(2).in(Radians)) + .forwardSoftLimit(upperLimitRad) .forwardSoftLimitEnabled(true) - .reverseSoftLimit(Math.PI - rangeOfMotion.div(2).in(Radians)) + .reverseSoftLimit(lowerLimitRad) .reverseSoftLimitEnabled(true); turnConfig @@ -135,13 +135,13 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double clampedSetpoint = MathUtil.clamp( setpoint, - Math.PI - rangeOfMotion.div(2).in(Radians), - Math.PI + rangeOfMotion.div(2).in(Radians)); + upperLimitRad, + lowerLimitRad); double clampedSetpointWithMargin = MathUtil.clamp( setpoint, - Math.PI - rangeOfMotion.div(2).in(Radians) + margin.in(Radians), - Math.PI + rangeOfMotion.div(2).in(Radians) - margin.in(Radians)); + upperLimitRad - marginRad, + lowerLimitRad + marginRad); oversaturation = setpoint - clampedSetpoint; oversaturationLessMargin = setpoint - clampedSetpointWithMargin; double feedforwardVolts = From 79c9d31c9c699f528349a3cf9cee9c8aeb15a001 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sat, 28 Feb 2026 19:14:11 -0500 Subject: [PATCH 2/3] formattin --- .../robot/subsystems/launcher/TurretIOSimSpark.java | 11 ++--------- .../frc/robot/subsystems/launcher/TurretIOSpark.java | 11 ++--------- 2 files changed, 4 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java index 75eab04..c5ce966 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java @@ -115,16 +115,9 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = MathUtil.inputModulus( rotation.getRadians() - mechanismOffset.getRadians(), 0.0, 2.0 * Math.PI); - double clampedSetpoint = - MathUtil.clamp( - setpoint, - upperLimitRad, - lowerLimitRad); + double clampedSetpoint = MathUtil.clamp(setpoint, upperLimitRad, lowerLimitRad); double clampedSetpointWithMargin = - MathUtil.clamp( - setpoint, - upperLimitRad - marginRad, - lowerLimitRad + marginRad); + MathUtil.clamp(setpoint, upperLimitRad - marginRad, lowerLimitRad + marginRad); oversaturation = setpoint - clampedSetpoint; oversaturationLessMargin = setpoint - clampedSetpointWithMargin; double feedforwardVolts = diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index ccee678..e0d10d0 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -132,16 +132,9 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = MathUtil.inputModulus( rotation.getRadians() - mechanismOffset.getRadians(), 0.0, 2 * Math.PI); - double clampedSetpoint = - MathUtil.clamp( - setpoint, - upperLimitRad, - lowerLimitRad); + double clampedSetpoint = MathUtil.clamp(setpoint, upperLimitRad, lowerLimitRad); double clampedSetpointWithMargin = - MathUtil.clamp( - setpoint, - upperLimitRad - marginRad, - lowerLimitRad + marginRad); + MathUtil.clamp(setpoint, upperLimitRad - marginRad, lowerLimitRad + marginRad); oversaturation = setpoint - clampedSetpoint; oversaturationLessMargin = setpoint - clampedSetpointWithMargin; double feedforwardVolts = From d102c563002b4e1180daa50d007ea5d493630c09 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Mon, 2 Mar 2026 20:23:26 -0500 Subject: [PATCH 3/3] fix inverted clamping limits --- .../java/frc/robot/subsystems/launcher/TurretIOSimSpark.java | 4 ++-- .../java/frc/robot/subsystems/launcher/TurretIOSpark.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java index 01e9909..2343663 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java @@ -114,9 +114,9 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = MathUtil.inputModulus( rotation.getRadians() - mechanismOffset.getRadians(), 0.0, 2.0 * Math.PI); - double clampedSetpoint = MathUtil.clamp(setpoint, upperLimitRad, lowerLimitRad); + double clampedSetpoint = MathUtil.clamp(setpoint, lowerLimitRad, upperLimitRad); double clampedSetpointWithMargin = - MathUtil.clamp(setpoint, upperLimitRad - marginRad, lowerLimitRad + marginRad); + MathUtil.clamp(setpoint, lowerLimitRad + marginRad, upperLimitRad - marginRad); oversaturation = setpoint - clampedSetpoint; oversaturationLessMargin = setpoint - clampedSetpointWithMargin; double feedforwardVolts = diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index a8604ff..7b14f37 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -131,9 +131,9 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = MathUtil.inputModulus( rotation.getRadians() - mechanismOffset.getRadians(), 0.0, 2 * Math.PI); - double clampedSetpoint = MathUtil.clamp(setpoint, upperLimitRad, lowerLimitRad); + double clampedSetpoint = MathUtil.clamp(setpoint, lowerLimitRad, upperLimitRad); double clampedSetpointWithMargin = - MathUtil.clamp(setpoint, upperLimitRad - marginRad, lowerLimitRad + marginRad); + MathUtil.clamp(setpoint, lowerLimitRad + marginRad, upperLimitRad - marginRad); oversaturation = setpoint - clampedSetpoint; oversaturationLessMargin = setpoint - clampedSetpointWithMargin; double feedforwardVolts =