From: moo Date: Mon, 13 Apr 2026 21:17:04 +0000 (-0700) Subject: turret advantagekit stuff X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=d62c29e640547e6e8d686d14e73580c7c704df88;p=FRC2026.git turret advantagekit stuff --- diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 6b05330..3f907b1 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -2,12 +2,7 @@ package frc.robot.subsystems.turret; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.MathUtil; @@ -16,7 +11,6 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.constants.Constants; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; @@ -26,24 +20,23 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.IdConstants; import frc.robot.util.ModifiedCRT; -public class Turret extends SubsystemBase implements TurretIO{ +public class Turret extends SubsystemBase { // Super low magnitude filter for the position to make it less jittery private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02); - private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); + private TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); - public boolean locked = false; + private TurretIO io; + + public boolean locked = false; private boolean calibrating; private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising); /* ---------------- Hardware ---------------- */ - private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.CANIVORE_SUB); - private TalonFXSimState simState; private SingleJointedArmSim turretSim; @@ -55,8 +48,6 @@ public class Turret extends SubsystemBase implements TurretIO{ private double lastFilteredRad = 0.0; private double lastRawSetpoint = 0.0; - - /* ---------------- Visualization ---------------- */ private final Mechanism2d mech = new Mechanism2d(100, 100); @@ -70,64 +61,44 @@ public class Turret extends SubsystemBase implements TurretIO{ /* ---------------- Constructor ---------------- */ public Turret() { - motor.setNeutralMode(NeutralModeValue.Brake); - - TalonFXConfiguration config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - config.Slot0.kP = 12.0; - config.Slot0.kS = 0.1; // Static friction compensation - config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio - config.Slot0.kD = 0.0; // The "Braking" term to stop overshoot - - var mm = config.MotionMagic; - mm.MotionMagicCruiseVelocity = Units.radiansToRotations(TurretConstants.MAX_VELOCITY) * TurretConstants.GEAR_RATIO; - mm.MotionMagicAcceleration = Units.radiansToRotations(TurretConstants.MAX_ACCELERATION) * TurretConstants.GEAR_RATIO; // Lowered for belt safety - mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed - motor.getConfigurator().apply(config); setCurrentLimits(TurretConstants.NORMAL_CURRENT_LIMIT); lastGoalRad = 0.0; - if (RobotBase.isSimulation()) { - simState = motor.getSimState(); - turretSim = new SingleJointedArmSim( - edu.wpi.first.math.system.plant.DCMotor.getKrakenX60(1), - TurretConstants.GEAR_RATIO, - 0.01, - 0.15, - Units.degreesToRadians(TurretConstants.MIN_ANGLE), - Units.degreesToRadians(TurretConstants.MAX_ANGLE), - false, - 0.0); - } - if (!Constants.DISABLE_SMART_DASHBOARD) { SmartDashboard.putData("Turret Mech", mech); - SmartDashboard.putData("Start turret calibration", new InstantCommand(()-> calibrate())); - SmartDashboard.putData("Stop turret calibration", new InstantCommand(()-> stopCalibrating())); - SmartDashboard.putData("Reset Turret Position to Zero", new InstantCommand(() -> resetTurretPosition())); + SmartDashboard.putData("Start turret calibration", new InstantCommand(() -> calibrate())); + SmartDashboard.putData("Stop turret calibration", new InstantCommand(() -> stopCalibrating())); SendableChooser turretTestChooser = new SendableChooser<>(); - turretTestChooser.setDefaultOption("Turn to 0", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0))); - turretTestChooser.addOption("Turn to -90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0))); - turretTestChooser.addOption("Turn to 90", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0))); - turretTestChooser.addOption("Turn to 200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0))); - turretTestChooser.addOption("Turn to -200", new InstantCommand(()-> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0))); + turretTestChooser.setDefaultOption("Turn to 0", + new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0))); + turretTestChooser.addOption("Turn to -90", + new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0))); + turretTestChooser.addOption("Turn to 90", + new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0))); + turretTestChooser.addOption("Turn to 200", + new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(200), 0.0))); + turretTestChooser.addOption("Turn to -200", + new InstantCommand(() -> setFieldRelativeTarget(Rotation2d.fromDegrees(-200), 0.0))); SmartDashboard.putData("Turret Test Positions", turretTestChooser); } - SmartDashboard.putData("Set Locked", new InstantCommand(() -> {locked = !locked;})); - //motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO); + SmartDashboard.putData("Set Locked", new InstantCommand(() -> { + locked = !locked; + })); + // motor.setPosition(Units.degreesToRotations(238.86) * + // TurretConstants.GEAR_RATIO); - motor.setPosition(0.0); } /* ---------------- Public API ---------------- */ /** - * Sets the setpoint position and velocity of the turret. Call in command execute. + * Sets the setpoint position and velocity of the turret. Call in command + * execute. + * * @param angle * @param velocityRadPerSec */ @@ -136,15 +107,12 @@ public class Turret extends SubsystemBase implements TurretIO{ goalVelocityRadPerSec = velocityRadPerSec; } - public void resetTurretPosition() { - inputs.positionDeg = 0.0; - } - /** * @return If the turret is at setpoint with tolerance of 10 degrees */ public boolean atSetpoint() { - if (locked) return true; + if (locked) + return true; return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(10.0); } @@ -152,34 +120,35 @@ public class Turret extends SubsystemBase implements TurretIO{ * @return Posiiton of the turret in radians */ public double getPositionRad() { - return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO; + return Units.degreesToRadians(inputs.positionDeg); } /** * @return Posiiton of the turret in degrees */ public double getPositionDeg() { - return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO; + return inputs.positionDeg; } /* ---------------- Periodic ---------------- */ @Override public void periodic() { - updateInputs(); + io.updateInputs(inputs); Logger.processInputs("Turret", inputs); // Position extrapolation - double lookAheadSeconds = TurretConstants.EXTRAPOLATION_TIME_CONSTANT; - double futureRobotAngle = goalAngle.getRadians() + (goalVelocityRadPerSec * lookAheadSeconds); + double lookAheadSeconds = TurretConstants.EXTRAPOLATION_TIME_CONSTANT; + double futureRobotAngle = goalAngle.getRadians() + (goalVelocityRadPerSec * lookAheadSeconds); - //Continuous wrap selection + // Continuous wrap selection double best = lastGoalRad; boolean found = false; for (int i = -2; i <= 2; i++) { double candidate = futureRobotAngle + 2.0 * Math.PI * i; - if (candidate < Units.degreesToRadians(TurretConstants.MIN_ANGLE) || candidate > Units.degreesToRadians(TurretConstants.MAX_ANGLE)) + if (candidate < Units.degreesToRadians(TurretConstants.MIN_ANGLE) + || candidate > Units.degreesToRadians(TurretConstants.MAX_ANGLE)) continue; if (!found || Math.abs(candidate - lastGoalRad) < Math.abs(best - lastGoalRad)) { @@ -192,10 +161,10 @@ public class Turret extends SubsystemBase implements TurretIO{ // calculate shortest angular delta double delta = best - lastRawSetpoint; - + // filter delta double filteredDelta = setpointFilter.calculate(delta); - + // apply filtered range lastFilteredRad += filteredDelta; lastRawSetpoint = best; @@ -205,29 +174,32 @@ public class Turret extends SubsystemBase implements TurretIO{ double motorGoalRotations = Units.radiansToRotations(best) * TurretConstants.GEAR_RATIO; // Clamp position setpoint to min and max angles - motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(TurretConstants.MIN_ANGLE) * TurretConstants.GEAR_RATIO, Units.degreesToRotations(TurretConstants.MAX_ANGLE) * TurretConstants.GEAR_RATIO); - + motorGoalRotations = MathUtil.clamp(motorGoalRotations, + Units.degreesToRotations(TurretConstants.MIN_ANGLE) * TurretConstants.GEAR_RATIO, + Units.degreesToRotations(TurretConstants.MAX_ANGLE) * TurretConstants.GEAR_RATIO); + // Multiply goal velocity by kV double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV * TurretConstants.GEAR_RATIO; - if(calibrating){ - motor.set(0.05); - boolean calibrated = Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= TurretConstants.CALIBRATION_CURRENT_THRESHOLD; - if(calibrationDebouncer.calculate(calibrated)){ + if (calibrating) { + io.setMotorRaw(0.05); + boolean calibrated = Math + .abs(inputs.motorCurrent) >= TurretConstants.CALIBRATION_CURRENT_THRESHOLD; + if (calibrationDebouncer.calculate(calibrated)) { stopCalibrating(); } } else { // Sets motor control with feedforward - motor.setControl(mmVoltageRequest - .withPosition(motorGoalRotations) - .withFeedForward(robotTurnCompensation) - .withEnableFOC(true)); + io.setControl(mmVoltageRequest + .withPosition(motorGoalRotations) + .withFeedForward(robotTurnCompensation) + .withEnableFOC(true)); } - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue()); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Turret/Voltage", inputs.motorVoltage); Logger.recordOutput("Turret/setpointDeg", goalAngle.getDegrees()); - } + } // --- Visualization --- ligament.setAngle(Units.radiansToDegrees(getPositionRad())); @@ -237,14 +209,14 @@ public class Turret extends SubsystemBase implements TurretIO{ SmartDashboard.putBoolean("Turret Calibrated", !calibrating); SmartDashboard.putBoolean("Turret At Setpoint", atSetpoint()); } - + } /* ---------------- Simulation ---------------- */ @Override public void simulationPeriodic() { - turretSim.setInputVoltage(motor.getMotorVoltage().getValueAsDouble()); + turretSim.setInputVoltage(inputs.motorVoltage); turretSim.update(Constants.LOOP_TIME); simState.setRawRotorPosition( @@ -254,47 +226,30 @@ public class Turret extends SubsystemBase implements TurretIO{ Units.radiansToRotations(turretSim.getVelocityRadPerSec()) * TurretConstants.GEAR_RATIO); } - @Override - public void updateInputs() { - inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO; - inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / TurretConstants.GEAR_RATIO; - inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); - inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble(); - } - /** - * sets supply and stator current limits - * @param limit the current limit for stator and supply current - */ - public void setCurrentLimits(double limit) { - CurrentLimitsConfigs limits = new CurrentLimitsConfigs() - .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(limit) - .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(limit); - - if(limit == TurretConstants.NORMAL_CURRENT_LIMIT){ - limits.SupplyCurrentLowerLimit = TurretConstants.CALIBRATION_CURRENT_LIMIT; - limits.SupplyCurrentLowerTime = 1.0; // Set to lower limit if over 1 second - } - - motor.getConfigurator().apply(limits); - } + * sets supply and stator current limits + * + * @param limit the current limit for stator and supply current + */ + public void setCurrentLimits(double limit) { + io.setCurrentLimits(limit); + } // Also ignore this for now private double wrapUnit(double value) { value %= 1.0; - if (value < 0) value += 1.0; + if (value < 0) + value += 1.0; return value; } - private void calibrate(){ + private void calibrate() { setCurrentLimits(TurretConstants.CALIBRATION_CURRENT_LIMIT); calibrating = true; } - private void stopCalibrating(){ - motor.set(Units.degreesToRotations(TurretConstants.CALIBRATION_OFFSET) * TurretConstants.GEAR_RATIO); + private void stopCalibrating() { + io.setMotorRaw(Units.degreesToRotations(TurretConstants.CALIBRATION_OFFSET) * TurretConstants.GEAR_RATIO); setCurrentLimits(TurretConstants.NORMAL_CURRENT_LIMIT); calibrating = false; setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(TurretConstants.CALIBRATION_OFFSET)), 0.0); diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java index a7bd928..87b3990 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java @@ -2,16 +2,27 @@ package frc.robot.subsystems.turret; import org.littletonrobotics.junction.AutoLog; +import com.ctre.phoenix6.controls.ControlRequest; + public interface TurretIO { - @AutoLog - public static class TurretIOInputs{ - public double positionDeg = 0; - public double velocityRadPerSec = 0; - public double motorCurrent = 0; - public double encoderLeftRot = 0; - public double encoderRightRot = 0; - public double motorVoltage = 0; - } + @AutoLog + public static class TurretIOInputs { + public double positionDeg = 0; + public double velocityRadPerSec = 0; + public double motorCurrent = 0; + public double motorVoltage = 0; + } + + public void updateInputs(TurretIOInputs inputs); + + public void setMotorRaw(double speed); + + public void setControl(ControlRequest request); - public void updateInputs(); + /** + * sets supply and stator current limits + * + * @param limit the current limit for stator and supply current + */ + public void setCurrentLimits(double limit); }