diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index 758d478..b339d7a 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -89,7 +89,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 b718146..7fb9e4d 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; @@ -79,8 +80,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 239f2da..2343663 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 @@ -114,16 +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, - Math.PI - rangeOfMotion.div(2).in(Radians), - Math.PI + rangeOfMotion.div(2).in(Radians)); + double clampedSetpoint = MathUtil.clamp(setpoint, lowerLimitRad, upperLimitRad); 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)); + 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 4ff675c..7b14f37 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 @@ -131,16 +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, - Math.PI - rangeOfMotion.div(2).in(Radians), - Math.PI + rangeOfMotion.div(2).in(Radians)); + double clampedSetpoint = MathUtil.clamp(setpoint, lowerLimitRad, upperLimitRad); 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)); + MathUtil.clamp(setpoint, lowerLimitRad + marginRad, upperLimitRad - marginRad); oversaturation = setpoint - clampedSetpoint; oversaturationLessMargin = setpoint - clampedSetpointWithMargin; double feedforwardVolts =