From: moo Date: Sun, 12 Apr 2026 21:26:27 +0000 (-0700) Subject: remove climb, rewrite hood with talonfx io for advantagekit X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=cf7bc1d2e8024617a3492f5e30ac14ebb5b262c4;p=FRC2026.git remove climb, rewrite hood with talonfx io for advantagekit --- diff --git a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java deleted file mode 100644 index 169c32c..0000000 --- a/src/main/java/frc/robot/subsystems/Climb/ClimbConstants.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.subsystems.Climb; - - -import edu.wpi.first.math.util.Units; - -public class ClimbConstants { - - // CHANGE LATER - // gear ratio for converting motor rotations to linear distance - public final static double CLIMB_GEAR_RATIO = 45.0; - // TODO: Get actual winch bobbin radius measurement - /** Winch spool radius in meters */ - public final static double WHEEL_RADIUS = Units.inchesToMeters(0.5); - /** climber stowed ? position in meters */ - public final static double BOTTOM_POSITION = Units.inchesToMeters(8); - /** Calibration position: lower than BOTTOM_POSITION to reduce motor strain */ - // public final static double CALIBRATION_POSITION = Units.inchesToMeters(8.5); - /** position that should put the robot off the ground? in meters. 6 inches */ - public final static double CLIMB_POSITION = Units.inchesToMeters(6); - public final static double UP_POSITION = 0.0; - - // current limits (in amps) - // CALIBRATION: Low current while finding hardstop to prevent damage - // NORMAL: Moderate current for PID-controlled movement - // CLIMB: High current for full-power climbing - public final static double CALIBRATION_CURRENT = 20.0; - public final static double CLIMB_CURRENT = 42.0; - public final static double CALIBRATION_CURRENT_THRESHOLD = 18.0; - - // PID Constants - // TODO: what are the units? Inches? Meters? - public final static double PID_P = 0.1; - public final static double PID_I = 0.0; - public final static double PID_D = 0.0; - public final static double PID_TOLERANCE = 0.2; - - // Motor Limits - public final static double CALIBRATION_POWER = 0.15; - - // Calibration - public final static int CALIBRATION_COUNTER_LIMIT = 250; -} diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java deleted file mode 100644 index 80389af..0000000 --- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java +++ /dev/null @@ -1,238 +0,0 @@ -package frc.robot.subsystems.Climb; - -import org.littletonrobotics.junction.Logger; - -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -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.filter.Debouncer.DebounceType; -import edu.wpi.first.math.util.Units; -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; - -/** - * Climber subsystem - */ -public class LinearClimb extends SubsystemBase implements LinearClimbIO { - /** climber motor */ - private final TalonFX motor; - /** whether the subsysgtem is calibrating */ - private boolean calibrating = false; - - /** should the subsystem perform calibration automatically */ - private boolean calibrateOnStartUp = false; - - private double MAX_POWER = 0.2; - - private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising); - - // logging information - private LinearClimbIOInputsAutoLogged inputs = new LinearClimbIOInputsAutoLogged(); - - /** This PID controller uses motor rotations */ - private final PIDController pid = new PIDController( - ClimbConstants.PID_P, - ClimbConstants.PID_I, - ClimbConstants.PID_D); - - public LinearClimb() { - motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID, Constants.CANIVORE_SUB); - pid.setTolerance(ClimbConstants.PID_TOLERANCE); - - motor.setNeutralMode(NeutralModeValue.Brake); - - TalonFXConfiguration config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - motor.getConfigurator().apply(config); - - setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT); - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putData("Calibrate", new InstantCommand(() -> hardstopCalibration())); - SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp())); // 0 - SmartDashboard.putData("Go Down", new InstantCommand(() -> retract())); // 8 - SmartDashboard.putData("Climb", new InstantCommand(() -> climbPosition())); // 6 - } - - // starting position - motor.setPosition(metersToRotations(ClimbConstants.BOTTOM_POSITION)); - setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION)); - - // calibrate on startup to find hardstop - if (calibrateOnStartUp) { - hardstopCalibration(); - } - } - - /** - * set the setpoint for the pid - * - * @param setpoint in rotations - */ - public void setSetpoint(double setpoint) { - pid.setSetpoint(setpoint); - } - - /** - * @return when the position is within 0.2 rotations - */ - public boolean atSetPoint() { - return pid.atSetpoint(); - } - - /** - * Returns the current position of the climb motor. - * - * @return Position in motor rotations. Positive values move the climb mechanism - * UP (toward the hardstop). Higher values = higher physical position. - * Use {@link #getAsMeters()} for linear distance in meters. - */ - public double getPosition() { - return motor.getPosition().getValueAsDouble(); - } - - /** - * Returns the climb position converted to linear distance in meters. - * This is useful for debugging and logging. - * - * @return Linear position in meters, calculated as: - * rotations * gearRatio * 2 * PI * radius - */ - public double getAsMeters() { - return inputs.positionMeters; - } - - /** - * goes to the up position - */ - public void goUp() { // 0 - MAX_POWER = 0.8; - setSetpoint(metersToRotations(ClimbConstants.UP_POSITION)); - } - - /** - * goes to the down position - */ - public void retract() { // 8 - MAX_POWER = 0.2; - setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION)); - } - - /** - * goes to the climb position - */ - public void climbPosition() { // 6 - MAX_POWER = 0.8; - setSetpoint(metersToRotations(ClimbConstants.CLIMB_POSITION)); - } - - @Override - public void periodic() { - double power = pid.calculate(motor.getPosition().getValueAsDouble()); - // if it is not calibrating, do normal stuff - if (!calibrating) { - power = MathUtil.clamp(power, -MAX_POWER, MAX_POWER); - } else { - power = ClimbConstants.CALIBRATION_POWER; - boolean atHardStop = Math - .abs(motor.getStatorCurrent().getValueAsDouble()) >= ClimbConstants.CALIBRATION_CURRENT_THRESHOLD; - if (calibrationDebouncer.calculate(atHardStop)) { - stopCalibrating(); - } - } - - // motor.set(power); // during calibration we have 20ms of high power before we stop calibration\ - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putNumber("Climb Power from PID", power); - SmartDashboard.putNumber("Climb PID_Setpoint_Rotations", pid.getSetpoint()); - SmartDashboard.putNumber("Climb Motor_Actual_Rotations", motor.getPosition().getValueAsDouble()); - SmartDashboard.putNumber("Climb Motor_Actual_Meters", inputs.positionMeters); - SmartDashboard.putBoolean("Climb Calibrated", !calibrating); - SmartDashboard.putBoolean("Climb At Setpoint", atSetPoint()); - } - - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("LinearClimb setpointMeters", Units.rotationsToRadians(pid.getSetpoint()) - * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO); - } - updateInputs(); - Logger.processInputs("LinearClimb", inputs); - } - - /** - * converts motor rotations to meters - * - * @param motorRotations - * @return - */ - public double rotationsToMeters(double motorRotations) { - double circ = 2 * Math.PI * ClimbConstants.WHEEL_RADIUS; - double meters = motorRotations / ClimbConstants.CLIMB_GEAR_RATIO * circ; - return meters; - } - - /** - * converts meters to motor rotations - * - * @param meters - * @return - */ - public double metersToRotations(double meters) { - double circ = 2 * Math.PI * ClimbConstants.WHEEL_RADIUS; - double motorRotations = meters / circ * ClimbConstants.CLIMB_GEAR_RATIO; - return motorRotations; - } - - /** - * sets supply and stator current limits - * - * @param limit the current limit for stator and supply current - */ - public void setCurrentLimits(double limit) { - TalonFXConfiguration config = new TalonFXConfiguration(); - - config.CurrentLimits = new CurrentLimitsConfigs(); - - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.StatorCurrentLimit = limit; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = limit; - - motor.getConfigurator().apply(config); - } - - /** - * Sets the motor to a slow speed until it hits the hard stop - */ - public void hardstopCalibration() { - calibrating = true; - setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT); - } - - /** - * stops calibration and sets current limits to normal. - */ - public void stopCalibrating() { - motor.setPosition(metersToRotations(ClimbConstants.BOTTOM_POSITION)); - calibrating = false; - setCurrentLimits(ClimbConstants.CLIMB_CURRENT); - setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION)); - } - - @Override - public void updateInputs() { - inputs.positionMeters = Units.rotationsToRadians(motor.getPosition().getValueAsDouble()) - * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO; - inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); - inputs.power = pid.calculate(motor.getPosition().getValueAsDouble()); - } -} diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimbIO.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimbIO.java deleted file mode 100644 index a3ed4e9..0000000 --- a/src/main/java/frc/robot/subsystems/Climb/LinearClimbIO.java +++ /dev/null @@ -1,14 +0,0 @@ -package frc.robot.subsystems.Climb; - -import org.littletonrobotics.junction.AutoLog; - -public interface LinearClimbIO { - @AutoLog - public static class LinearClimbIOInputs{ - public double positionMeters = 0.0; - public double motorCurrent = 0.0; - public double power = 0.0; - } - - public void updateInputs(); -} diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java index e0b8bdd..54bdda1 100644 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -2,13 +2,7 @@ package frc.robot.subsystems.hood; 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 edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.LinearFilter; @@ -19,10 +13,8 @@ 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; -public class Hood extends SubsystemBase implements HoodIO { - private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.CANIVORE_SUB); +public class Hood extends SubsystemBase { private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02); @@ -31,41 +23,28 @@ public class Hood extends SubsystemBase implements HoodIO { private double lastFilteredRad = 0.0; private double lastRawSetpoint = 0.0; - private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO); + private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage( + Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO); private boolean calibrating = false; private Debouncer calibrateDebouncer = new Debouncer(0.5, DebounceType.kRising); private boolean forceHoodDown = false; - private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged(); - - public Hood(){ - motor.setNeutralMode(NeutralModeValue.Brake); + private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged(); + private HoodIO io; - TalonFXConfiguration config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - config.Slot0.kP = 2.0; - config.Slot0.kS = 0.1; // Static friction compensation - config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio - config.Slot0.kD = 0.02; // The "Braking" term to stop overshoot - - var mm = config.MotionMagic; - mm.MotionMagicCruiseVelocity = Units.radiansToRotations(HoodConstants.MAX_VELOCITY) * HoodConstants.HOOD_GEAR_RATIO; - mm.MotionMagicAcceleration = Units.radiansToRotations(HoodConstants.MAX_ACCELERATION) * HoodConstants.HOOD_GEAR_RATIO; // Lowered for belt safety - mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed - motor.getConfigurator().apply(config); - - setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT); - - motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO); + public Hood(HoodIO io) { + this.io = io; if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putData("max", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0))); - SmartDashboard.putData("medium", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0))); - SmartDashboard.putData("min", new InstantCommand(() -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0))); - + SmartDashboard.putData("max", new InstantCommand( + () -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0))); + SmartDashboard.putData("medium", new InstantCommand(() -> setFieldRelativeTarget( + new Rotation2d(Units.degreesToRadians((HoodConstants.MAX_ANGLE + HoodConstants.MIN_ANGLE) / 2)), 0))); + SmartDashboard.putData("min", new InstantCommand( + () -> setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MIN_ANGLE)), 0))); + SmartDashboard.putData("force hood down", new InstantCommand(() -> forceHoodDown(true))); SmartDashboard.putData("unforce hood", new InstantCommand(() -> forceHoodDown(false))); } @@ -74,16 +53,17 @@ public class Hood extends SubsystemBase implements HoodIO { /** * @return Position of the MOTOR in radians */ - public double getMotorPositionRad(){ - return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()); - } + public double getMotorPositionRad() { + return Units.degreesToRadians(inputs.positionDeg); + } /** * Sets the setpoint position and velocity of the hood. Call in command execute. + * * @param angle * @param velocityRadPerSec */ - public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) { + public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) { goalAngle = angle; goalVelocityRadPerSec = velocityRadPerSec; } @@ -91,11 +71,11 @@ public class Hood extends SubsystemBase implements HoodIO { /** * @return Position of turret in degrees */ - public double getPositionDeg(){ - return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO; + public double getPositionDeg() { + return inputs.positionDeg; } - public void forceHoodDown(boolean taranNathan){ + public void forceHoodDown(boolean taranNathan) { forceHoodDown = taranNathan; } @@ -103,12 +83,13 @@ public class Hood extends SubsystemBase implements HoodIO { return this.forceHoodDown; } - @Override - public void periodic() { - updateInputs(); + @Override + public void periodic() { + io.updateInputs(inputs); Logger.processInputs("Hood", inputs); - // goalAngle = Rotation2d.fromDegrees(SmartDashboard.getNumber("Hood Setpoint", goalAngle.getDegrees())); + // goalAngle = Rotation2d.fromDegrees(SmartDashboard.getNumber("Hood Setpoint", + // goalAngle.getDegrees())); // SmartDashboard.putNumber("Hood Setpoint", goalAngle.getDegrees()); if (forceHoodDown) { @@ -118,13 +99,13 @@ public class Hood extends SubsystemBase implements HoodIO { double setpointRad = goalAngle.getRadians(); - // calculate shortest angular delta + // calculate shortest angular delta double delta = setpointRad - lastRawSetpoint; delta = MathUtil.angleModulus(delta); - + // filter delta double filteredDelta = setpointFilter.calculate(delta); - + // apply filtered range lastFilteredRad = MathUtil.angleModulus(lastFilteredRad + filteredDelta); lastRawSetpoint = setpointRad; @@ -133,69 +114,55 @@ public class Hood extends SubsystemBase implements HoodIO { // Tells the Kraken to get to this position using 1000Hz profile double motorGoalRotations = Units.radiansToRotations(setpointRad) * HoodConstants.HOOD_GEAR_RATIO; - //Clamp the setpoint rotations - motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.radiansToRotations(Units.degreesToRadians(HoodConstants.MIN_ANGLE)) * HoodConstants.HOOD_GEAR_RATIO, Units.radiansToRotations(Units.degreesToRadians(HoodConstants.MAX_ANGLE)) * HoodConstants.HOOD_GEAR_RATIO); - + // Clamp the setpoint rotations + motorGoalRotations = MathUtil.clamp(motorGoalRotations, + Units.radiansToRotations(Units.degreesToRadians(HoodConstants.MIN_ANGLE)) * HoodConstants.HOOD_GEAR_RATIO, + Units.radiansToRotations(Units.degreesToRadians(HoodConstants.MAX_ANGLE)) * HoodConstants.HOOD_GEAR_RATIO); + // Multiply goal velocity by kV double velocityCompensation = goalVelocityRadPerSec * HoodConstants.FEEDFORWARD_KV; - if (calibrating){ - motor.set(0.1); - boolean atZero = Math.abs(motor.getStatorCurrent().getValueAsDouble()) >= HoodConstants.CALIBRATION_CURRENT_THRESHOLD; + if (calibrating) { + io.setMotorRaw(0.1); + boolean atZero = Math + .abs(inputs.motorCurrent) >= HoodConstants.CALIBRATION_CURRENT_THRESHOLD; boolean calibrated = calibrateDebouncer.calculate(atZero); - if (calibrated){ + if (calibrated) { stopCalibrating(); } - } else{ + } else { // Set control with feedforward - motor.setControl(mmVoltageRequest - .withPosition(motorGoalRotations) - .withFeedForward(velocityCompensation) - .withEnableFOC(true)); + io.setMotorControl(mmVoltageRequest + .withPosition(motorGoalRotations) + .withFeedForward(velocityCompensation) + .withEnableFOC(true)); } - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue()); - Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO); - Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians())); - } - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putBoolean("Hood Calibrated", !calibrating); - SmartDashboard.putBoolean("Hood At Setpoint", Math.abs(getPositionDeg() - goalAngle.getDegrees()) < 2.0); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Hood/Voltage", inputs.motorVoltage); + Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / HoodConstants.HOOD_GEAR_RATIO); + Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians())); } } - public void calibrate(){ + public void calibrate() { calibrating = true; setCurrentLimits(HoodConstants.CALIBRATING_CURRENT_LIMIT); } - public void stopCalibrating(){ - motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO); + public void stopCalibrating() { + io.setPositionRaw(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO); goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)); setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT); calibrating = false; } /** - * sets supply and stator current limits - * @param limitAmps the current limit for stator and supply current - */ - public void setCurrentLimits(double limitAmps) { - CurrentLimitsConfigs limits = new CurrentLimitsConfigs() - .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(limitAmps) - .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(limitAmps); - - motor.getConfigurator().apply(limits); - } - - @Override - public void updateInputs() { - inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO; - inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO; - inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); + * sets supply and stator current limits + * + * @param limitAmps the current limit for stator and supply current + */ + public void setCurrentLimits(double limitAmps) { + io.setCurrentLimits(limitAmps); } } diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/hood/HoodIO.java index 42886b5..7e2c91b 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodIO.java @@ -2,13 +2,31 @@ package frc.robot.subsystems.hood; import org.littletonrobotics.junction.AutoLog; +import com.ctre.phoenix6.controls.MotionMagicVoltage; + public interface HoodIO { @AutoLog public static class HoodIOInputs{ public double positionDeg = HoodConstants.MAX_ANGLE; public double velocityRadPerSec = 0.0; public double motorCurrent = 0.0; + public double motorVoltage = 0.0; } - public void updateInputs(); + public void updateInputs(HoodIOInputs inputs); + + public void setMotorRaw(double speed); + + public void setMotorControl(MotionMagicVoltage control); + + public void setPositionRaw(double pos); + + /** + * sets supply and stator current limits + * + * @param limitAmps the current limit for stator and supply current + */ + void setCurrentLimits(double limitAmps); + } + diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIOTalonFX.java b/src/main/java/frc/robot/subsystems/hood/HoodIOTalonFX.java new file mode 100644 index 0000000..8619363 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hood/HoodIOTalonFX.java @@ -0,0 +1,81 @@ +package frc.robot.subsystems.hood; + +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 edu.wpi.first.math.util.Units; +import frc.robot.constants.IdConstants; +import sun.jvm.hotspot.utilities.Unsigned5.SetPosition; + +public class HoodIOTalonFX implements HoodIO { + private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.CANIVORE_SUB); + + public HoodIOTalonFX() { + motor.setNeutralMode(NeutralModeValue.Brake); + + TalonFXConfiguration config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.Slot0.kP = 2.0; + config.Slot0.kS = 0.1; // Static friction compensation + config.Slot0.kV = 0.12; // Adjusted kV for the gear ratio + config.Slot0.kD = 0.02; // The "Braking" term to stop overshoot + + var mm = config.MotionMagic; + mm.MotionMagicCruiseVelocity = Units.radiansToRotations(HoodConstants.MAX_VELOCITY) * HoodConstants.HOOD_GEAR_RATIO; + mm.MotionMagicAcceleration = Units.radiansToRotations(HoodConstants.MAX_ACCELERATION) + * HoodConstants.HOOD_GEAR_RATIO; // Lowered for belt safety + mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed + motor.getConfigurator().apply(config); + + setCurrentLimits(HoodConstants.NORMAL_CURRENT_LIMIT); + + motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * HoodConstants.HOOD_GEAR_RATIO); + } + + @Override + public void updateInputs(HoodIOInputs inputs) { + inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) + / HoodConstants.HOOD_GEAR_RATIO; + inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) + / HoodConstants.HOOD_GEAR_RATIO; + inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); + inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble(); + } + + @Override + public void setMotorRaw(double speed) { + motor.set(speed); + + } + + @Override + public void setMotorControl(MotionMagicVoltage control) { + motor.setControl(control); + } + + @Override + public void setPositionRaw(double pos) { + motor.setPosition(pos); + } + + /** + * sets supply and stator current limits + * + * @param limitAmps the current limit for stator and supply current + */ + @Override + public void setCurrentLimits(double limitAmps) { + CurrentLimitsConfigs limits = new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(limitAmps) + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(limitAmps); + + motor.getConfigurator().apply(limits); + } +}