From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Wed, 18 Feb 2026 18:31:59 +0000 (-0800) Subject: Merge branch 'beta-bot' of https://github.com/iron-claw-972/FRC2026 into beta-bot X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=03c80dc6d125692301da2c261f6eb4ead46ccab5;p=FRC2026.git Merge branch 'beta-bot' of https://github.com/iron-claw-972/FRC2026 into beta-bot --- 03c80dc6d125692301da2c261f6eb4ead46ccab5 diff --cc src/main/java/frc/robot/subsystems/hood/Hood.java index e164243,3948af5..8188ca7 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@@ -2,10 -2,8 +2,9 @@@ package frc.robot.subsystems.hood import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.Slot0Configs; 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; @@@ -34,103 -31,98 +33,83 @@@ public class Hood extends SubsystemBas private double MAX_VEL_RAD_PER_SEC = 25; private double MAX_ACCEL_RAD_PER_SEC2 = 160.0; - private double GEAR_RATIO = HoodConstants.HOOD_GEAR_RATIO; + private double GEAR_RATIO = HoodConstants.HOOD_GEAR_RATIO; - private static final PIDController positionPID = new PIDController(6.0, 0, 0.2); + private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02 + , 0.02); - private final TrapezoidProfile profile = new TrapezoidProfile( - new TrapezoidProfile.Constraints( - MAX_VEL_RAD_PER_SEC, - MAX_ACCEL_RAD_PER_SEC2)); - private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, - 1. / DCMotor.getFalcon500(1).KvRadPerSecPerVolt, 0); + private double FEEDFORWARD_KV = 0.12; - private State setpoint = new State(); + //private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0); private Rotation2d goalAngle = new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)); private double goalVelocityRadPerSec = 0.0; + private double lastFilteredRad = 0.0; + private double lastRawSetpoint = 0.0; - private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged(); + private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0); - public Hood() { - motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * GEAR_RATIO); - try { - Thread.sleep(100); - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); - } - motor.setNeutralMode(NeutralModeValue.Coast); + private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged(); - TalonFXConfiguration config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - motor.getConfigurator().apply(config); - - setpoint = new State(getPositionRad() / GEAR_RATIO, 0.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))); - } + public Hood(){ + motor.setNeutralMode(NeutralModeValue.Brake); - public double getPositionRad() { - return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()); - } + TalonFXConfiguration config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = InvertedValue.Clockwise_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(MAX_VEL_RAD_PER_SEC) * GEAR_RATIO; + mm.MotionMagicAcceleration = Units.radiansToRotations(MAX_ACCEL_RAD_PER_SEC2) * GEAR_RATIO; // Lowered for belt safety + mm.MotionMagicJerk = 0; // Set to > 0 for "S-Curve" smoothing if needed + motor.getConfigurator().apply(config); - public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) { + motor.setPosition(Units.degreesToRotations(HoodConstants.MAX_ANGLE) * GEAR_RATIO); - - 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))); - } - - /** - * @return Position of the MOTOR in radians - */ - public double getMotorPositionRad(){ - return Units.rotationsToRadians(motor.getPosition().getValueAsDouble()); - } - - /** - * Sets the setpoint position and velocity of the hood - * @param angle - * @param velocityRadPerSec - */ - public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) { goalAngle = angle; goalVelocityRadPerSec = velocityRadPerSec; } - @Override - public void periodic() { + @Override + public void periodic() { updateInputs(); + Logger.processInputs("Hood", inputs); - State goalState = new State( - MathUtil.clamp(goalAngle.getRadians(), MIN_ANGLE_RAD, MAX_ANGLE_RAD), - goalVelocityRadPerSec); - - setpoint = profile.calculate( - Constants.LOOP_TIME, - setpoint, - goalState); - - double motorVelRotPerSec = Units.radiansToRotations(setpoint.velocity) * GEAR_RATIO; - - double targetVelocity; - - double motorSetpointPosition = (setpoint.position) * GEAR_RATIO; - - targetVelocity = positionPID.calculate( - motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians), - motorSetpointPosition); + double setpointRad = goalAngle.getRadians(); - targetVelocity += Units.rotationsToRadians(motorVelRotPerSec); + // 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; + setpointRad = lastFilteredRad; - double voltage = feedForward.calculate(targetVelocity); + // Tells the Kraken to get to this position using 1000Hz profile + double motorGoalRotations = Units.radiansToRotations(setpointRad) * GEAR_RATIO; - motor.setVoltage(voltage); + //Clamp the setpoint rotations + motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(HoodConstants.MIN_ANGLE) * GEAR_RATIO, Units.degreesToRotations(HoodConstants.MAX_ANGLE) * GEAR_RATIO); + + double velocityCompensation = goalVelocityRadPerSec * FEEDFORWARD_KV; - Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue()); - Logger.recordOutput("Hood/velocitySetpoint", targetVelocity / GEAR_RATIO); - Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(setpoint.position) / GEAR_RATIO); + motor.setControl(mmVoltageRequest + .withPosition(motorGoalRotations) + .withFeedForward(velocityCompensation)); - SmartDashboard.putData("Hood PID", positionPID); + Logger.recordOutput("Hood/Voltage", motor.getMotorVoltage().getValue()); + Logger.recordOutput("Hood/velocitySetpoint", goalVelocityRadPerSec / GEAR_RATIO); + Logger.recordOutput("Hood/SetpointDeg", Units.radiansToDegrees(goalAngle.getRadians()) / GEAR_RATIO); - SmartDashboard.putNumber("Hood Position Deg", Units.radiansToDegrees(getPositionRad()) / GEAR_RATIO); - Logger.processInputs("Hood", inputs); } - @Override + @Override public void updateInputs() { inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO; inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / GEAR_RATIO;