From 4caa8ff3cb5a3eec09ac07b3b8a0f609a7095a18 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 3 Feb 2026 19:19:06 -0500 Subject: [PATCH 1/9] changed from Spark PID controller to WPILib PID controller --- .../launcher/LauncherConstants.java | 1 + .../launcher/TurretIOSparkAlternate.java | 129 ++++++++++++++++++ 2 files changed, 130 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 54b1ac5..711d1d1 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -43,6 +43,7 @@ public static final class TurretConstants { // Motor controller public static final int port = 12; + public static final int DIOPort = 5; public static final double motorReduction = 5.0; public static final AngularVelocity maxAngularVelocity = NEO550Constants.kFreeSpeed.div(motorReduction); diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java new file mode 100644 index 0000000..6cf1aa7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java @@ -0,0 +1,129 @@ +package frc.robot.subsystems.launcher; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; +import static frc.robot.util.SparkUtil.*; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import frc.robot.Constants.MotorConstants.NEO550Constants; +import frc.robot.Constants.RobotConstants; +import java.util.function.DoubleSupplier; + +public class TurretIOSparkAlternate implements TurretIO { + + private final SparkBase turnSpark; + private final RelativeEncoder turnSparkEncoder; + private final PIDController controller; + private final DutyCycleEncoder absoluteEncoder; + private final Debouncer turnConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private boolean closedLoop = false; + private double appliedVolts = 0.0; + private double feedforwardVolts = 0.0; + + public TurretIOSparkAlternate() { + turnSpark = new SparkMax(port, MotorType.kBrushless); + turnSparkEncoder = turnSpark.getEncoder(); + controller = new PIDController(kPReal, 0.0, 0.0); + absoluteEncoder = new DutyCycleEncoder(new DigitalInput(DIOPort), 2 * Math.PI, Math.PI); + + var turnConfig = new SparkMaxConfig(); + + turnConfig + .inverted(false) + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.kNominalVoltage); + + turnConfig + .encoder + .inverted(false) + .positionConversionFactor(encoderPositionFactor) + .velocityConversionFactor(encoderVelocityFactor); + + turnConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPReal, 0.0, 0.0); + + turnConfig + .signals + .absoluteEncoderPositionAlwaysOn(true) + .absoluteEncoderPositionPeriodMs(20) + .absoluteEncoderVelocityAlwaysOn(true) + .absoluteEncoderVelocityPeriodMs(20) + .appliedOutputPeriodMs(20) + .busVoltagePeriodMs(20) + .outputCurrentPeriodMs(20); + + tryUntilOk( + turnSpark, + 5, + () -> + turnSpark.configure( + turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + + turnSparkEncoder.setPosition(absoluteEncoder.get()); + } + + @Override + public void updateInputs(TurretIOInputs inputs) { + // Run closed-loop control + if (closedLoop) { + appliedVolts = controller.calculate(turnSparkEncoder.getPosition()) + feedforwardVolts; + } else { + controller.reset(); + } + + // Update state + turnSpark.setVoltage( + MathUtil.clamp( + appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + + sparkStickyFault = false; + ifOk( + turnSpark, + turnSparkEncoder::getPosition, + (value) -> inputs.position = new Rotation2d(value)); + ifOk(turnSpark, turnSparkEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); + ifOk( + turnSpark, + new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, + (values) -> inputs.appliedVolts = values[0] * values[1]); + ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.currentAmps = value); + inputs.connected = turnConnectedDebounce.calculate(!sparkStickyFault); + } + + @Override + public void setOpenLoop(double output) { + closedLoop = false; + appliedVolts = output; + } + + @Override + public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { + closedLoop = true; + double setpoint = + // Math.max(minAngle, Math.min(maxAngle, rotation.getRadians())) + + // rotationOffset.getRadians(); + MathUtil.inputModulus(rotation.getRadians(), minInput, maxInput); + this.feedforwardVolts = + RobotConstants.kNominalVoltage + * angularVelocity.in(RadiansPerSecond) + / maxAngularVelocity.in(RadiansPerSecond); + controller.setSetpoint(setpoint); + } +} From 77ca589eeb50f47d7003c24f6cbfbd7911242287 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 3 Feb 2026 20:11:38 -0500 Subject: [PATCH 2/9] made some code tweaks, added an offset --- src/main/java/frc/robot/Robot.java | 4 ++-- .../frc/robot/subsystems/launcher/LauncherConstants.java | 4 ++-- .../subsystems/launcher/TurretIOSparkAlternate.java | 9 ++++++--- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b9adf0e..0fb966d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,7 +41,7 @@ import frc.robot.subsystems.launcher.Launcher; import frc.robot.subsystems.launcher.TurretIO; import frc.robot.subsystems.launcher.TurretIOSim; -import frc.robot.subsystems.launcher.TurretIOSpark; +import frc.robot.subsystems.launcher.TurretIOSparkAlternate; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhotonVision; @@ -118,7 +118,7 @@ public Robot() { new Launcher( drive::getPose, drive::getRobotRelativeChassisSpeeds, - new TurretIOSpark(), + new TurretIOSparkAlternate(), new FlywheelIOSim() {}, new HoodIOSim()); break; diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 711d1d1..3031cb2 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -33,8 +33,8 @@ public static final class TurretConstants { public static final Rotation2d rotationOffset = new Rotation2d(0.44); // Absolute encoder - public static final double encoderPositionFactor = 2 * Math.PI; // Radians - public static final double encoderVelocityFactor = (2 * Math.PI) / 60.0; // Rad/sec + public static final double encoderPositionFactor = (2 * Math.PI) / 5.0; // Radians + public static final double encoderVelocityFactor = (2 * Math.PI) / (60.0 * 5.0); // Rad/sec // Position controller public static final double minInput = 0.0; diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java index 6cf1aa7..31dcbf9 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java @@ -23,6 +23,7 @@ import frc.robot.Constants.MotorConstants.NEO550Constants; import frc.robot.Constants.RobotConstants; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; public class TurretIOSparkAlternate implements TurretIO { @@ -41,7 +42,7 @@ public TurretIOSparkAlternate() { turnSpark = new SparkMax(port, MotorType.kBrushless); turnSparkEncoder = turnSpark.getEncoder(); controller = new PIDController(kPReal, 0.0, 0.0); - absoluteEncoder = new DutyCycleEncoder(new DigitalInput(DIOPort), 2 * Math.PI, Math.PI); + absoluteEncoder = new DutyCycleEncoder(new DigitalInput(DIOPort), 2 * Math.PI, 0.5); var turnConfig = new SparkMaxConfig(); @@ -53,7 +54,6 @@ public TurretIOSparkAlternate() { turnConfig .encoder - .inverted(false) .positionConversionFactor(encoderPositionFactor) .velocityConversionFactor(encoderVelocityFactor); @@ -76,7 +76,9 @@ public TurretIOSparkAlternate() { turnSpark.configure( turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - turnSparkEncoder.setPosition(absoluteEncoder.get()); + // tryUntilOk(turnSpark, 5, () -> turnSparkEncoder.setPosition(absoluteEncoder.get())); + + ifOk(turnSpark, absoluteEncoder::get, (value) -> turnSparkEncoder.setPosition(value)); } @Override @@ -105,6 +107,7 @@ public void updateInputs(TurretIOInputs inputs) { (values) -> inputs.appliedVolts = values[0] * values[1]); ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.currentAmps = value); inputs.connected = turnConnectedDebounce.calculate(!sparkStickyFault); + Logger.recordOutput("Turret/AbsoluteEncoder", absoluteEncoder.get()); } @Override From c5754dd16944b340b0f9aa4c87f6c31126dff3a5 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Tue, 3 Feb 2026 20:49:35 -0500 Subject: [PATCH 3/9] attempted to add a way to reset the encoder of the turret --- src/main/java/frc/robot/Robot.java | 4 +++- .../frc/robot/subsystems/launcher/Launcher.java | 16 ++++++++++++++++ .../frc/robot/subsystems/launcher/TurretIO.java | 6 ++++++ .../launcher/TurretIOSparkAlternate.java | 12 ++++++++++-- 4 files changed, 35 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0fb966d..55d3089 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -204,7 +204,9 @@ public Robot() { launcher.setDefaultCommand( Commands.run(() -> launcher.aim(GameState.getMyHubPose().getTranslation()), launcher) - .withName("Aim at hub")); + .withName("Aim at hub") + .beforeStarting(launcher::initializeTurret) + .ignoringDisable(true)); } /** This function is called periodically during all modes. */ diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index c00e959..9b3a43f 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -16,6 +16,8 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.game.GameState; import frc.robot.Robot; @@ -332,4 +334,18 @@ public Translation3d[] getBallTrajectory(ArrayList traj) { } return t; } + + public Command initializeTurret() { + return new FunctionalCommand( + // initialize + () -> {}, + // execute + () -> {}, + // end + interrupted -> turretIO.resetEncoder(), + // isFinished + () -> turretIO.isAbsoluteEncoderConnected(), + // requirements + this); + } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java index 32dfe86..94090a5 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java @@ -19,4 +19,10 @@ public default void updateInputs(TurretIOInputs inputs) {} public default void setOpenLoop(double output) {} public default void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {} + + public default void resetEncoder() {} + + public default boolean isAbsoluteEncoderConnected() { + return false; + } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java index 31dcbf9..0c9d198 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java @@ -77,8 +77,6 @@ public TurretIOSparkAlternate() { turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); // tryUntilOk(turnSpark, 5, () -> turnSparkEncoder.setPosition(absoluteEncoder.get())); - - ifOk(turnSpark, absoluteEncoder::get, (value) -> turnSparkEncoder.setPosition(value)); } @Override @@ -129,4 +127,14 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { / maxAngularVelocity.in(RadiansPerSecond); controller.setSetpoint(setpoint); } + + @Override + public void resetEncoder() { + ifOk(turnSpark, absoluteEncoder::get, (value) -> turnSparkEncoder.setPosition(value)); + } + + @Override + public boolean isAbsoluteEncoderConnected() { + return absoluteEncoder.isConnected(); + } } From b7a98a02c61ddaa133800035e9c4da6d42793b2c Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 5 Feb 2026 18:27:13 -0500 Subject: [PATCH 4/9] move initialize command into robotInit --- src/main/java/frc/robot/Robot.java | 10 ++++++--- .../robot/subsystems/launcher/Launcher.java | 22 ++++++++++--------- 2 files changed, 19 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 55d3089..30d2477 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -204,9 +204,7 @@ public Robot() { launcher.setDefaultCommand( Commands.run(() -> launcher.aim(GameState.getMyHubPose().getTranslation()), launcher) - .withName("Aim at hub") - .beforeStarting(launcher::initializeTurret) - .ignoringDisable(true)); + .withName("Aim at hub")); } /** This function is called periodically during all modes. */ @@ -233,7 +231,13 @@ public void robotPeriodic() { // Threads.setCurrentThreadPriority(false, 10); } + @Override + public void robotInit() { + launcher.initializeTurret().schedule(); + } + /** This function is called once when the robot is disabled. */ + @Override public void disabledInit() {} /** This function is called periodically when disabled. */ diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index 9b3a43f..88e0a56 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -337,15 +337,17 @@ public Translation3d[] getBallTrajectory(ArrayList traj) { public Command initializeTurret() { return new FunctionalCommand( - // initialize - () -> {}, - // execute - () -> {}, - // end - interrupted -> turretIO.resetEncoder(), - // isFinished - () -> turretIO.isAbsoluteEncoderConnected(), - // requirements - this); + // initialize + () -> {}, + // execute + () -> {}, + // end + interrupted -> turretIO.resetEncoder(), + // isFinished + () -> turretIO.isAbsoluteEncoderConnected(), + // requirements + this) + .ignoringDisable(true) + .withName("Initialize turret"); } } From de991e485e8b68ae439f6701a32b8ee747b871c6 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Thu, 5 Feb 2026 18:45:51 -0500 Subject: [PATCH 5/9] cleaning up code --- src/main/java/frc/robot/Robot.java | 12 +++++----- .../robot/subsystems/launcher/Launcher.java | 22 +++++++++---------- .../robot/subsystems/launcher/TurretIO.java | 7 +++--- .../launcher/TurretIOSparkAlternate.java | 10 +++------ 4 files changed, 23 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 30d2477..2bdca75 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.game.Field; import frc.game.GameState; @@ -207,6 +208,12 @@ public Robot() { .withName("Aim at hub")); } + @Override + public void robotInit() { + CommandScheduler.getInstance() + .schedule(launcher.initializeTurret().andThen(new WaitCommand(3.0))); + } + /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { @@ -231,11 +238,6 @@ public void robotPeriodic() { // Threads.setCurrentThreadPriority(false, 10); } - @Override - public void robotInit() { - launcher.initializeTurret().schedule(); - } - /** This function is called once when the robot is disabled. */ @Override public void disabledInit() {} diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index 88e0a56..0f07c6f 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -337,17 +337,15 @@ public Translation3d[] getBallTrajectory(ArrayList traj) { public Command initializeTurret() { return new FunctionalCommand( - // initialize - () -> {}, - // execute - () -> {}, - // end - interrupted -> turretIO.resetEncoder(), - // isFinished - () -> turretIO.isAbsoluteEncoderConnected(), - // requirements - this) - .ignoringDisable(true) - .withName("Initialize turret"); + // initialize + () -> {}, + // execute + () -> {}, + // end + interrupted -> turretIO.resetEncoder(), + // isFinished + () -> turretInputs.absoluteEncoderConnected, + // requirements + this); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java index 94090a5..725d986 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java @@ -12,6 +12,9 @@ public static class TurretIOInputs { public double velocityRadPerSec = 0.0; public double appliedVolts = 0.0; public double currentAmps = 0.0; + + public boolean absoluteEncoderConnected = false; + public Rotation2d absolutePosition = Rotation2d.kZero; } public default void updateInputs(TurretIOInputs inputs) {} @@ -21,8 +24,4 @@ public default void setOpenLoop(double output) {} public default void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {} public default void resetEncoder() {} - - public default boolean isAbsoluteEncoderConnected() { - return false; - } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java index 0c9d198..a17cc2d 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java @@ -23,7 +23,6 @@ import frc.robot.Constants.MotorConstants.NEO550Constants; import frc.robot.Constants.RobotConstants; import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.Logger; public class TurretIOSparkAlternate implements TurretIO { @@ -105,7 +104,9 @@ public void updateInputs(TurretIOInputs inputs) { (values) -> inputs.appliedVolts = values[0] * values[1]); ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.currentAmps = value); inputs.connected = turnConnectedDebounce.calculate(!sparkStickyFault); - Logger.recordOutput("Turret/AbsoluteEncoder", absoluteEncoder.get()); + + inputs.absoluteEncoderConnected = absoluteEncoder.isConnected(); + inputs.absolutePosition = new Rotation2d(absoluteEncoder.get()); } @Override @@ -132,9 +133,4 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { public void resetEncoder() { ifOk(turnSpark, absoluteEncoder::get, (value) -> turnSparkEncoder.setPosition(value)); } - - @Override - public boolean isAbsoluteEncoderConnected() { - return absoluteEncoder.isConnected(); - } } From 8ea6bb1b2a4266070d6519b37df71d9a8fd0e683 Mon Sep 17 00:00:00 2001 From: DylanTaylor29 <132412179+DylanTaylor29@users.noreply.github.com> Date: Thu, 5 Feb 2026 18:54:10 -0500 Subject: [PATCH 6/9] fixed an issue where the absolute encoder and relative encoder on the turret would not initialize at the same value as instructed to by code --- src/main/java/frc/robot/Robot.java | 4 ++-- .../robot/subsystems/launcher/TurretIOSparkAlternate.java | 8 ++------ 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2bdca75..908e9a4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -210,8 +210,8 @@ public Robot() { @Override public void robotInit() { - CommandScheduler.getInstance() - .schedule(launcher.initializeTurret().andThen(new WaitCommand(3.0))); + // CommandScheduler.getInstance() + // .schedule(launcher.initializeTurret().andThen(new WaitCommand(3.0))); } /** This function is called periodically during all modes. */ diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java index a17cc2d..b4e1a02 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java @@ -60,10 +60,6 @@ public TurretIOSparkAlternate() { turnConfig .signals - .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs(20) - .absoluteEncoderVelocityAlwaysOn(true) - .absoluteEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) .busVoltagePeriodMs(20) .outputCurrentPeriodMs(20); @@ -75,7 +71,7 @@ public TurretIOSparkAlternate() { turnSpark.configure( turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - // tryUntilOk(turnSpark, 5, () -> turnSparkEncoder.setPosition(absoluteEncoder.get())); + tryUntilOk(turnSpark, 5, () -> turnSparkEncoder.setPosition(absoluteEncoder.get())); } @Override @@ -131,6 +127,6 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { @Override public void resetEncoder() { - ifOk(turnSpark, absoluteEncoder::get, (value) -> turnSparkEncoder.setPosition(value)); + // ifOk(turnSpark, absoluteEncoder::get, (value) -> turnSparkEncoder.setPosition(value)); } } From bce01b6c548b0fad25ca91ea27d2a32ac851a678 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 5 Feb 2026 19:18:31 -0500 Subject: [PATCH 7/9] initializing relative encoder works --- src/main/java/frc/robot/Robot.java | 7 ------- .../robot/subsystems/launcher/Launcher.java | 16 ---------------- .../robot/subsystems/launcher/TurretIO.java | 2 -- .../launcher/TurretIOSparkAlternate.java | 19 +++++++------------ 4 files changed, 7 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 908e9a4..df3637d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.game.Field; import frc.game.GameState; @@ -208,12 +207,6 @@ public Robot() { .withName("Aim at hub")); } - @Override - public void robotInit() { - // CommandScheduler.getInstance() - // .schedule(launcher.initializeTurret().andThen(new WaitCommand(3.0))); - } - /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index 0f07c6f..c00e959 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -16,8 +16,6 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.game.GameState; import frc.robot.Robot; @@ -334,18 +332,4 @@ public Translation3d[] getBallTrajectory(ArrayList traj) { } return t; } - - public Command initializeTurret() { - return new FunctionalCommand( - // initialize - () -> {}, - // execute - () -> {}, - // end - interrupted -> turretIO.resetEncoder(), - // isFinished - () -> turretInputs.absoluteEncoderConnected, - // requirements - this); - } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java index 725d986..88c32df 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java @@ -22,6 +22,4 @@ public default void updateInputs(TurretIOInputs inputs) {} public default void setOpenLoop(double output) {} public default void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {} - - public default void resetEncoder() {} } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java index b4e1a02..9b407c4 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java @@ -36,6 +36,7 @@ public class TurretIOSparkAlternate implements TurretIO { private boolean closedLoop = false; private double appliedVolts = 0.0; private double feedforwardVolts = 0.0; + private boolean seeded = false; public TurretIOSparkAlternate() { turnSpark = new SparkMax(port, MotorType.kBrushless); @@ -58,11 +59,7 @@ public TurretIOSparkAlternate() { turnConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPReal, 0.0, 0.0); - turnConfig - .signals - .appliedOutputPeriodMs(20) - .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); + turnConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); tryUntilOk( turnSpark, @@ -70,12 +67,15 @@ public TurretIOSparkAlternate() { () -> turnSpark.configure( turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - - tryUntilOk(turnSpark, 5, () -> turnSparkEncoder.setPosition(absoluteEncoder.get())); } @Override public void updateInputs(TurretIOInputs inputs) { + if (!seeded && absoluteEncoder.isConnected()) { + turnSparkEncoder.setPosition(absoluteEncoder.get()); + seeded = true; + } + // Run closed-loop control if (closedLoop) { appliedVolts = controller.calculate(turnSparkEncoder.getPosition()) + feedforwardVolts; @@ -124,9 +124,4 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { / maxAngularVelocity.in(RadiansPerSecond); controller.setSetpoint(setpoint); } - - @Override - public void resetEncoder() { - // ifOk(turnSpark, absoluteEncoder::get, (value) -> turnSparkEncoder.setPosition(value)); - } } From c48a5f43e4151e50a53a2c0d0eae58d20f4a262b Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 5 Feb 2026 20:45:14 -0500 Subject: [PATCH 8/9] offset hard stop --- .../frc/robot/subsystems/launcher/LauncherConstants.java | 2 +- .../robot/subsystems/launcher/TurretIOSparkAlternate.java | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 3031cb2..56a80b2 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -39,7 +39,7 @@ public static final class TurretConstants { // Position controller public static final double minInput = 0.0; public static final double maxInput = 2 * Math.PI; - public static final double kPReal = 0.35; + public static final double kPReal = 0.5; // Motor controller public static final int port = 12; diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java index 9b407c4..2cb540a 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java @@ -37,12 +37,14 @@ public class TurretIOSparkAlternate implements TurretIO { private double appliedVolts = 0.0; private double feedforwardVolts = 0.0; private boolean seeded = false; + private Rotation2d offset = Rotation2d.kCCW_Pi_2; public TurretIOSparkAlternate() { turnSpark = new SparkMax(port, MotorType.kBrushless); turnSparkEncoder = turnSpark.getEncoder(); controller = new PIDController(kPReal, 0.0, 0.0); - absoluteEncoder = new DutyCycleEncoder(new DigitalInput(DIOPort), 2 * Math.PI, 0.5); + absoluteEncoder = + new DutyCycleEncoder(new DigitalInput(DIOPort), 2 * Math.PI, 0.5 + offset.getRadians()); var turnConfig = new SparkMaxConfig(); @@ -92,7 +94,7 @@ public void updateInputs(TurretIOInputs inputs) { ifOk( turnSpark, turnSparkEncoder::getPosition, - (value) -> inputs.position = new Rotation2d(value)); + (value) -> inputs.position = new Rotation2d(value).plus(offset)); ifOk(turnSpark, turnSparkEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); ifOk( turnSpark, @@ -117,7 +119,7 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = // Math.max(minAngle, Math.min(maxAngle, rotation.getRadians())) + // rotationOffset.getRadians(); - MathUtil.inputModulus(rotation.getRadians(), minInput, maxInput); + MathUtil.inputModulus(rotation.getRadians() - offset.getRadians(), minInput, maxInput); this.feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) From 12eb8ab16bbcaf1d146cad444539621f4151cbad Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Sat, 7 Feb 2026 09:35:03 -0500 Subject: [PATCH 9/9] renaming & reorganizing --- src/main/java/frc/robot/Robot.java | 4 +- .../robot/subsystems/launcher/Launcher.java | 7 +- .../launcher/LauncherConstants.java | 3 +- .../robot/subsystems/launcher/TurretIO.java | 4 +- .../subsystems/launcher/TurretIOSim.java | 4 +- .../subsystems/launcher/TurretIOSpark.java | 102 +++++++------- .../launcher/TurretIOSparkAlternate.java | 129 ------------------ 7 files changed, 68 insertions(+), 185 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index df3637d..dd60c42 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,7 +41,7 @@ import frc.robot.subsystems.launcher.Launcher; import frc.robot.subsystems.launcher.TurretIO; import frc.robot.subsystems.launcher.TurretIOSim; -import frc.robot.subsystems.launcher.TurretIOSparkAlternate; +import frc.robot.subsystems.launcher.TurretIOSpark; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhotonVision; @@ -118,7 +118,7 @@ public Robot() { new Launcher( drive::getPose, drive::getRobotRelativeChassisSpeeds, - new TurretIOSparkAlternate(), + new TurretIOSpark(), new FlywheelIOSim() {}, new HoodIOSim()); break; diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index c00e959..ad9b4d3 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -79,7 +79,7 @@ public void periodic() { Logger.processInputs("Flywheel", flywheelInputs); Logger.processInputs("Hood", hoodInputs); - turretDisconnectedAlert.set(!turretInputs.connected); + turretDisconnectedAlert.set(!turretInputs.motorControllerConnected); flywheelDisconnectedAlert.set(!flywheelInputs.connected); hoodDisconnectedAlert.set(!hoodInputs.connected); @@ -141,7 +141,8 @@ public void aim(Translation3d target) { // Get actual hood & turret position Rotation2d hoodPosition = hoodInputs.position; - Rotation2d turretPosition = turretInputs.position.plus(turretBasePose.toPose2d().getRotation()); + Rotation2d turretPosition = + turretInputs.relativePosition.plus(turretBasePose.toPose2d().getRotation()); // Build actual velocities Translation3d v0_actual = @@ -180,7 +181,7 @@ public void aim(Translation3d target) { @AutoLogOutput(key = "Launcher/TurretPose") public Pose2d getTurretPose() { - return turretBasePose.toPose2d().plus(new Transform2d(0, 0, turretInputs.position)); + return turretBasePose.toPose2d().plus(new Transform2d(0, 0, turretInputs.relativePosition)); } // @AutoLogOutput(key = "Turret/IsOnTarget") diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 56a80b2..7ec5330 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -30,7 +30,8 @@ public static final class TurretConstants { // Geometry public static final Transform3d chassisToTurretBase = new Transform3d(Inches.of(0), Inches.of(10), Inches.of(22), Rotation3d.kZero); - public static final Rotation2d rotationOffset = new Rotation2d(0.44); + public static final Rotation2d absEncoderOffset = new Rotation2d(0.5); + public static final Rotation2d mechanismOffset = Rotation2d.kCCW_Pi_2; // Absolute encoder public static final double encoderPositionFactor = (2 * Math.PI) / 5.0; // Radians diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java index 88c32df..14802c9 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java @@ -7,8 +7,8 @@ public interface TurretIO { @AutoLog public static class TurretIOInputs { - public boolean connected = false; - public Rotation2d position = Rotation2d.kZero; + public boolean motorControllerConnected = false; + public Rotation2d relativePosition = Rotation2d.kZero; public double velocityRadPerSec = 0.0; public double appliedVolts = 0.0; public double currentAmps = 0.0; diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java index d39e36c..1a8278a 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSim.java @@ -45,8 +45,8 @@ public void updateInputs(TurretIOInputs inputs) { turnSim.update(Robot.defaultPeriodSecs); // Update turn inputs - inputs.connected = true; - inputs.position = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.motorControllerConnected = true; + inputs.relativePosition = new Rotation2d(turnSim.getAngularPositionRad()); inputs.velocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.appliedVolts = appliedVolts; inputs.currentAmps = Math.abs(turnSim.getCurrentDrawAmps()); diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index 8273d08..07e5198 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -4,22 +4,22 @@ import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; import static frc.robot.util.SparkUtil.*; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; -import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import frc.robot.Constants.MotorConstants.NEO550Constants; import frc.robot.Constants.RobotConstants; import java.util.function.DoubleSupplier; @@ -27,15 +27,26 @@ public class TurretIOSpark implements TurretIO { private final SparkBase turnSpark; - private final AbsoluteEncoder turnEncoder; - private final SparkClosedLoopController turnController; + private final RelativeEncoder turnSparkEncoder; + private final PIDController controller; + private final DutyCycleEncoder absoluteEncoder; private final Debouncer turnConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private boolean closedLoop = false; + private double appliedVolts = 0.0; + private double feedforwardVolts = 0.0; + private boolean seeded = false; + public TurretIOSpark() { turnSpark = new SparkMax(port, MotorType.kBrushless); - turnEncoder = turnSpark.getAbsoluteEncoder(); - turnController = turnSpark.getClosedLoopController(); + turnSparkEncoder = turnSpark.getEncoder(); + controller = new PIDController(kPReal, 0.0, 0.0); + absoluteEncoder = + new DutyCycleEncoder( + new DigitalInput(DIOPort), + 2 * Math.PI, + absEncoderOffset.getRadians() + mechanismOffset.getRadians()); var turnConfig = new SparkMaxConfig(); @@ -46,37 +57,13 @@ public TurretIOSpark() { .voltageCompensation(RobotConstants.kNominalVoltage); turnConfig - .absoluteEncoder - .inverted(false) - .zeroCentered(false) - .zeroOffset(rotationOffset.getRotations()) + .encoder .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor) - .averageDepth(2); + .velocityConversionFactor(encoderVelocityFactor); - turnConfig - .closedLoop - .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) - .positionWrappingEnabled(true) - .positionWrappingInputRange(0, 2 * Math.PI) - .pid(kPReal, 0.0, 0.0); - - // turnConfig - // .softLimit - // .forwardSoftLimitEnabled(false) - // .forwardSoftLimit(maxAngle) - // .reverseSoftLimitEnabled(true) - // .reverseSoftLimit(minAngle); + turnConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPReal, 0.0, 0.0); - turnConfig - .signals - .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs(20) - .absoluteEncoderVelocityAlwaysOn(true) - .absoluteEncoderVelocityPeriodMs(20) - .appliedOutputPeriodMs(20) - .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); + turnConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); tryUntilOk( turnSpark, @@ -88,33 +75,56 @@ public TurretIOSpark() { @Override public void updateInputs(TurretIOInputs inputs) { + if (!seeded && absoluteEncoder.isConnected()) { + turnSparkEncoder.setPosition(absoluteEncoder.get()); + seeded = true; + } + + // Run closed-loop control + if (closedLoop) { + appliedVolts = controller.calculate(turnSparkEncoder.getPosition()) + feedforwardVolts; + } else { + controller.reset(); + } + + // Update state + turnSpark.setVoltage( + MathUtil.clamp( + appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + sparkStickyFault = false; - ifOk(turnSpark, turnEncoder::getPosition, (value) -> inputs.position = new Rotation2d(value)); - ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); + ifOk( + turnSpark, + turnSparkEncoder::getPosition, + (value) -> inputs.relativePosition = new Rotation2d(value).plus(mechanismOffset)); + ifOk(turnSpark, turnSparkEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); ifOk( turnSpark, new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, (values) -> inputs.appliedVolts = values[0] * values[1]); ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.currentAmps = value); - inputs.connected = turnConnectedDebounce.calculate(!sparkStickyFault); + inputs.motorControllerConnected = turnConnectedDebounce.calculate(!sparkStickyFault); + + inputs.absoluteEncoderConnected = absoluteEncoder.isConnected(); + inputs.absolutePosition = new Rotation2d(absoluteEncoder.get()); } @Override public void setOpenLoop(double output) { - turnSpark.setVoltage(output); + closedLoop = false; + appliedVolts = output; } @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { + closedLoop = true; double setpoint = - // Math.max(minAngle, Math.min(maxAngle, rotation.getRadians())) + - // rotationOffset.getRadians(); - MathUtil.inputModulus(rotation.getRadians(), minInput, maxInput); - double feedforwardVolts = + MathUtil.inputModulus( + rotation.getRadians() - mechanismOffset.getRadians(), minInput, maxInput); + this.feedforwardVolts = RobotConstants.kNominalVoltage * angularVelocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); - turnController.setSetpoint( - setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); + controller.setSetpoint(setpoint); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java deleted file mode 100644 index 2cb540a..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSparkAlternate.java +++ /dev/null @@ -1,129 +0,0 @@ -package frc.robot.subsystems.launcher; - -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; -import static frc.robot.util.SparkUtil.*; - -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import frc.robot.Constants.MotorConstants.NEO550Constants; -import frc.robot.Constants.RobotConstants; -import java.util.function.DoubleSupplier; - -public class TurretIOSparkAlternate implements TurretIO { - - private final SparkBase turnSpark; - private final RelativeEncoder turnSparkEncoder; - private final PIDController controller; - private final DutyCycleEncoder absoluteEncoder; - private final Debouncer turnConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); - - private boolean closedLoop = false; - private double appliedVolts = 0.0; - private double feedforwardVolts = 0.0; - private boolean seeded = false; - private Rotation2d offset = Rotation2d.kCCW_Pi_2; - - public TurretIOSparkAlternate() { - turnSpark = new SparkMax(port, MotorType.kBrushless); - turnSparkEncoder = turnSpark.getEncoder(); - controller = new PIDController(kPReal, 0.0, 0.0); - absoluteEncoder = - new DutyCycleEncoder(new DigitalInput(DIOPort), 2 * Math.PI, 0.5 + offset.getRadians()); - - var turnConfig = new SparkMaxConfig(); - - turnConfig - .inverted(false) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); - - turnConfig - .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor); - - turnConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPReal, 0.0, 0.0); - - turnConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); - - tryUntilOk( - turnSpark, - 5, - () -> - turnSpark.configure( - turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - } - - @Override - public void updateInputs(TurretIOInputs inputs) { - if (!seeded && absoluteEncoder.isConnected()) { - turnSparkEncoder.setPosition(absoluteEncoder.get()); - seeded = true; - } - - // Run closed-loop control - if (closedLoop) { - appliedVolts = controller.calculate(turnSparkEncoder.getPosition()) + feedforwardVolts; - } else { - controller.reset(); - } - - // Update state - turnSpark.setVoltage( - MathUtil.clamp( - appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); - - sparkStickyFault = false; - ifOk( - turnSpark, - turnSparkEncoder::getPosition, - (value) -> inputs.position = new Rotation2d(value).plus(offset)); - ifOk(turnSpark, turnSparkEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); - ifOk( - turnSpark, - new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage}, - (values) -> inputs.appliedVolts = values[0] * values[1]); - ifOk(turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.currentAmps = value); - inputs.connected = turnConnectedDebounce.calculate(!sparkStickyFault); - - inputs.absoluteEncoderConnected = absoluteEncoder.isConnected(); - inputs.absolutePosition = new Rotation2d(absoluteEncoder.get()); - } - - @Override - public void setOpenLoop(double output) { - closedLoop = false; - appliedVolts = output; - } - - @Override - public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - closedLoop = true; - double setpoint = - // Math.max(minAngle, Math.min(maxAngle, rotation.getRadians())) + - // rotationOffset.getRadians(); - MathUtil.inputModulus(rotation.getRadians() - offset.getRadians(), minInput, maxInput); - this.feedforwardVolts = - RobotConstants.kNominalVoltage - * angularVelocity.in(RadiansPerSecond) - / maxAngularVelocity.in(RadiansPerSecond); - controller.setSetpoint(setpoint); - } -}