+++ /dev/null
-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;
-}
+++ /dev/null
-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());
- }
-}
+++ /dev/null
-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();
-}
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;
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);
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)));
}
/**
* @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;
}
/**
* @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;
}
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) {
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;
// 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);
}
}
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);
+
}
+
--- /dev/null
+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);
+ }
+}