From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Tue, 24 Feb 2026 01:13:01 +0000 (-0800) Subject: turret calibration X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=550f973db27e396d7909e6b9d6241a0683cce61f;p=FRC2026.git turret calibration --- diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index d4992d3..d4cb2a9 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -30,10 +30,6 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { limitConfig.SupplyCurrentLowerLimit = SpindexerConstants.currentLimit; limitConfig.SupplyCurrentLowerTime = 1.5; motor.getConfigurator().apply(limitConfig); - - SmartDashboard.putData("Max speed spindexer", new InstantCommand(() -> maxSpindexer())); - SmartDashboard.putData("Turn off spindexer", new InstantCommand(() -> stopSpindexer())); - SmartDashboard.putData("Spindexer 50%", new InstantCommand(() -> setSpindexer(0.5))); } @Override @@ -45,7 +41,6 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { // scale threshold based on power double velocityThreshold = SpindexerConstants.spindexerVelocityWithBall * power; - SmartDashboard.putNumber("Spindexer Velocity Threshold", velocityThreshold); SmartDashboard.putNumber("Spindexer Ball Count", ballCount); boolean isSpindexerSlow = inputs.spindexerVelocity < velocityThreshold; diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 8ac2b12..363927b 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -2,6 +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.CANcoder; @@ -20,6 +21,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; 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.Constants; import frc.robot.constants.IdConstants; @@ -31,6 +33,8 @@ public class Turret extends SubsystemBase implements TurretIO{ private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); + private boolean calibrating; + /* ---------------- Hardware ---------------- */ private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.CANIVORE_SUB); @@ -75,6 +79,8 @@ public class Turret extends SubsystemBase implements TurretIO{ 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()) { @@ -203,10 +209,17 @@ public class Turret extends SubsystemBase implements TurretIO{ // Multiply goal velocity by kV double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV; - // Sets motor control with feedforward - motor.setControl(mmVoltageRequest + if(calibrating){ + motor.set(0.05); + if(Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= TurretConstants.CALIBRATION_CURRENT_THRESHOLD){ + stopCalibrating(); + } + } else{ + // Sets motor control with feedforward + motor.setControl(mmVoltageRequest .withPosition(motorGoalRotations) .withFeedForward(robotTurnCompensation)); + } Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue()); Logger.recordOutput("Turret/setpointDeg", goalAngle.getDegrees()); @@ -221,7 +234,8 @@ public class Turret extends SubsystemBase implements TurretIO{ SmartDashboard.putNumber("Encoder left position", encoderLeft.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Encoder right position", encoderRight.getAbsolutePosition().getValueAsDouble()); - + SmartDashboard.putData("Start turret calibration", new InstantCommand(()-> calibrate())); + SmartDashboard.putData("Stop turret calibration", new InstantCommand(()-> stopCalibrating())); } /* ---------------- Simulation ---------------- */ @@ -248,10 +262,41 @@ public class Turret extends SubsystemBase implements TurretIO{ 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); + } + // Also ignore this for now private double wrapUnit(double value) { value %= 1.0; if (value < 0) value += 1.0; return value; } + + private void calibrate(){ + setCurrentLimits(TurretConstants.CALIBRATION_CURRENT_LIMIT); + calibrating = true; + } + + private void stopCalibrating(){ + motor.set(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/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 44425c0..83dd1c3 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -7,6 +7,8 @@ public class TurretConstants { public static double MAX_ANGLE = 200; // Deg public static double MIN_ANGLE = -200; // Deg + public static double CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop + public static double MAX_VELOCITY = 600; // rad/s public static double MAX_ACCELERATION = 120.0; // rad/s^2 @@ -31,4 +33,9 @@ public class TurretConstants { public static final double EXTRAPOLATION_TIME_CONSTANT = 0.06; public static final double FEEDFORWARD_KV = 0.185; + + public static final double NORMAL_CURRENT_LIMIT = 50.0; // A + public static final double CALIBRATION_CURRENT_LIMIT = 10.0; // A + public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A + }