From f452a3e408ab3325f5909b4f4aaf9610a965f2ac Mon Sep 17 00:00:00 2001 From: Mond Date: Wed, 18 Mar 2026 19:14:41 -0700 Subject: [PATCH 01/10] Brown protection go! --- emsdk | 1 + .../lib/swerve/MotionSwerveDrive.java | 66 +++++++++++ .../swerve/commands/DriveToStateCommand.java | 53 ++++++--- .../lib/swerve/motion/VelocityMotion.java | 16 ++- src/main/java/frc/robot/RobotContainer.java | 3 + .../java/frc/robot/power/BrownProtection.java | 111 ++++++++++++++++++ .../robot/subsystems/hood/ShooterHood.java | 85 ++++++++++---- .../intakeextension/IntakeExtension.java | 55 +++++++-- .../frc/robot/subsystems/turret/Turret.java | 77 +++++++++--- 9 files changed, 399 insertions(+), 68 deletions(-) create mode 160000 emsdk create mode 100644 src/main/java/frc/robot/power/BrownProtection.java diff --git a/emsdk b/emsdk new file mode 160000 index 00000000..d6b88f4f --- /dev/null +++ b/emsdk @@ -0,0 +1 @@ +Subproject commit d6b88f4ffd8d6163aadb6ff48ca4b32ceec890dd diff --git a/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java b/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java index 33ccfba4..3e22569e 100644 --- a/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java +++ b/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java @@ -2,8 +2,10 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; @@ -25,6 +27,7 @@ import com.team6962.lib.swerve.util.FieldLogger; import com.team6962.lib.swerve.util.SwerveComponent; import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Twist2d; @@ -32,6 +35,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -82,6 +86,8 @@ public class MotionSwerveDrive implements AutoCloseable { private BaseStatusSignal[] statusSignals; private SwerveDriveKinematics kinematics; private SwerveMotionManager motionManager; + private volatile double velocityScale = 1.0; + private volatile double motionConstraintScale = 1.0; /** * Creates a new MotionSwerveDrive with the specified drivetrain configuration. @@ -501,6 +507,66 @@ public void updateMotion() { motionManager.update(); } + /** + * Sets runtime scale that is applied to requested drivetrain velocities. + * + * @param scale the velocity scale to apply + */ + public void setVelocityScale(double scale) { + velocityScale = MathUtil.clamp(scale, 0.0, 1.0); + } + + /** + * Gets runtime scale applied to requested drivetrain velocities. + * + * @return the velocity scale + */ + public double getVelocityScale() { + return velocityScale; + } + + /** + * Sets runtime scale applied to auton drivetrain motion profile constraints. + * + * @param scale the constraint scale to apply + */ + public void setMotionConstraintScale(double scale) { + motionConstraintScale = MathUtil.clamp(scale, 0.0, 1.0); + } + + /** + * Gets runtime scale currently applied to auton drivetrain motion profile constraints. + * + * @return the constraint scale + */ + public double getMotionConstraintScale() { + return motionConstraintScale; + } + + /** + * Gets auton translation constraints after applying current runtime scale. + * + * @return scaled auton translation constraints + */ + public TrapezoidProfile.Constraints getScaledTranslationConstraints() { + return new TrapezoidProfile.Constraints( + constants.Driving.AutoLinearVelocity.in(MetersPerSecond) * motionConstraintScale, + constants.Driving.AutoLinearAcceleration.in(MetersPerSecondPerSecond) + * motionConstraintScale); + } + + /** + * Gets auton rotation constraints after applying current runtime scale. + * + * @return scaled auton rotation constraints + */ + public TrapezoidProfile.Constraints getScaledRotationConstraints() { + return new TrapezoidProfile.Constraints( + constants.Driving.AutoAngularVelocity.in(RadiansPerSecond) * motionConstraintScale, + constants.Driving.AutoAngularAcceleration.in(RadiansPerSecondPerSecond) + * motionConstraintScale); + } + /** * Applies a motion to be executed during the next control loop cycle. * diff --git a/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java b/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java index 4630c391..9b80df86 100644 --- a/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java +++ b/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java @@ -48,6 +48,9 @@ public class DriveToStateCommand extends Command { /** Whether the motion profiles have finished at least once. */ private boolean motionProfilesFinished = false; + /** Most recent drivetrain motion profile scale used to create controllers. */ + private double previousMotionConstraintScale = Double.NaN; + /** Class representing the target state for the drive command. */ public static class State { /** The target translation position. If null, the robot translation will not be controlled. */ @@ -153,29 +156,11 @@ public DriveToStateCommand(CommandSwerveDrive swerveDrive, State target) { this.swerveDrive = swerveDrive; this.target = target; - // Initialize translation and rotation controllers with PID constants - // and motion profile constraints if (target.translation != null) { - translationController = - new TranslationController( - swerveDrive.getConstants().Driving.TranslationFeedbackKP, - swerveDrive.getConstants().Driving.TranslationFeedbackKI, - swerveDrive.getConstants().Driving.TranslationFeedbackKD, - swerveDrive.getConstants().Driving.getTranslationConstraints(), - Hertz.of(50)); - addRequirements(swerveDrive.useTranslation()); } if (target.angle != null) { - headingController = - new ProfiledController( - swerveDrive.getConstants().Driving.AngleFeedbackKP, - swerveDrive.getConstants().Driving.AngleFeedbackKI, - swerveDrive.getConstants().Driving.AngleFeedbackKD, - new TrapezoidalProfile(swerveDrive.getConstants().Driving.getRotationConstraints()), - Hertz.of(50)); - addRequirements(swerveDrive.useRotation()); } } @@ -185,6 +170,8 @@ public void initialize() { // Reset state motionProfilesFinished = false; + createControllers(); + // Generate initial motion profiles createMotionProfiles(); @@ -196,6 +183,31 @@ public void initialize() { DogLog.log("Drivetrain/DriveToState/FinalAngularVelocity", target.angularVelocity); } + /** Creates controllers for translation and rotation using the current scaled constraints. */ + private void createControllers() { + previousMotionConstraintScale = swerveDrive.getMotionConstraintScale(); + + if (target.translation != null) { + translationController = + new TranslationController( + swerveDrive.getConstants().Driving.TranslationFeedbackKP, + swerveDrive.getConstants().Driving.TranslationFeedbackKI, + swerveDrive.getConstants().Driving.TranslationFeedbackKD, + swerveDrive.getScaledTranslationConstraints(), + Hertz.of(50)); + } + + if (target.angle != null) { + headingController = + new ProfiledController( + swerveDrive.getConstants().Driving.AngleFeedbackKP, + swerveDrive.getConstants().Driving.AngleFeedbackKI, + swerveDrive.getConstants().Driving.AngleFeedbackKD, + new TrapezoidalProfile(swerveDrive.getScaledRotationConstraints()), + Hertz.of(50)); + } + } + /** Creates motion profiles for translation and rotation controllers. */ private void createMotionProfiles() { if (translationController != null) { @@ -231,6 +243,11 @@ private void createMotionProfiles() { @Override public void execute() { + if (Math.abs(swerveDrive.getMotionConstraintScale() - previousMotionConstraintScale) > 0.05) { + createControllers(); + createMotionProfiles(); + } + // Check if motion profiles have finished if ((translationController == null || translationController.isFinished()) && (headingController == null || headingController.isFinished())) { diff --git a/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java b/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java index 45ec4092..29a4c6ae 100644 --- a/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java +++ b/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java @@ -141,15 +141,22 @@ public SwerveMotion fuseWith(SwerveMotion other) { */ @Override public void update(double deltaTimeSeconds) { - if (Math.abs(velocity.omegaRadiansPerSecond) < 0.01 - && Math.abs(velocity.vxMetersPerSecond) < 0.01 - && Math.abs(velocity.vyMetersPerSecond) < 0.01) { + double velocityScale = swerveDrive.getVelocityScale(); + ChassisSpeeds scaledVelocity = + new ChassisSpeeds( + velocity.vxMetersPerSecond * velocityScale, + velocity.vyMetersPerSecond * velocityScale, + velocity.omegaRadiansPerSecond * velocityScale); + + if (Math.abs(scaledVelocity.omegaRadiansPerSecond) < 0.01 + && Math.abs(scaledVelocity.vxMetersPerSecond) < 0.01 + && Math.abs(scaledVelocity.vyMetersPerSecond) < 0.01) { brake(); return; } ChassisSpeeds robotRelativeVelocity = - ChassisSpeeds.fromFieldRelativeSpeeds(velocity, new Rotation2d(swerveDrive.getHeading())); + ChassisSpeeds.fromFieldRelativeSpeeds(scaledVelocity, new Rotation2d(swerveDrive.getHeading())); SwerveModuleState[] states = swerveDrive.getKinematics().toSwerveModuleStates(robotRelativeVelocity); @@ -234,6 +241,7 @@ public void logTelemetry(String basePath) { DogLog.log(basePath + "LinearVelocityX", velocity.vxMetersPerSecond); DogLog.log(basePath + "LinearVelocityY", velocity.vyMetersPerSecond); DogLog.log(basePath + "AngularVelocity", velocity.omegaRadiansPerSecond); + DogLog.log(basePath + "VelocityScale", swerveDrive.getVelocityScale()); } /** Sets all motors to neutral/brake mode */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b759b801..d4453bb4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.robot.auto.shoot.ShooterFunctions; import frc.robot.constants.RobotConstants; import frc.robot.controls.TeleopControls; +import frc.robot.power.BrownProtection; import frc.robot.subsystems.hood.ShooterHood; import frc.robot.subsystems.hopper.Hopper; import frc.robot.subsystems.intakeextension.IntakeExtension; @@ -54,6 +55,7 @@ public class RobotContainer { private final ShooterFunctions hubFunctions; private final ShooterFunctions passFunctions; private final Autonomous autonomous; + private final BrownProtection brownoutProtection; private final Command noneAutonomous = Commands.none(); public RobotContainer() { @@ -74,6 +76,7 @@ public RobotContainer() { turret = new Turret(); intakeExtension = new IntakeExtension(); hopper = new Hopper(); + brownoutProtection = new BrownProtection(swerveDrive, turret, shooterHood, intakeExtension); aprilTagVision = new AprilTagVision(swerveDrive, constants.getAprilTagVisionConstants()); fuelClumpLocalization = diff --git a/src/main/java/frc/robot/power/BrownProtection.java b/src/main/java/frc/robot/power/BrownProtection.java new file mode 100644 index 00000000..c4f569bd --- /dev/null +++ b/src/main/java/frc/robot/power/BrownProtection.java @@ -0,0 +1,111 @@ +package frc.robot.power; + +import static edu.wpi.first.units.Units.Amps; + +import com.team6962.lib.logging.CurrentDrawLogger; +import com.team6962.lib.swerve.CommandSwerveDrive; +import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.hood.ShooterHood; +import frc.robot.subsystems.intakeextension.IntakeExtension; +import frc.robot.subsystems.turret.Turret; + +/** + * Dynamically slows drivetrain and lower-priority mechanism motion when battery voltage + * drops or robot is drawing too high of a current. + */ +public class BrownProtection extends SubsystemBase { + private final CommandSwerveDrive swerveDrive; + private final Turret turret; + private final ShooterHood shooterHood; + private final IntakeExtension intakeExtension; + + private final LinearFilter batteryVoltageFilter = LinearFilter.movingAverage(10); + private final LinearFilter totalCurrentFilter = LinearFilter.movingAverage(10); + + private double startLimitingVoltage = 10.2; + private double minimumVoltage = 8.8; + private double startLimitingCurrentAmps = 180.0; + private double maximumCurrentAmps = 280.0; + private double minimumDriveScale = 0.45; + private double minimumMechanismScale = 0.2; + + public BrownProtection( + CommandSwerveDrive swerveDrive, + Turret turret, + ShooterHood shooterHood, + IntakeExtension intakeExtension) { + this.swerveDrive = swerveDrive; + this.turret = turret; + this.shooterHood = shooterHood; + this.intakeExtension = intakeExtension; + + DogLog.tunable( + "Brownout/Start Limiting Voltage", + startLimitingVoltage, + value -> startLimitingVoltage = value); + DogLog.tunable( + "Brownout/Minimum Voltage", minimumVoltage, value -> minimumVoltage = value); + DogLog.tunable( + "Brownout/Start Limiting Current", + startLimitingCurrentAmps, + value -> startLimitingCurrentAmps = value); + DogLog.tunable( + "Brownout/Maximum Current", maximumCurrentAmps, value -> maximumCurrentAmps = value); + DogLog.tunable( + "Brownout/Minimum Drive Scale", minimumDriveScale, value -> minimumDriveScale = value); + DogLog.tunable( + "Brownout/Minimum Mechanism Scale", + minimumMechanismScale, + value -> minimumMechanismScale = value); + } + + @Override + public void periodic() { + double batteryVoltage = RobotController.getBatteryVoltage(); + double filteredBatteryVoltage = batteryVoltageFilter.calculate(batteryVoltage); + double totalCurrentAmps = CurrentDrawLogger.getTotalCurrent().in(Amps); + double filteredTotalCurrentAmps = totalCurrentFilter.calculate(totalCurrentAmps); + + double voltageBudget = + unitRange(filteredBatteryVoltage, minimumVoltage, startLimitingVoltage); + double currentBudget = + 1.0 + - unitRange( + filteredTotalCurrentAmps, startLimitingCurrentAmps, maximumCurrentAmps); + double protectionBudget = + RobotState.isDisabled() ? 1.0 : Math.min(voltageBudget, currentBudget); + + double driveScale = MathUtil.interpolate(minimumDriveScale, 1.0, protectionBudget); + double mechanismScale = + MathUtil.interpolate(minimumMechanismScale, 1.0, protectionBudget); + + swerveDrive.setVelocityScale(driveScale); + swerveDrive.setMotionConstraintScale(driveScale); + turret.setMotionProfileConstraintScale(mechanismScale); + shooterHood.setMotionProfileConstraintScale(mechanismScale); + intakeExtension.setMotionProfileConstraintScale(mechanismScale); + + DogLog.log("Brownout/BatteryVoltage", batteryVoltage); + DogLog.log("Brownout/FilteredBatteryVoltage", filteredBatteryVoltage); + DogLog.log("Brownout/TotalCurrentAmps", totalCurrentAmps); + DogLog.log("Brownout/FilteredCurrentAmps", filteredTotalCurrentAmps); + DogLog.log("Brownout/VoltageBudget", voltageBudget); + DogLog.log("Brownout/CurrentBudget", currentBudget); + DogLog.log("Brownout/ProtectionBudget", protectionBudget); + DogLog.log("Brownout/DriveScale", driveScale); + DogLog.log("Brownout/MechanismScale", mechanismScale); + } + + private static double unitRange(double value, double minimumValue, double maximumValue) { + if (maximumValue <= minimumValue) { + return value >= maximumValue ? 1.0 : 0.0; + } + + return MathUtil.clamp((value - minimumValue) / (maximumValue - minimumValue), 0.0, 1.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java index 01ef1604..a0084028 100644 --- a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java +++ b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; @@ -19,6 +20,7 @@ import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.math.MeasureUtil; import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; @@ -56,6 +58,13 @@ public class ShooterHood extends SubsystemBase { private ShooterHoodSim simulation; private boolean isZeroed = false; private double kG = ShooterHoodConstants.kG; + private double baseMotionMagicCruiseVelocity = + ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity; + private double baseMotionMagicAcceleration = + ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration; + private double motionProfileConstraintScale = 1.0; + private double appliedCruiseVelocity = Double.NaN; + private double appliedAcceleration = Double.NaN; private Supplier shouldLowerHoodSupplier; @@ -80,6 +89,11 @@ public ShooterHood(Supplier shouldLowerHoodSupplier) { 1000.0; // 30.0; } + baseMotionMagicCruiseVelocity = + ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity; + baseMotionMagicAcceleration = + ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration; + hoodMotor.getConfigurator().apply(ShooterHoodConstants.MOTOR_CONFIGURATION); candi.getConfigurator().apply(ShooterHoodConstants.CANDI_CONFIGURATION); @@ -153,24 +167,18 @@ public ShooterHood(Supplier shouldLowerHoodSupplier) { DogLog.tunable( "Hood/Hood Cruise Velocity", - ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity, + baseMotionMagicCruiseVelocity, newCruiseVelocity -> { - hoodMotor - .getConfigurator() - .apply( - ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic - .withMotionMagicCruiseVelocity(newCruiseVelocity)); + baseMotionMagicCruiseVelocity = newCruiseVelocity; + applyMotionMagicConfig(); }); DogLog.tunable( "Hood/Hood Acceleration", - ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration, + baseMotionMagicAcceleration, newAcceleration -> { - hoodMotor - .getConfigurator() - .apply( - ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.withMotionMagicAcceleration( - newAcceleration)); + baseMotionMagicAcceleration = newAcceleration; + applyMotionMagicConfig(); }); DogLog.tunable( @@ -188,6 +196,7 @@ public ShooterHood(Supplier shouldLowerHoodSupplier) { } CurrentDrawLogger.add("Shooter Hood", this::getSupplyCurrent); + applyMotionMagicConfig(); } @Override @@ -227,6 +236,7 @@ public void periodic() { "Hood/ProfileReferenceAngle", Rotations.of(profileReferenceSignal.getValue()).in(Degrees), Degrees); + DogLog.log("Hood/MotionMagicScale", motionProfileConstraintScale); // LoggingUtil.log("Hood/ControlRequest", hoodMotor.getAppliedControl()); @@ -445,15 +455,6 @@ public Command track( Supplier targetAngleSupplier, Supplier targetVelocitySupplier) { Command command = new Command() { - private TrapezoidProfile profile = - new TrapezoidProfile( - new TrapezoidProfile.Constraints( - ShooterHoodConstants.MOTOR_CONFIGURATION - .MotionMagic - .MotionMagicCruiseVelocity, - ShooterHoodConstants.MOTOR_CONFIGURATION - .MotionMagic - .MotionMagicAcceleration)); private TrapezoidProfile.State previousProfileState; private double previousUpdateTimestamp; @@ -474,6 +475,12 @@ public void execute() { if (targetVelocity == null) targetVelocity = RotationsPerSecond.of(0); Angle targetPosition = clampPositionToSafeRange(targetAngleSupplier.get()); + + TrapezoidProfile profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + getScaledMotionMagicCruiseVelocity(), + getScaledMotionMagicAcceleration())); TrapezoidProfile.State profileState = profile.calculate( @@ -504,6 +511,42 @@ public void execute() { return command; } + public void setMotionProfileConstraintScale(double scale) { + motionProfileConstraintScale = + MathUtil.clamp(Math.round(scale * 20.0) / 20.0, 0.1, 1.0); + applyMotionMagicConfig(); + } + + private double getScaledMotionMagicCruiseVelocity() { + return baseMotionMagicCruiseVelocity * motionProfileConstraintScale; + } + + private double getScaledMotionMagicAcceleration() { + return baseMotionMagicAcceleration * motionProfileConstraintScale; + } + + private void applyMotionMagicConfig() { + double scaledCruiseVelocity = getScaledMotionMagicCruiseVelocity(); + double scaledAcceleration = getScaledMotionMagicAcceleration(); + + if (Math.abs(scaledCruiseVelocity - appliedCruiseVelocity) < 1e-6 + && Math.abs(scaledAcceleration - appliedAcceleration) < 1e-6) { + return; + } + + hoodMotor + .getConfigurator() + .apply( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(scaledCruiseVelocity) + .withMotionMagicAcceleration(scaledAcceleration) + .withMotionMagicJerk( + ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicJerk)); + + appliedCruiseVelocity = scaledCruiseVelocity; + appliedAcceleration = scaledAcceleration; + } + /** * Zeroes the hood without the hall effect sensor by assuming the hood is at the minimum angle. * diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java index 85710e03..ed54284a 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java @@ -21,6 +21,7 @@ import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.phoenix.StatusUtil; import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; @@ -55,6 +56,14 @@ public class IntakeExtension extends SubsystemBase { private StatusSignal closedLoopReferenceSignal; private IntakeExtensionSim simulation; + + private double baseMotionMagicCruiseVelocity = + IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity; + private double baseMotionMagicAcceleration = + IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration; + private double motionProfileConstraintScale = 1.0; + private double appliedCruiseVelocity = Double.NaN; + private double appliedAcceleration = Double.NaN; public boolean isZeroed = false; @@ -163,22 +172,18 @@ public IntakeExtension() { DogLog.tunable( "IntakeExtension/Velocity", - IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity, + baseMotionMagicCruiseVelocity, newVelocity -> { - MotionMagicConfigs config = new MotionMagicConfigs(); - StatusUtil.check(motor.getConfigurator().refresh(config)); - config.MotionMagicCruiseVelocity = newVelocity; - motor.getConfigurator().apply(config); + baseMotionMagicCruiseVelocity = newVelocity; + applyMotionMagicConfig(); }); DogLog.tunable( "IntakeExtension/Acceleration", - IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration, + baseMotionMagicAcceleration, newAcceleration -> { - MotionMagicConfigs config = new MotionMagicConfigs(); - StatusUtil.check(motor.getConfigurator().refresh(config)); - config.MotionMagicAcceleration = newAcceleration; - motor.getConfigurator().apply(config); + baseMotionMagicAcceleration = newAcceleration; + applyMotionMagicConfig(); }); if (RobotBase.isSimulation()) { @@ -189,6 +194,35 @@ public IntakeExtension() { } CurrentDrawLogger.add("Intake Extension", this::getSupplyCurrent); + applyMotionMagicConfig(); + } + + public void setMotionProfileConstraintScale(double scale) { + motionProfileConstraintScale = + MathUtil.clamp(Math.round(scale * 20.0) / 20.0, 0.1, 1.0); + applyMotionMagicConfig(); + } + + private void applyMotionMagicConfig() { + double scaledCruiseVelocity = baseMotionMagicCruiseVelocity * motionProfileConstraintScale; + double scaledAcceleration = baseMotionMagicAcceleration * motionProfileConstraintScale; + + if (Math.abs(scaledCruiseVelocity - appliedCruiseVelocity) < 1e-6 + && Math.abs(scaledAcceleration - appliedAcceleration) < 1e-6) { + return; + } + + motor + .getConfigurator() + .apply( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(scaledCruiseVelocity) + .withMotionMagicAcceleration(scaledAcceleration) + .withMotionMagicJerk( + IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicJerk)); + + appliedCruiseVelocity = scaledCruiseVelocity; + appliedAcceleration = scaledAcceleration; } /** @@ -401,5 +435,6 @@ && getPosition().lt(IntakeExtensionConstants.MIN_POSITION)) { DogLog.log("Intake/HallSensorTriggered", isHallSensorTriggered()); DogLog.log("Intake/ClosedLoopReference", getClosedLoopReference()); DogLog.forceNt.log("Intake/IsZeroed", isZeroed); + DogLog.log("Intake/MotionMagicScale", motionProfileConstraintScale); } } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 1ef1650b..84fb37ec 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.MotionMagicVoltage; @@ -22,6 +23,7 @@ import com.team6962.lib.math.AngleMath; import com.team6962.lib.math.MeasureUtil; import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; @@ -78,6 +80,11 @@ public class Turret extends SubsystemBase { /** The current motor configuration */ private TalonFXConfiguration config; + private double baseMotionMagicCruiseVelocity; + private double baseMotionMagicAcceleration; + private double motionProfileConstraintScale = 1.0; + private double appliedCruiseVelocity = Double.NaN; + private double appliedAcceleration = Double.NaN; /** Assigns Status Signal variables to the different methods part of the motor.get() */ public Turret() { @@ -103,6 +110,8 @@ public Turret() { config.MotionMagic.MotionMagicCruiseVelocity = TurretConstants.MOTION_MAGIC_CRUISE_VELOCITY; config.MotionMagic.MotionMagicAcceleration = TurretConstants.MOTION_MAGIC_ACCELERATION; config.Feedback.SensorToMechanismRatio = TurretConstants.SENSOR_TO_MECHANISM_RATIO; + baseMotionMagicCruiseVelocity = config.MotionMagic.MotionMagicCruiseVelocity; + baseMotionMagicAcceleration = config.MotionMagic.MotionMagicAcceleration; config.MotorOutput.NeutralMode = TurretConstants.MOTOR_NEUTRAL_MODE; @@ -167,19 +176,21 @@ public Turret() { DogLog.tunable( "Turret/Turret Cruise Velocity", - config.MotionMagic.MotionMagicCruiseVelocity, - newVel -> - motor - .getConfigurator() - .apply(config.MotionMagic.withMotionMagicCruiseVelocity(newVel))); + baseMotionMagicCruiseVelocity, + newVel -> { + baseMotionMagicCruiseVelocity = newVel; + config.MotionMagic.MotionMagicCruiseVelocity = newVel; + applyMotionMagicConfig(); + }); DogLog.tunable( "Turret/Turret Max Acceleration", - config.MotionMagic.MotionMagicAcceleration, - newAccel -> - motor - .getConfigurator() - .apply(config.MotionMagic.withMotionMagicAcceleration(newAccel))); + baseMotionMagicAcceleration, + newAccel -> { + baseMotionMagicAcceleration = newAccel; + config.MotionMagic.MotionMagicAcceleration = newAccel; + applyMotionMagicConfig(); + }); DogLog.tunable("Turret/Turret kW", TurretConstants.kW, newKW -> TurretConstants.kW = newKW); @@ -204,6 +215,7 @@ public Turret() { } CurrentDrawLogger.add("Turret", this::getSupplyCurrent); + applyMotionMagicConfig(); } @Override @@ -247,6 +259,7 @@ public void periodic() { "Turret/ProfilePosition", Rotations.of(profilePositionSignal.getValue()).in(Radians), Radians); + DogLog.log("Turret/MotionMagicScale", motionProfileConstraintScale); // LoggingUtil.log("Turret/ControlRequest", motor.getAppliedControl()); @@ -498,11 +511,6 @@ public Command track( Supplier targetAngleSupplier, Supplier targetVelocitySupplier) { Command command = new Command() { - private TrapezoidProfile profile = - new TrapezoidProfile( - new TrapezoidProfile.Constraints( - TurretConstants.MOTION_MAGIC_CRUISE_VELOCITY, - TurretConstants.MOTION_MAGIC_ACCELERATION)); private TrapezoidProfile.State previousProfileState; private double previousUpdateTimestamp; @@ -530,6 +538,12 @@ public void execute() { TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE)); + TrapezoidProfile profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + getScaledMotionMagicCruiseVelocity(), + getScaledMotionMagicAcceleration())); + TrapezoidProfile.State profileState = profile.calculate( Timer.getFPGATimestamp() - previousUpdateTimestamp, @@ -634,6 +648,39 @@ public Command moveAtVoltage(Voltage voltage) { .onlyIf(() -> isZeroed()); } + public void setMotionProfileConstraintScale(double scale) { + motionProfileConstraintScale = MathUtil.clamp(Math.round(scale * 20.0) / 20.0, 0.1, 1.0); + applyMotionMagicConfig(); + } + + private double getScaledMotionMagicCruiseVelocity() { + return baseMotionMagicCruiseVelocity * motionProfileConstraintScale; + } + + private double getScaledMotionMagicAcceleration() { + return baseMotionMagicAcceleration * motionProfileConstraintScale; + } + + private void applyMotionMagicConfig() { + double scaledCruiseVelocity = getScaledMotionMagicCruiseVelocity(); + double scaledAcceleration = getScaledMotionMagicAcceleration(); + + if (Math.abs(scaledCruiseVelocity - appliedCruiseVelocity) < 1e-6 + && Math.abs(scaledAcceleration - appliedAcceleration) < 1e-6) { + return; + } + + motor + .getConfigurator() + .apply( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(scaledCruiseVelocity) + .withMotionMagicAcceleration(scaledAcceleration)); + + appliedCruiseVelocity = scaledCruiseVelocity; + appliedAcceleration = scaledAcceleration; + } + public void zero() { motor.setPosition(Degrees.of(180)); } From 94ab8c094ede31229668a0553c5b66c1fe513f4c Mon Sep 17 00:00:00 2001 From: Mond-Mann <63756512+Mond-Mann@users.noreply.github.com> Date: Thu, 19 Mar 2026 02:26:02 +0000 Subject: [PATCH 02/10] style: Apply Spotless fixes --- .../lib/swerve/motion/VelocityMotion.java | 3 ++- .../java/frc/robot/power/BrownProtection.java | 17 ++++++----------- .../frc/robot/subsystems/hood/ShooterHood.java | 8 +++----- .../intakeextension/IntakeExtension.java | 5 ++--- .../frc/robot/subsystems/turret/Turret.java | 4 ++-- 5 files changed, 15 insertions(+), 22 deletions(-) diff --git a/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java b/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java index 29a4c6ae..a1195dd5 100644 --- a/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java +++ b/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java @@ -156,7 +156,8 @@ public void update(double deltaTimeSeconds) { } ChassisSpeeds robotRelativeVelocity = - ChassisSpeeds.fromFieldRelativeSpeeds(scaledVelocity, new Rotation2d(swerveDrive.getHeading())); + ChassisSpeeds.fromFieldRelativeSpeeds( + scaledVelocity, new Rotation2d(swerveDrive.getHeading())); SwerveModuleState[] states = swerveDrive.getKinematics().toSwerveModuleStates(robotRelativeVelocity); diff --git a/src/main/java/frc/robot/power/BrownProtection.java b/src/main/java/frc/robot/power/BrownProtection.java index c4f569bd..2fa449eb 100644 --- a/src/main/java/frc/robot/power/BrownProtection.java +++ b/src/main/java/frc/robot/power/BrownProtection.java @@ -15,8 +15,8 @@ import frc.robot.subsystems.turret.Turret; /** - * Dynamically slows drivetrain and lower-priority mechanism motion when battery voltage - * drops or robot is drawing too high of a current. + * Dynamically slows drivetrain and lower-priority mechanism motion when battery voltage drops or + * robot is drawing too high of a current. */ public class BrownProtection extends SubsystemBase { private final CommandSwerveDrive swerveDrive; @@ -48,8 +48,7 @@ public BrownProtection( "Brownout/Start Limiting Voltage", startLimitingVoltage, value -> startLimitingVoltage = value); - DogLog.tunable( - "Brownout/Minimum Voltage", minimumVoltage, value -> minimumVoltage = value); + DogLog.tunable("Brownout/Minimum Voltage", minimumVoltage, value -> minimumVoltage = value); DogLog.tunable( "Brownout/Start Limiting Current", startLimitingCurrentAmps, @@ -71,18 +70,14 @@ public void periodic() { double totalCurrentAmps = CurrentDrawLogger.getTotalCurrent().in(Amps); double filteredTotalCurrentAmps = totalCurrentFilter.calculate(totalCurrentAmps); - double voltageBudget = - unitRange(filteredBatteryVoltage, minimumVoltage, startLimitingVoltage); + double voltageBudget = unitRange(filteredBatteryVoltage, minimumVoltage, startLimitingVoltage); double currentBudget = - 1.0 - - unitRange( - filteredTotalCurrentAmps, startLimitingCurrentAmps, maximumCurrentAmps); + 1.0 - unitRange(filteredTotalCurrentAmps, startLimitingCurrentAmps, maximumCurrentAmps); double protectionBudget = RobotState.isDisabled() ? 1.0 : Math.min(voltageBudget, currentBudget); double driveScale = MathUtil.interpolate(minimumDriveScale, 1.0, protectionBudget); - double mechanismScale = - MathUtil.interpolate(minimumMechanismScale, 1.0, protectionBudget); + double mechanismScale = MathUtil.interpolate(minimumMechanismScale, 1.0, protectionBudget); swerveDrive.setVelocityScale(driveScale); swerveDrive.setMotionConstraintScale(driveScale); diff --git a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java index a0084028..bdec78de 100644 --- a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java +++ b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java @@ -475,12 +475,11 @@ public void execute() { if (targetVelocity == null) targetVelocity = RotationsPerSecond.of(0); Angle targetPosition = clampPositionToSafeRange(targetAngleSupplier.get()); - + TrapezoidProfile profile = new TrapezoidProfile( new TrapezoidProfile.Constraints( - getScaledMotionMagicCruiseVelocity(), - getScaledMotionMagicAcceleration())); + getScaledMotionMagicCruiseVelocity(), getScaledMotionMagicAcceleration())); TrapezoidProfile.State profileState = profile.calculate( @@ -512,8 +511,7 @@ public void execute() { } public void setMotionProfileConstraintScale(double scale) { - motionProfileConstraintScale = - MathUtil.clamp(Math.round(scale * 20.0) / 20.0, 0.1, 1.0); + motionProfileConstraintScale = MathUtil.clamp(Math.round(scale * 20.0) / 20.0, 0.1, 1.0); applyMotionMagicConfig(); } diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java index ed54284a..acbea7da 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java @@ -56,7 +56,7 @@ public class IntakeExtension extends SubsystemBase { private StatusSignal closedLoopReferenceSignal; private IntakeExtensionSim simulation; - + private double baseMotionMagicCruiseVelocity = IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicCruiseVelocity; private double baseMotionMagicAcceleration = @@ -198,8 +198,7 @@ public IntakeExtension() { } public void setMotionProfileConstraintScale(double scale) { - motionProfileConstraintScale = - MathUtil.clamp(Math.round(scale * 20.0) / 20.0, 0.1, 1.0); + motionProfileConstraintScale = MathUtil.clamp(Math.round(scale * 20.0) / 20.0, 0.1, 1.0); applyMotionMagicConfig(); } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 84fb37ec..5c6c5f2f 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -80,6 +80,7 @@ public class Turret extends SubsystemBase { /** The current motor configuration */ private TalonFXConfiguration config; + private double baseMotionMagicCruiseVelocity; private double baseMotionMagicAcceleration; private double motionProfileConstraintScale = 1.0; @@ -541,8 +542,7 @@ public void execute() { TrapezoidProfile profile = new TrapezoidProfile( new TrapezoidProfile.Constraints( - getScaledMotionMagicCruiseVelocity(), - getScaledMotionMagicAcceleration())); + getScaledMotionMagicCruiseVelocity(), getScaledMotionMagicAcceleration())); TrapezoidProfile.State profileState = profile.calculate( From c2766d13fdefcf402c4fd95a61e59680b3eb9d53 Mon Sep 17 00:00:00 2001 From: Mond Date: Thu, 19 Mar 2026 11:16:01 -0700 Subject: [PATCH 03/10] fixes --- simgui-ds.json | 16 ++-- .../lib/swerve/MotionSwerveDrive.java | 44 ----------- .../swerve/commands/DriveToStateCommand.java | 16 +--- .../team6962/lib/swerve/util/FieldLogger.java | 73 +------------------ src/main/java/frc/robot/Preferences.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 13 +++- .../constants/CompetitionBotSimConstants.java | 38 ++++++++++ .../BrownoutProtection.java} | 49 ++++++++----- .../robot/subsystems/hood/ShooterHood.java | 60 +++++---------- .../hopper/beltfloor/BeltFloor.java | 13 +++- .../hopper/beltfloor/BeltFloorSim.java | 4 +- .../intakeextension/IntakeExtension.java | 59 +++++++-------- .../intakerollers/IntakeRollerSim.java | 3 +- .../intakerollers/IntakeRollers.java | 28 +++++-- .../shooterrollers/ShooterRollersSim.java | 4 +- .../frc/robot/subsystems/turret/Turret.java | 46 +++--------- 16 files changed, 186 insertions(+), 284 deletions(-) create mode 100644 src/main/java/frc/robot/constants/CompetitionBotSimConstants.java rename src/main/java/frc/robot/{power/BrownProtection.java => subsystems/BrownoutProtection.java} (65%) diff --git a/simgui-ds.json b/simgui-ds.json index b681e123..d58b23a0 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,6 +4,11 @@ "visible": false } }, + "Keyboard 1 Settings": { + "window": { + "visible": true + } + }, "System Joysticks": { "window": { "visible": false @@ -29,8 +34,8 @@ "incKey": 44 }, { - "decKey": 81, - "incKey": 69 + "decKey": 74, + "incKey": 76 } ], "axisCount": 5, @@ -45,7 +50,7 @@ 77, -1, -1, - 47 + 340 ], "povConfig": [ { @@ -63,10 +68,7 @@ }, { "axisConfig": [ - { - "decKey": 74, - "incKey": 76 - }, + {}, { "decKey": 73, "incKey": 75 diff --git a/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java b/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java index 3e22569e..21bd5e7d 100644 --- a/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java +++ b/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java @@ -35,7 +35,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -87,7 +86,6 @@ public class MotionSwerveDrive implements AutoCloseable { private SwerveDriveKinematics kinematics; private SwerveMotionManager motionManager; private volatile double velocityScale = 1.0; - private volatile double motionConstraintScale = 1.0; /** * Creates a new MotionSwerveDrive with the specified drivetrain configuration. @@ -525,48 +523,6 @@ public double getVelocityScale() { return velocityScale; } - /** - * Sets runtime scale applied to auton drivetrain motion profile constraints. - * - * @param scale the constraint scale to apply - */ - public void setMotionConstraintScale(double scale) { - motionConstraintScale = MathUtil.clamp(scale, 0.0, 1.0); - } - - /** - * Gets runtime scale currently applied to auton drivetrain motion profile constraints. - * - * @return the constraint scale - */ - public double getMotionConstraintScale() { - return motionConstraintScale; - } - - /** - * Gets auton translation constraints after applying current runtime scale. - * - * @return scaled auton translation constraints - */ - public TrapezoidProfile.Constraints getScaledTranslationConstraints() { - return new TrapezoidProfile.Constraints( - constants.Driving.AutoLinearVelocity.in(MetersPerSecond) * motionConstraintScale, - constants.Driving.AutoLinearAcceleration.in(MetersPerSecondPerSecond) - * motionConstraintScale); - } - - /** - * Gets auton rotation constraints after applying current runtime scale. - * - * @return scaled auton rotation constraints - */ - public TrapezoidProfile.Constraints getScaledRotationConstraints() { - return new TrapezoidProfile.Constraints( - constants.Driving.AutoAngularVelocity.in(RadiansPerSecond) * motionConstraintScale, - constants.Driving.AutoAngularAcceleration.in(RadiansPerSecondPerSecond) - * motionConstraintScale); - } - /** * Applies a motion to be executed during the next control loop cycle. * diff --git a/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java b/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java index 9b80df86..d32147cb 100644 --- a/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java +++ b/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java @@ -48,9 +48,6 @@ public class DriveToStateCommand extends Command { /** Whether the motion profiles have finished at least once. */ private boolean motionProfilesFinished = false; - /** Most recent drivetrain motion profile scale used to create controllers. */ - private double previousMotionConstraintScale = Double.NaN; - /** Class representing the target state for the drive command. */ public static class State { /** The target translation position. If null, the robot translation will not be controlled. */ @@ -183,17 +180,15 @@ public void initialize() { DogLog.log("Drivetrain/DriveToState/FinalAngularVelocity", target.angularVelocity); } - /** Creates controllers for translation and rotation using the current scaled constraints. */ + /** Creates controllers for translation and rotation using the configured constraints. */ private void createControllers() { - previousMotionConstraintScale = swerveDrive.getMotionConstraintScale(); - if (target.translation != null) { translationController = new TranslationController( swerveDrive.getConstants().Driving.TranslationFeedbackKP, swerveDrive.getConstants().Driving.TranslationFeedbackKI, swerveDrive.getConstants().Driving.TranslationFeedbackKD, - swerveDrive.getScaledTranslationConstraints(), + swerveDrive.getConstants().Driving.getTranslationConstraints(), Hertz.of(50)); } @@ -203,7 +198,7 @@ private void createControllers() { swerveDrive.getConstants().Driving.AngleFeedbackKP, swerveDrive.getConstants().Driving.AngleFeedbackKI, swerveDrive.getConstants().Driving.AngleFeedbackKD, - new TrapezoidalProfile(swerveDrive.getScaledRotationConstraints()), + new TrapezoidalProfile(swerveDrive.getConstants().Driving.getRotationConstraints()), Hertz.of(50)); } } @@ -243,11 +238,6 @@ private void createMotionProfiles() { @Override public void execute() { - if (Math.abs(swerveDrive.getMotionConstraintScale() - previousMotionConstraintScale) > 0.05) { - createControllers(); - createMotionProfiles(); - } - // Check if motion profiles have finished if ((translationController == null || translationController.isFinished()) && (headingController == null || headingController.isFinished())) { diff --git a/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java b/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java index b5fcd3a2..31bb2a70 100644 --- a/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java +++ b/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java @@ -7,7 +7,6 @@ import com.team6962.lib.swerve.localization.Odometry; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -41,43 +40,6 @@ public class FieldLogger implements SwerveComponent { /** Cached previous pose to detect user-initiated changes. */ private Pose2d previousRobotPose; - /** Notifier for periodic field logging updates. */ - private Notifier fieldLoggerNotifier; - - /** Data structure to hold the latest robot and module poses for logging. */ - private LogData logData; - - /** - * Data structure to hold the robot pose and module poses for logging. This is cloned before being - * accessed in the logging thread to avoid concurrency issues. - */ - private static class LogData implements Cloneable { - /** The robot's current pose on the field. */ - public Pose2d robotPose; - - /** The poses of the swerve modules relative to the robot. */ - public Pose2d[] modulePoses; - - public LogData(Pose2d robotPose, Pose2d[] modulePoses) { - this.robotPose = robotPose; - this.modulePoses = modulePoses; - } - - @Override - public LogData clone() { - Pose2d clonedRobotPose = new Pose2d(robotPose.getTranslation(), robotPose.getRotation()); - Pose2d[] clonedModulePoses = null; - if (modulePoses != null) { - clonedModulePoses = new Pose2d[modulePoses.length]; - for (int i = 0; i < modulePoses.length; i++) { - clonedModulePoses[i] = - new Pose2d(modulePoses[i].getTranslation(), modulePoses[i].getRotation()); - } - } - return new LogData(clonedRobotPose, clonedModulePoses); - } - } - /** * Creates a new FieldLogger, which logs robot and module poses to NetworkTables. * @@ -89,33 +51,6 @@ public FieldLogger(DrivetrainConstants constants, Localization localization, Odo this.constants = constants; this.localization = localization; this.odometry = odometry; - fieldLoggerNotifier = new Notifier(this::threadedLog); - fieldLoggerNotifier.setName("FieldLogger"); - fieldLoggerNotifier.startPeriodic(0.02); - } - - /** - * Logs the robot pose and module poses to the Field2d widget. This method is called periodically - * by the Notifier in a separate thread. - */ - private void threadedLog() { - LogData dataCopy; - - synchronized (this) { - if (logData == null) { - return; - } - - dataCopy = logData.clone(); - } - - if (dataCopy.robotPose != null) { - field.setRobotPose(dataCopy.robotPose); - } - - if (dataCopy.modulePoses != null) { - field.getObject("Swerve Modules").setPoses(dataCopy.modulePoses); - } } @Override @@ -153,11 +88,11 @@ public void logTelemetry(String basePath) { previousRobotPose = robotPose; - synchronized (this) { - logData = new LogData(robotPose, modulePoses); - } - field.setRobotPose(robotPose); + + if (modulePoses != null) { + field.getObject("Swerve Modules").setPoses(modulePoses); + } } /** diff --git a/src/main/java/frc/robot/Preferences.java b/src/main/java/frc/robot/Preferences.java index 675472cb..59daffe6 100644 --- a/src/main/java/frc/robot/Preferences.java +++ b/src/main/java/frc/robot/Preferences.java @@ -1,6 +1,6 @@ package frc.robot; -import frc.robot.constants.CompetitionBotConstants; +import frc.robot.constants.CompetitionBotSimConstants; import frc.robot.constants.RobotConstants; public class Preferences { @@ -27,5 +27,5 @@ public class Preferences { * corresponds to the robot being simulated. This is used to determine which subsystems exist on * the simulated robot and to provide the correct configuration values for those subsystems. */ - public static final RobotConstants simulatedRobot = new CompetitionBotConstants(); + public static final RobotConstants simulatedRobot = new CompetitionBotSimConstants(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d4453bb4..3b00595e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,7 +26,7 @@ import frc.robot.auto.shoot.ShooterFunctions; import frc.robot.constants.RobotConstants; import frc.robot.controls.TeleopControls; -import frc.robot.power.BrownProtection; +import frc.robot.subsystems.BrownoutProtection; import frc.robot.subsystems.hood.ShooterHood; import frc.robot.subsystems.hopper.Hopper; import frc.robot.subsystems.intakeextension.IntakeExtension; @@ -55,7 +55,7 @@ public class RobotContainer { private final ShooterFunctions hubFunctions; private final ShooterFunctions passFunctions; private final Autonomous autonomous; - private final BrownProtection brownoutProtection; + private final BrownoutProtection brownoutProtection; private final Command noneAutonomous = Commands.none(); public RobotContainer() { @@ -76,7 +76,14 @@ public RobotContainer() { turret = new Turret(); intakeExtension = new IntakeExtension(); hopper = new Hopper(); - brownoutProtection = new BrownProtection(swerveDrive, turret, shooterHood, intakeExtension); + brownoutProtection = + new BrownoutProtection( + swerveDrive, + turret, + shooterHood, + intakeExtension, + hopper.getBeltFloor(), + intakeRollers); aprilTagVision = new AprilTagVision(swerveDrive, constants.getAprilTagVisionConstants()); fuelClumpLocalization = diff --git a/src/main/java/frc/robot/constants/CompetitionBotSimConstants.java b/src/main/java/frc/robot/constants/CompetitionBotSimConstants.java new file mode 100644 index 00000000..13dbdd94 --- /dev/null +++ b/src/main/java/frc/robot/constants/CompetitionBotSimConstants.java @@ -0,0 +1,38 @@ +package frc.robot.constants; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; + +import com.team6962.lib.swerve.config.DrivetrainConstants; + +/** + * Simulation-only tuning for the competition robot. + * + *

This keeps simulation realism improvements isolated from the real robot constants so we can + * make autos and driver practice behave better in sim without changing match behavior on hardware. + */ +public class CompetitionBotSimConstants extends CompetitionBotConstants { + @Override + public DrivetrainConstants getDrivetrainConstants() { + DrivetrainConstants constants = super.getDrivetrainConstants(); + + return constants + .withStructure(constants.Structure.clone().estimateMomentOfInertia()) + .withDriving( + constants.Driving.clone() + .withAutoLinearVelocity(MetersPerSecond.of(3.2)) + .withAutoLinearAcceleration(MetersPerSecondPerSecond.of(2.6)) + .withAutoAngularVelocity(RotationsPerSecond.of(0.85)) + .withAutoAngularAcceleration(RotationsPerSecondPerSecond.of(0.4)) + .withTranslationFeedbackKP(8.0) + .withTranslationFeedbackKD(0.45) + .withAngleFeedbackKP(16.0) + .withAngleFeedbackKD(0.9)) + .withSimulation( + constants.Simulation.clone() + .withSimulationSubTicksPerPeriod(4) + .withEnableRampCollider(true)); + } +} diff --git a/src/main/java/frc/robot/power/BrownProtection.java b/src/main/java/frc/robot/subsystems/BrownoutProtection.java similarity index 65% rename from src/main/java/frc/robot/power/BrownProtection.java rename to src/main/java/frc/robot/subsystems/BrownoutProtection.java index 2fa449eb..760936a3 100644 --- a/src/main/java/frc/robot/power/BrownProtection.java +++ b/src/main/java/frc/robot/subsystems/BrownoutProtection.java @@ -1,4 +1,4 @@ -package frc.robot.power; +package frc.robot.subsystems; import static edu.wpi.first.units.Units.Amps; @@ -11,18 +11,22 @@ import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.hood.ShooterHood; +import frc.robot.subsystems.hopper.beltfloor.BeltFloor; import frc.robot.subsystems.intakeextension.IntakeExtension; +import frc.robot.subsystems.intakerollers.IntakeRollers; import frc.robot.subsystems.turret.Turret; /** * Dynamically slows drivetrain and lower-priority mechanism motion when battery voltage drops or * robot is drawing too high of a current. */ -public class BrownProtection extends SubsystemBase { +public class BrownoutProtection extends SubsystemBase { private final CommandSwerveDrive swerveDrive; private final Turret turret; private final ShooterHood shooterHood; private final IntakeExtension intakeExtension; + private final BeltFloor beltFloor; + private final IntakeRollers intakeRollers; private final LinearFilter batteryVoltageFilter = LinearFilter.movingAverage(10); private final LinearFilter totalCurrentFilter = LinearFilter.movingAverage(10); @@ -34,31 +38,35 @@ public class BrownProtection extends SubsystemBase { private double minimumDriveScale = 0.45; private double minimumMechanismScale = 0.2; - public BrownProtection( + public BrownoutProtection( CommandSwerveDrive swerveDrive, Turret turret, ShooterHood shooterHood, - IntakeExtension intakeExtension) { + IntakeExtension intakeExtension, + BeltFloor beltFloor, + IntakeRollers intakeRollers) { this.swerveDrive = swerveDrive; this.turret = turret; this.shooterHood = shooterHood; this.intakeExtension = intakeExtension; + this.beltFloor = beltFloor; + this.intakeRollers = intakeRollers; DogLog.tunable( - "Brownout/Start Limiting Voltage", + "BrownoutProtection/Start Limiting Voltage", startLimitingVoltage, value -> startLimitingVoltage = value); - DogLog.tunable("Brownout/Minimum Voltage", minimumVoltage, value -> minimumVoltage = value); + DogLog.tunable("BrownoutProtection/Minimum Voltage", minimumVoltage, value -> minimumVoltage = value); DogLog.tunable( - "Brownout/Start Limiting Current", + "BrownoutProtection/Start Limiting Current", startLimitingCurrentAmps, value -> startLimitingCurrentAmps = value); DogLog.tunable( - "Brownout/Maximum Current", maximumCurrentAmps, value -> maximumCurrentAmps = value); + "BrownoutProtection/Maximum Current", maximumCurrentAmps, value -> maximumCurrentAmps = value); DogLog.tunable( - "Brownout/Minimum Drive Scale", minimumDriveScale, value -> minimumDriveScale = value); + "BrownoutProtection/Minimum Drive Scale", minimumDriveScale, value -> minimumDriveScale = value); DogLog.tunable( - "Brownout/Minimum Mechanism Scale", + "BrownoutProtection/Minimum Mechanism Scale", minimumMechanismScale, value -> minimumMechanismScale = value); } @@ -80,20 +88,21 @@ public void periodic() { double mechanismScale = MathUtil.interpolate(minimumMechanismScale, 1.0, protectionBudget); swerveDrive.setVelocityScale(driveScale); - swerveDrive.setMotionConstraintScale(driveScale); turret.setMotionProfileConstraintScale(mechanismScale); shooterHood.setMotionProfileConstraintScale(mechanismScale); intakeExtension.setMotionProfileConstraintScale(mechanismScale); + beltFloor.setVoltageScale(mechanismScale); + intakeRollers.setVoltageScale(mechanismScale); - DogLog.log("Brownout/BatteryVoltage", batteryVoltage); - DogLog.log("Brownout/FilteredBatteryVoltage", filteredBatteryVoltage); - DogLog.log("Brownout/TotalCurrentAmps", totalCurrentAmps); - DogLog.log("Brownout/FilteredCurrentAmps", filteredTotalCurrentAmps); - DogLog.log("Brownout/VoltageBudget", voltageBudget); - DogLog.log("Brownout/CurrentBudget", currentBudget); - DogLog.log("Brownout/ProtectionBudget", protectionBudget); - DogLog.log("Brownout/DriveScale", driveScale); - DogLog.log("Brownout/MechanismScale", mechanismScale); + DogLog.log("BrownoutProtection/BatteryVoltage", batteryVoltage); + DogLog.log("BrownoutProtection/FilteredBatteryVoltage", filteredBatteryVoltage); + DogLog.log("BrownoutProtection/TotalCurrentAmps", totalCurrentAmps); + DogLog.log("BrownoutProtection/FilteredCurrentAmps", filteredTotalCurrentAmps); + DogLog.log("BrownoutProtection/VoltageBudget", voltageBudget); + DogLog.log("BrownoutProtection/CurrentBudget", currentBudget); + DogLog.log("BrownoutProtection/ProtectionBudget", protectionBudget); + DogLog.log("BrownoutProtection/DriveScale", driveScale); + DogLog.log("BrownoutProtection/MechanismScale", mechanismScale); } private static double unitRange(double value, double minimumValue, double maximumValue) { diff --git a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java index bdec78de..66736aff 100644 --- a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java +++ b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java @@ -9,8 +9,8 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.controls.CoastOut; +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionVoltage; @@ -63,8 +63,6 @@ public class ShooterHood extends SubsystemBase { private double baseMotionMagicAcceleration = ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration; private double motionProfileConstraintScale = 1.0; - private double appliedCruiseVelocity = Double.NaN; - private double appliedAcceleration = Double.NaN; private Supplier shouldLowerHoodSupplier; @@ -170,7 +168,6 @@ public ShooterHood(Supplier shouldLowerHoodSupplier) { baseMotionMagicCruiseVelocity, newCruiseVelocity -> { baseMotionMagicCruiseVelocity = newCruiseVelocity; - applyMotionMagicConfig(); }); DogLog.tunable( @@ -178,7 +175,6 @@ public ShooterHood(Supplier shouldLowerHoodSupplier) { baseMotionMagicAcceleration, newAcceleration -> { baseMotionMagicAcceleration = newAcceleration; - applyMotionMagicConfig(); }); DogLog.tunable( @@ -196,7 +192,6 @@ public ShooterHood(Supplier shouldLowerHoodSupplier) { } CurrentDrawLogger.add("Shooter Hood", this::getSupplyCurrent); - applyMotionMagicConfig(); } @Override @@ -240,9 +235,11 @@ public void periodic() { // LoggingUtil.log("Hood/ControlRequest", hoodMotor.getAppliedControl()); - // Update the currently applied control request with a new gravity feedforward - // voltage if the control request is a MotionMagicVoltage - if (hoodMotor.getAppliedControl() instanceof MotionMagicVoltage motionMagicControlRequest) { + // Update currently applied control request with the latest gravity feedforward and + // brownout-scaled constraints if the hood is using Motion Magic position control. + if (hoodMotor.getAppliedControl() instanceof DynamicMotionMagicVoltage dynamicControlRequest) { + setPositionControl(Rotations.of(dynamicControlRequest.Position)); + } else if (hoodMotor.getAppliedControl() instanceof MotionMagicVoltage motionMagicControlRequest) { setPositionControl(motionMagicControlRequest.getPositionMeasure()); } @@ -401,22 +398,24 @@ public Command moveAtVoltage(Voltage voltage) { } /** - * Applies a control request to move to a specific position with a MotionMagicVoltage control + * Applies a control request to move to a specific position with a Dynamic Motion Magic voltage * request, including an additional gravity feedforward. The gravity feedforward will be - * automatically updated in periodic(). + * automatically updated in periodic. * * @param position The target position to move to. */ private void setPositionControl(Angle position) { if (!isZeroed) { hoodMotor.setControl(new NeutralOut()); - } else if (shouldLowerHoodSupplier.get()) { - hoodMotor.setControl( - new MotionMagicVoltage(ShooterHoodConstants.MIN_ANGLE) - .withFeedForward(Math.cos(getPosition().in(Radians)) * kG)); } else { + Angle targetPosition = + shouldLowerHoodSupplier.get() ? ShooterHoodConstants.MIN_ANGLE : position; hoodMotor.setControl( - new MotionMagicVoltage(position) + new DynamicMotionMagicVoltage( + targetPosition.in(Rotations), + getScaledMotionMagicCruiseVelocity(), + getScaledMotionMagicAcceleration()) + .withJerk(ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicJerk) .withFeedForward(Math.cos(getPosition().in(Radians)) * kG)); } } @@ -425,9 +424,7 @@ private void setPositionVelocityControl(Angle position, AngularVelocity velocity if (!isZeroed) { hoodMotor.setControl(new NeutralOut()); } else if (shouldLowerHoodSupplier.get()) { - hoodMotor.setControl( - new MotionMagicVoltage(ShooterHoodConstants.MIN_ANGLE) - .withFeedForward(Math.cos(getPosition().in(Radians)) * kG)); + setPositionControl(ShooterHoodConstants.MIN_ANGLE); } else { hoodMotor.setControl( new PositionVoltage(position) @@ -511,8 +508,7 @@ public void execute() { } public void setMotionProfileConstraintScale(double scale) { - motionProfileConstraintScale = MathUtil.clamp(Math.round(scale * 20.0) / 20.0, 0.1, 1.0); - applyMotionMagicConfig(); + motionProfileConstraintScale = MathUtil.clamp(scale, 0.1, 1.0); } private double getScaledMotionMagicCruiseVelocity() { @@ -523,28 +519,6 @@ private double getScaledMotionMagicAcceleration() { return baseMotionMagicAcceleration * motionProfileConstraintScale; } - private void applyMotionMagicConfig() { - double scaledCruiseVelocity = getScaledMotionMagicCruiseVelocity(); - double scaledAcceleration = getScaledMotionMagicAcceleration(); - - if (Math.abs(scaledCruiseVelocity - appliedCruiseVelocity) < 1e-6 - && Math.abs(scaledAcceleration - appliedAcceleration) < 1e-6) { - return; - } - - hoodMotor - .getConfigurator() - .apply( - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(scaledCruiseVelocity) - .withMotionMagicAcceleration(scaledAcceleration) - .withMotionMagicJerk( - ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicJerk)); - - appliedCruiseVelocity = scaledCruiseVelocity; - appliedAcceleration = scaledAcceleration; - } - /** * Zeroes the hood without the hall effect sensor by assuming the hood is at the minimum angle. * diff --git a/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloor.java b/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloor.java index e6d63b74..7b8e4c74 100644 --- a/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloor.java +++ b/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloor.java @@ -13,6 +13,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.team6962.lib.logging.CurrentDrawLogger; import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -36,6 +37,7 @@ public class BeltFloor extends SubsystemBase { private StatusSignal statorCurrentSignal; private StatusSignal voltageSignal; private BeltFloorSim simulation; + private double voltageScale = 1.0; /** * Initializes the motor controller, configures status signals for logging, and sets up DogLog @@ -78,11 +80,11 @@ public BeltFloor() { * motor go clockwise. */ private Command feedDump(Voltage targetVoltage) { - - return startEnd( + return runEnd( () -> { // defines a local function to set motor voltage to make it go - beltFloorMotor.setControl(new VoltageOut(targetVoltage).withEnableFOC(false)); + beltFloorMotor.setControl( + new VoltageOut(targetVoltage.times(voltageScale)).withEnableFOC(false)); }, () -> { // defines a local function to stop motor @@ -148,6 +150,11 @@ public void periodic() { DogLog.log("Hopper/BeltFloor/StatorCurrent", getStatorCurrent()); DogLog.log("Hopper/BeltFloor/AngularAcceleration", getAngularAcceleration()); DogLog.log("Hopper/BeltFloor/SupplyCurrent", getSupplyCurrent()); + DogLog.log("Hopper/BeltFloor/VoltageScale", voltageScale); + } + + public void setVoltageScale(double scale) { + voltageScale = MathUtil.clamp(scale, 0.0, 1.0); } /** gets the angular velocity */ diff --git a/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloorSim.java b/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloorSim.java index 5bf61188..ac0e2c5c 100644 --- a/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloorSim.java +++ b/src/main/java/frc/robot/subsystems/hopper/beltfloor/BeltFloorSim.java @@ -33,10 +33,10 @@ public BeltFloorSim(TalonFX motor) { public void update() { motorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); double motorVoltage = invert(motorSim.getMotorVoltage(), false); - Angle position = physicsSim.getAngularPosition(); - AngularVelocity velocity = RadiansPerSecond.of(physicsSim.getAngularVelocityRadPerSec()); physicsSim.setInput(motorVoltage); physicsSim.update(0.02); + Angle position = physicsSim.getAngularPosition(); + AngularVelocity velocity = RadiansPerSecond.of(physicsSim.getAngularVelocityRadPerSec()); motorSim.setRawRotorPosition( invert(position, false) .times(HopperConstants.BELT_FLOOR_MOTOR_CONFIG.Feedback.SensorToMechanismRatio)); diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java index acbea7da..7165103a 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java @@ -12,8 +12,8 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANdi; @@ -62,8 +62,6 @@ public class IntakeExtension extends SubsystemBase { private double baseMotionMagicAcceleration = IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicAcceleration; private double motionProfileConstraintScale = 1.0; - private double appliedCruiseVelocity = Double.NaN; - private double appliedAcceleration = Double.NaN; public boolean isZeroed = false; @@ -175,7 +173,6 @@ public IntakeExtension() { baseMotionMagicCruiseVelocity, newVelocity -> { baseMotionMagicCruiseVelocity = newVelocity; - applyMotionMagicConfig(); }); DogLog.tunable( @@ -183,7 +180,6 @@ public IntakeExtension() { baseMotionMagicAcceleration, newAcceleration -> { baseMotionMagicAcceleration = newAcceleration; - applyMotionMagicConfig(); }); if (RobotBase.isSimulation()) { @@ -194,34 +190,27 @@ public IntakeExtension() { } CurrentDrawLogger.add("Intake Extension", this::getSupplyCurrent); - applyMotionMagicConfig(); } public void setMotionProfileConstraintScale(double scale) { - motionProfileConstraintScale = MathUtil.clamp(Math.round(scale * 20.0) / 20.0, 0.1, 1.0); - applyMotionMagicConfig(); + motionProfileConstraintScale = MathUtil.clamp(scale, 0.1, 1.0); } - private void applyMotionMagicConfig() { - double scaledCruiseVelocity = baseMotionMagicCruiseVelocity * motionProfileConstraintScale; - double scaledAcceleration = baseMotionMagicAcceleration * motionProfileConstraintScale; + private double getScaledMotionMagicCruiseVelocity() { + return baseMotionMagicCruiseVelocity * motionProfileConstraintScale; + } - if (Math.abs(scaledCruiseVelocity - appliedCruiseVelocity) < 1e-6 - && Math.abs(scaledAcceleration - appliedAcceleration) < 1e-6) { - return; - } + private double getScaledMotionMagicAcceleration() { + return baseMotionMagicAcceleration * motionProfileConstraintScale; + } - motor - .getConfigurator() - .apply( - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(scaledCruiseVelocity) - .withMotionMagicAcceleration(scaledAcceleration) - .withMotionMagicJerk( - IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicJerk)); - - appliedCruiseVelocity = scaledCruiseVelocity; - appliedAcceleration = scaledAcceleration; + private void setPositionControl(Distance position) { + motor.setControl( + new DynamicMotionMagicVoltage( + position.in(Meters), + getScaledMotionMagicCruiseVelocity(), + getScaledMotionMagicAcceleration()) + .withJerk(IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicJerk)); } /** @@ -233,15 +222,14 @@ private void applyMotionMagicConfig() { public Command extend() { return startEnd( () -> { - motor.setControl( - new MotionMagicVoltage(IntakeExtensionConstants.MAX_POSITION.in(Meters))); + setPositionControl(IntakeExtensionConstants.MAX_POSITION); }, () -> { if (!getPosition() .isNear( IntakeExtensionConstants.MAX_POSITION, IntakeExtensionConstants.POSITION_TOLERANCE)) { - motor.setControl(new MotionMagicVoltage(getPosition().in(Meters))); + setPositionControl(getPosition()); } }) .until( @@ -261,15 +249,14 @@ public Command extend() { public Command retract() { return startEnd( () -> { - motor.setControl( - new MotionMagicVoltage(IntakeExtensionConstants.RETRACT_POSITION.in(Meters))); + setPositionControl(IntakeExtensionConstants.RETRACT_POSITION); }, () -> { if (!getPosition() .isNear( IntakeExtensionConstants.RETRACT_POSITION, IntakeExtensionConstants.POSITION_TOLERANCE)) { - motor.setControl(new MotionMagicVoltage(getPosition().in(Meters))); + setPositionControl(getPosition()); } }) .until( @@ -301,7 +288,7 @@ public Command moveAtVoltage(Voltage voltage) { .plus(Volts.of(IntakeExtensionConstants.MOTOR_CONFIGURATION.Slot0.kG)))); }, () -> { - motor.setControl(new MotionMagicVoltage(getPosition().in(Meters))); + setPositionControl(getPosition()); }) .onlyIf(() -> isZeroed || voltage.in(Volts) < 0); } @@ -435,5 +422,11 @@ && getPosition().lt(IntakeExtensionConstants.MIN_POSITION)) { DogLog.log("Intake/ClosedLoopReference", getClosedLoopReference()); DogLog.forceNt.log("Intake/IsZeroed", isZeroed); DogLog.log("Intake/MotionMagicScale", motionProfileConstraintScale); + + if (motor.getAppliedControl() instanceof DynamicMotionMagicVoltage dynamicControlRequest) { + setPositionControl(Meters.of(dynamicControlRequest.Position)); + } else if (motor.getAppliedControl() instanceof MotionMagicVoltage motionMagicControlRequest) { + setPositionControl(Meters.of(motionMagicControlRequest.Position)); + } } } diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollerSim.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollerSim.java index 517e75ff..1e981e3f 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollerSim.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollerSim.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DCMotorSim; public class IntakeRollerSim { @@ -28,7 +29,7 @@ public IntakeRollerSim(TalonFX motor) { /** updates physicsSim */ public void update() { - intakeMotor.setSupplyVoltage(12); + intakeMotor.setSupplyVoltage(RobotController.getBatteryVoltage()); physicsSim.setInputVoltage(intakeMotor.getMotorVoltage()); physicsSim.update(0.02); diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java index ff6b83af..75cc914f 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java @@ -7,18 +7,19 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.CoastOut; -import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.phoenix.StatusUtil; import dev.doglog.DogLog; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -33,6 +34,7 @@ public class IntakeRollers extends SubsystemBase { private double intakeStallVoltage = 12.0; private Debouncer stallDebouncer = new Debouncer(0.25, DebounceType.kRising); private boolean stalling = false; + private double voltageScale = 1.0; /** Intializes motor and status signals Class for Intake Rollers */ public IntakeRollers() { @@ -68,9 +70,9 @@ public IntakeRollers() { /** Returns command to make the motor move and stop */ @SuppressWarnings("unused") private Command move(Voltage voltage) { - return startEnd( + return runEnd( () -> { - intakeMotor.setControl(new VoltageOut(voltage).withEnableFOC(false)); + intakeMotor.setControl(new VoltageOut(voltage.times(voltageScale)).withEnableFOC(false)); }, () -> { intakeMotor.setControl(new CoastOut()); @@ -86,7 +88,8 @@ public Command intake() { return runEnd( () -> { intakeMotor.setControl( - new VoltageOut(stalling ? intakeStallVoltage : intakeVoltage).withEnableFOC(false)); + new VoltageOut((stalling ? intakeStallVoltage : intakeVoltage) * voltageScale) + .withEnableFOC(false)); }, () -> { intakeMotor.setControl(new CoastOut()); @@ -100,9 +103,11 @@ public Command intake() { * @return The command to intake fuel at full speed. */ public Command intakeFast() { - return startEnd( + return runEnd( () -> { - intakeMotor.setControl(new DutyCycleOut(1).withEnableFOC(false)); + intakeMotor.setControl( + new VoltageOut(RobotController.getBatteryVoltage() * voltageScale) + .withEnableFOC(false)); }, () -> { intakeMotor.setControl(new CoastOut()); @@ -115,9 +120,11 @@ public Command intakeFast() { * @return */ public Command outtake() { - return startEnd( + return runEnd( () -> { - intakeMotor.setControl(new DutyCycleOut(-1).withEnableFOC(false)); + intakeMotor.setControl( + new VoltageOut(-RobotController.getBatteryVoltage() * voltageScale) + .withEnableFOC(false)); }, () -> { intakeMotor.setControl(new CoastOut()); @@ -173,9 +180,14 @@ public void periodic() { DogLog.log("intakeRollers/supplyCurrent", getSupplyCurrent()); DogLog.log("intakeRollers/appliedVoltage", getAppliedVoltage()); DogLog.log("intakeRollers/stalling", stalling); + DogLog.log("intakeRollers/voltageScale", voltageScale); stalling = stallDebouncer.calculate( getVelocity().abs(RotationsPerSecond) < 1.0 && getStatorCurrent().abs(Amps) > 100.0); } + + public void setVoltageScale(double scale) { + voltageScale = MathUtil.clamp(scale, 0.0, 1.0); + } } diff --git a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersSim.java b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersSim.java index 9b0478bf..358dcb79 100644 --- a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersSim.java +++ b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersSim.java @@ -33,10 +33,10 @@ public ShooterRollersSim(TalonFX motor) { public void update() { motorSim.setSupplyVoltage(RobotController.getBatteryVoltage()); double motorVoltage = invert(motorSim.getMotorVoltage(), false); - Angle position = physicsSim.getAngularPosition(); - AngularVelocity velocity = RadiansPerSecond.of(physicsSim.getAngularVelocityRadPerSec()); physicsSim.setInput(motorVoltage); physicsSim.update(0.02); + Angle position = physicsSim.getAngularPosition(); + AngularVelocity velocity = RadiansPerSecond.of(physicsSim.getAngularVelocityRadPerSec()); motorSim.setRawRotorPosition( invert(position, false) .times(ShooterRollersConstants.MOTOR_CONFIGURATION.Feedback.SensorToMechanismRatio)); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 5c6c5f2f..3c641561 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -9,9 +9,9 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.CoastOut; +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionVoltage; @@ -84,8 +84,6 @@ public class Turret extends SubsystemBase { private double baseMotionMagicCruiseVelocity; private double baseMotionMagicAcceleration; private double motionProfileConstraintScale = 1.0; - private double appliedCruiseVelocity = Double.NaN; - private double appliedAcceleration = Double.NaN; /** Assigns Status Signal variables to the different methods part of the motor.get() */ public Turret() { @@ -180,8 +178,6 @@ public Turret() { baseMotionMagicCruiseVelocity, newVel -> { baseMotionMagicCruiseVelocity = newVel; - config.MotionMagic.MotionMagicCruiseVelocity = newVel; - applyMotionMagicConfig(); }); DogLog.tunable( @@ -189,8 +185,6 @@ public Turret() { baseMotionMagicAcceleration, newAccel -> { baseMotionMagicAcceleration = newAccel; - config.MotionMagic.MotionMagicAcceleration = newAccel; - applyMotionMagicConfig(); }); DogLog.tunable("Turret/Turret kW", TurretConstants.kW, newKW -> TurretConstants.kW = newKW); @@ -216,7 +210,6 @@ public Turret() { } CurrentDrawLogger.add("Turret", this::getSupplyCurrent); - applyMotionMagicConfig(); } @Override @@ -264,7 +257,9 @@ public void periodic() { // LoggingUtil.log("Turret/ControlRequest", motor.getAppliedControl()); - if (motor.getAppliedControl() instanceof MotionMagicVoltage control) { + if (motor.getAppliedControl() instanceof DynamicMotionMagicVoltage dynamicControlRequest) { + setPositionControl(Rotations.of(dynamicControlRequest.Position)); + } else if (motor.getAppliedControl() instanceof MotionMagicVoltage control) { setPositionControl(control.getPositionMeasure()); } } @@ -601,15 +596,19 @@ public Command moveTo(Supplier targetAngleSupplier) { } /** - * Applies a Motion Magic position control request to move the turret to the target angle. If the - * turret is not yet zeroed, the motor is set to neutral mode instead. + * Applies a Dynamic Motion Magic position control request to move the turret to the target + * angle. If the turret is not yet zeroed, the motor is set to neutral mode instead. * * @param targetAngle The target angle to move the turret to */ private void setPositionControl(Angle targetAngle) { if (isZeroed()) { motor.setControl( - new MotionMagicVoltage(targetAngle) + new DynamicMotionMagicVoltage( + targetAngle.in(Rotations), + getScaledMotionMagicCruiseVelocity(), + getScaledMotionMagicAcceleration()) + .withJerk(config.MotionMagic.MotionMagicJerk) .withFeedForward( targetAngle.lt(TurretConstants.MIN_KW_ANGLE) ? -TurretConstants.kW @@ -649,8 +648,7 @@ public Command moveAtVoltage(Voltage voltage) { } public void setMotionProfileConstraintScale(double scale) { - motionProfileConstraintScale = MathUtil.clamp(Math.round(scale * 20.0) / 20.0, 0.1, 1.0); - applyMotionMagicConfig(); + motionProfileConstraintScale = MathUtil.clamp(scale, 0.1, 1.0); } private double getScaledMotionMagicCruiseVelocity() { @@ -661,26 +659,6 @@ private double getScaledMotionMagicAcceleration() { return baseMotionMagicAcceleration * motionProfileConstraintScale; } - private void applyMotionMagicConfig() { - double scaledCruiseVelocity = getScaledMotionMagicCruiseVelocity(); - double scaledAcceleration = getScaledMotionMagicAcceleration(); - - if (Math.abs(scaledCruiseVelocity - appliedCruiseVelocity) < 1e-6 - && Math.abs(scaledAcceleration - appliedAcceleration) < 1e-6) { - return; - } - - motor - .getConfigurator() - .apply( - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(scaledCruiseVelocity) - .withMotionMagicAcceleration(scaledAcceleration)); - - appliedCruiseVelocity = scaledCruiseVelocity; - appliedAcceleration = scaledAcceleration; - } - public void zero() { motor.setPosition(Degrees.of(180)); } From 50b12a7787af698d07c7604dcb7d495d44b7380c Mon Sep 17 00:00:00 2001 From: Mond-Mann <63756512+Mond-Mann@users.noreply.github.com> Date: Thu, 19 Mar 2026 18:25:55 +0000 Subject: [PATCH 04/10] style: Apply Spotless fixes --- .../com/team6962/lib/swerve/MotionSwerveDrive.java | 2 -- .../java/frc/robot/subsystems/BrownoutProtection.java | 11 ++++++++--- .../java/frc/robot/subsystems/hood/ShooterHood.java | 3 ++- src/main/java/frc/robot/subsystems/turret/Turret.java | 4 ++-- 4 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java b/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java index 21bd5e7d..1cadfed4 100644 --- a/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java +++ b/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java @@ -2,10 +2,8 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; diff --git a/src/main/java/frc/robot/subsystems/BrownoutProtection.java b/src/main/java/frc/robot/subsystems/BrownoutProtection.java index 760936a3..86ca46ca 100644 --- a/src/main/java/frc/robot/subsystems/BrownoutProtection.java +++ b/src/main/java/frc/robot/subsystems/BrownoutProtection.java @@ -56,15 +56,20 @@ public BrownoutProtection( "BrownoutProtection/Start Limiting Voltage", startLimitingVoltage, value -> startLimitingVoltage = value); - DogLog.tunable("BrownoutProtection/Minimum Voltage", minimumVoltage, value -> minimumVoltage = value); + DogLog.tunable( + "BrownoutProtection/Minimum Voltage", minimumVoltage, value -> minimumVoltage = value); DogLog.tunable( "BrownoutProtection/Start Limiting Current", startLimitingCurrentAmps, value -> startLimitingCurrentAmps = value); DogLog.tunable( - "BrownoutProtection/Maximum Current", maximumCurrentAmps, value -> maximumCurrentAmps = value); + "BrownoutProtection/Maximum Current", + maximumCurrentAmps, + value -> maximumCurrentAmps = value); DogLog.tunable( - "BrownoutProtection/Minimum Drive Scale", minimumDriveScale, value -> minimumDriveScale = value); + "BrownoutProtection/Minimum Drive Scale", + minimumDriveScale, + value -> minimumDriveScale = value); DogLog.tunable( "BrownoutProtection/Minimum Mechanism Scale", minimumMechanismScale, diff --git a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java index 66736aff..d78c20ff 100644 --- a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java +++ b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java @@ -239,7 +239,8 @@ public void periodic() { // brownout-scaled constraints if the hood is using Motion Magic position control. if (hoodMotor.getAppliedControl() instanceof DynamicMotionMagicVoltage dynamicControlRequest) { setPositionControl(Rotations.of(dynamicControlRequest.Position)); - } else if (hoodMotor.getAppliedControl() instanceof MotionMagicVoltage motionMagicControlRequest) { + } else if (hoodMotor.getAppliedControl() + instanceof MotionMagicVoltage motionMagicControlRequest) { setPositionControl(motionMagicControlRequest.getPositionMeasure()); } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 3c641561..1d1d4582 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -596,8 +596,8 @@ public Command moveTo(Supplier targetAngleSupplier) { } /** - * Applies a Dynamic Motion Magic position control request to move the turret to the target - * angle. If the turret is not yet zeroed, the motor is set to neutral mode instead. + * Applies a Dynamic Motion Magic position control request to move the turret to the target angle. + * If the turret is not yet zeroed, the motor is set to neutral mode instead. * * @param targetAngle The target angle to move the turret to */ From b0d55ccff636a6311420b5ad5eeb3c19ffa61278 Mon Sep 17 00:00:00 2001 From: Mond Date: Wed, 25 Mar 2026 18:39:10 -0700 Subject: [PATCH 05/10] adjusted brownoutprotection.java --- .vscode/settings.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 1 + .../robot/subsystems/BrownoutProtection.java | 216 ++++++++++++++---- 3 files changed, 176 insertions(+), 43 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index cdcc56da..42bffd52 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,6 +58,6 @@ "edu.wpi.first.math.**.struct.*", ], "java.dependency.enableDependencyCheckup": false, - "java.jdt.ls.vmargs": "-Xmx8G", + "java.jdt.ls.vmargs": "-Xmx16G", "java.compile.nullAnalysis.mode": "disabled" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3b00595e..518397e1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,6 +79,7 @@ public RobotContainer() { brownoutProtection = new BrownoutProtection( swerveDrive, + shooterRollers, turret, shooterHood, intakeExtension, diff --git a/src/main/java/frc/robot/subsystems/BrownoutProtection.java b/src/main/java/frc/robot/subsystems/BrownoutProtection.java index 86ca46ca..66501596 100644 --- a/src/main/java/frc/robot/subsystems/BrownoutProtection.java +++ b/src/main/java/frc/robot/subsystems/BrownoutProtection.java @@ -1,12 +1,13 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.swerve.CommandSwerveDrive; import dev.doglog.DogLog; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -14,38 +15,56 @@ import frc.robot.subsystems.hopper.beltfloor.BeltFloor; import frc.robot.subsystems.intakeextension.IntakeExtension; import frc.robot.subsystems.intakerollers.IntakeRollers; +import frc.robot.subsystems.shooterrollers.ShooterRollers; import frc.robot.subsystems.turret.Turret; /** - * Dynamically slows drivetrain and lower-priority mechanism motion when battery voltage drops or - * robot is drawing too high of a current. + * Dynamically slows subsystems when electrical headroom gets tight, prioritizing shooter/feed while + * shooting, intake while intaking, and drivetrain otherwise. */ public class BrownoutProtection extends SubsystemBase { + private static final int ELECTRICAL_SAMPLE_WINDOW = 8; + private static final double ACTIVE_MECHANISM_VOLTAGE_THRESHOLD_VOLTS = 1.0; + private static final double ACTIVE_SHOOTER_SPEED_THRESHOLD_RPS = 5.0; + private final CommandSwerveDrive swerveDrive; + private final ShooterRollers shooterRollers; private final Turret turret; private final ShooterHood shooterHood; private final IntakeExtension intakeExtension; private final BeltFloor beltFloor; private final IntakeRollers intakeRollers; - private final LinearFilter batteryVoltageFilter = LinearFilter.movingAverage(10); - private final LinearFilter totalCurrentFilter = LinearFilter.movingAverage(10); + private final double[] recentBatteryVoltageSamplesVolts = new double[ELECTRICAL_SAMPLE_WINDOW]; + private final double[] recentTotalCurrentSamplesAmps = new double[ELECTRICAL_SAMPLE_WINDOW]; + private int recentElectricalSampleCount = 0; + private int nextElectricalSampleIndex = 0; private double startLimitingVoltage = 10.2; private double minimumVoltage = 8.8; private double startLimitingCurrentAmps = 180.0; private double maximumCurrentAmps = 280.0; - private double minimumDriveScale = 0.45; - private double minimumMechanismScale = 0.2; + private double minimumDrivePriorityVelocityScalingFactor = 0.45; + private double minimumReducedDriveVelocityScalingFactor = 0.3; + private double minimumPriorityMechanismScalingFactor = 0.65; + private double minimumReducedMechanismScalingFactor = 0.2; + + private enum PriorityMode { + DRIVING, + INTAKING, + SHOOTING + } public BrownoutProtection( CommandSwerveDrive swerveDrive, + ShooterRollers shooterRollers, Turret turret, ShooterHood shooterHood, IntakeExtension intakeExtension, BeltFloor beltFloor, IntakeRollers intakeRollers) { this.swerveDrive = swerveDrive; + this.shooterRollers = shooterRollers; this.turret = turret; this.shooterHood = shooterHood; this.intakeExtension = intakeExtension; @@ -67,50 +86,163 @@ public BrownoutProtection( maximumCurrentAmps, value -> maximumCurrentAmps = value); DogLog.tunable( - "BrownoutProtection/Minimum Drive Scale", - minimumDriveScale, - value -> minimumDriveScale = value); + "BrownoutProtection/Minimum Drive Priority Velocity Scaling Factor", + minimumDrivePriorityVelocityScalingFactor, + value -> minimumDrivePriorityVelocityScalingFactor = value); + DogLog.tunable( + "BrownoutProtection/Minimum Reduced Drive Velocity Scaling Factor", + minimumReducedDriveVelocityScalingFactor, + value -> minimumReducedDriveVelocityScalingFactor = value); DogLog.tunable( - "BrownoutProtection/Minimum Mechanism Scale", - minimumMechanismScale, - value -> minimumMechanismScale = value); + "BrownoutProtection/Minimum Priority Mechanism Scaling Factor", + minimumPriorityMechanismScalingFactor, + value -> minimumPriorityMechanismScalingFactor = value); + DogLog.tunable( + "BrownoutProtection/Minimum Reduced Mechanism Scaling Factor", + minimumReducedMechanismScalingFactor, + value -> minimumReducedMechanismScalingFactor = value); } @Override public void periodic() { - double batteryVoltage = RobotController.getBatteryVoltage(); - double filteredBatteryVoltage = batteryVoltageFilter.calculate(batteryVoltage); + double batteryVoltageVolts = RobotController.getBatteryVoltage(); double totalCurrentAmps = CurrentDrawLogger.getTotalCurrent().in(Amps); - double filteredTotalCurrentAmps = totalCurrentFilter.calculate(totalCurrentAmps); - - double voltageBudget = unitRange(filteredBatteryVoltage, minimumVoltage, startLimitingVoltage); - double currentBudget = - 1.0 - unitRange(filteredTotalCurrentAmps, startLimitingCurrentAmps, maximumCurrentAmps); - double protectionBudget = - RobotState.isDisabled() ? 1.0 : Math.min(voltageBudget, currentBudget); - - double driveScale = MathUtil.interpolate(minimumDriveScale, 1.0, protectionBudget); - double mechanismScale = MathUtil.interpolate(minimumMechanismScale, 1.0, protectionBudget); - - swerveDrive.setVelocityScale(driveScale); - turret.setMotionProfileConstraintScale(mechanismScale); - shooterHood.setMotionProfileConstraintScale(mechanismScale); - intakeExtension.setMotionProfileConstraintScale(mechanismScale); - beltFloor.setVoltageScale(mechanismScale); - intakeRollers.setVoltageScale(mechanismScale); - - DogLog.log("BrownoutProtection/BatteryVoltage", batteryVoltage); - DogLog.log("BrownoutProtection/FilteredBatteryVoltage", filteredBatteryVoltage); + + recordElectricalSample(batteryVoltageVolts, totalCurrentAmps); + + double recentMinimumBatteryVoltageVolts = getRecentMinimumBatteryVoltageVolts(); + double recentMaximumTotalCurrentAmps = getRecentMaximumTotalCurrentAmps(); + + double voltageHeadroomFraction = + mapToUnitRange(recentMinimumBatteryVoltageVolts, minimumVoltage, startLimitingVoltage); + double currentHeadroomFraction = + 1.0 - mapToUnitRange(recentMaximumTotalCurrentAmps, startLimitingCurrentAmps, maximumCurrentAmps); + // 1.0 means there is enough electrical headroom for full performance. 0.0 means outputs + // should be clamped to their configured minimum scaling factors. + double allowedPerformanceFraction = + RobotState.isDisabled() ? 1.0 : Math.min(voltageHeadroomFraction, currentHeadroomFraction); + + PriorityMode priorityMode = determinePriorityMode(); + + double prioritizedDriveVelocityScalingFactor = + MathUtil.interpolate( + minimumDrivePriorityVelocityScalingFactor, 1.0, allowedPerformanceFraction); + double reducedDriveVelocityScalingFactor = + MathUtil.interpolate( + minimumReducedDriveVelocityScalingFactor, 1.0, allowedPerformanceFraction); + double prioritizedMechanismScalingFactor = + MathUtil.interpolate( + minimumPriorityMechanismScalingFactor, 1.0, allowedPerformanceFraction); + double reducedMechanismScalingFactor = + MathUtil.interpolate( + minimumReducedMechanismScalingFactor, 1.0, allowedPerformanceFraction); + + double driveVelocityScalingFactor = + priorityMode == PriorityMode.DRIVING + ? prioritizedDriveVelocityScalingFactor + : reducedDriveVelocityScalingFactor; + double shooterAimingScalingFactor = + priorityMode == PriorityMode.SHOOTING + ? prioritizedMechanismScalingFactor + : reducedMechanismScalingFactor; + double intakeMotionScalingFactor = + priorityMode == PriorityMode.INTAKING + ? prioritizedMechanismScalingFactor + : reducedMechanismScalingFactor; + double beltFloorVoltageScalingFactor = + priorityMode == PriorityMode.DRIVING + ? reducedMechanismScalingFactor + : prioritizedMechanismScalingFactor; + double intakeRollerVoltageScalingFactor = + priorityMode == PriorityMode.INTAKING + ? prioritizedMechanismScalingFactor + : reducedMechanismScalingFactor; + + swerveDrive.setVelocityScale(driveVelocityScalingFactor); + turret.setMotionProfileConstraintScale(shooterAimingScalingFactor); + shooterHood.setMotionProfileConstraintScale(shooterAimingScalingFactor); + intakeExtension.setMotionProfileConstraintScale(intakeMotionScalingFactor); + beltFloor.setVoltageScale(beltFloorVoltageScalingFactor); + intakeRollers.setVoltageScale(intakeRollerVoltageScalingFactor); + + DogLog.log("BrownoutProtection/PriorityMode", priorityMode.name()); + DogLog.log("BrownoutProtection/BatteryVoltageVolts", batteryVoltageVolts); + DogLog.log( + "BrownoutProtection/RecentMinimumBatteryVoltageVolts", recentMinimumBatteryVoltageVolts); DogLog.log("BrownoutProtection/TotalCurrentAmps", totalCurrentAmps); - DogLog.log("BrownoutProtection/FilteredCurrentAmps", filteredTotalCurrentAmps); - DogLog.log("BrownoutProtection/VoltageBudget", voltageBudget); - DogLog.log("BrownoutProtection/CurrentBudget", currentBudget); - DogLog.log("BrownoutProtection/ProtectionBudget", protectionBudget); - DogLog.log("BrownoutProtection/DriveScale", driveScale); - DogLog.log("BrownoutProtection/MechanismScale", mechanismScale); + DogLog.log("BrownoutProtection/RecentMaximumTotalCurrentAmps", recentMaximumTotalCurrentAmps); + DogLog.log("BrownoutProtection/VoltageHeadroomFraction", voltageHeadroomFraction); + DogLog.log("BrownoutProtection/CurrentHeadroomFraction", currentHeadroomFraction); + DogLog.log("BrownoutProtection/AllowedPerformanceFraction", allowedPerformanceFraction); + DogLog.log( + "BrownoutProtection/DriveVelocityScalingFactor", driveVelocityScalingFactor); + DogLog.log( + "BrownoutProtection/ShooterAimingScalingFactor", shooterAimingScalingFactor); + DogLog.log( + "BrownoutProtection/IntakeMotionScalingFactor", intakeMotionScalingFactor); + DogLog.log( + "BrownoutProtection/BeltFloorVoltageScalingFactor", beltFloorVoltageScalingFactor); + DogLog.log( + "BrownoutProtection/IntakeRollerVoltageScalingFactor", intakeRollerVoltageScalingFactor); + } + + private PriorityMode determinePriorityMode() { + boolean shooterActive = + shooterRollers.getCurrentCommand() != null + || Math.abs(shooterRollers.getMotorVoltage().in(Volts)) + > ACTIVE_MECHANISM_VOLTAGE_THRESHOLD_VOLTS + || Math.abs(shooterRollers.getAngularVelocity().in(RotationsPerSecond)) + > ACTIVE_SHOOTER_SPEED_THRESHOLD_RPS; + + if (shooterActive) { + return PriorityMode.SHOOTING; + } + + boolean intakeActive = + intakeExtension.getCurrentCommand() != null + || intakeRollers.getAppliedVoltage().in(Volts) > ACTIVE_MECHANISM_VOLTAGE_THRESHOLD_VOLTS + || beltFloor.getMotorVoltage().in(Volts) > ACTIVE_MECHANISM_VOLTAGE_THRESHOLD_VOLTS; + + return intakeActive ? PriorityMode.INTAKING : PriorityMode.DRIVING; + } + + private void recordElectricalSample(double batteryVoltageVolts, double totalCurrentAmps) { + recentBatteryVoltageSamplesVolts[nextElectricalSampleIndex] = batteryVoltageVolts; + recentTotalCurrentSamplesAmps[nextElectricalSampleIndex] = totalCurrentAmps; + nextElectricalSampleIndex = (nextElectricalSampleIndex + 1) % ELECTRICAL_SAMPLE_WINDOW; + recentElectricalSampleCount = + Math.min(recentElectricalSampleCount + 1, ELECTRICAL_SAMPLE_WINDOW); + } + + private double getRecentMinimumBatteryVoltageVolts() { + double recentMinimumBatteryVoltageVolts = Double.POSITIVE_INFINITY; + + for (int i = 0; i < recentElectricalSampleCount; i++) { + recentMinimumBatteryVoltageVolts = + Math.min(recentMinimumBatteryVoltageVolts, recentBatteryVoltageSamplesVolts[i]); + } + + return recentElectricalSampleCount == 0 + ? startLimitingVoltage + : recentMinimumBatteryVoltageVolts; + } + + private double getRecentMaximumTotalCurrentAmps() { + double recentMaximumTotalCurrentAmps = Double.NEGATIVE_INFINITY; + + for (int i = 0; i < recentElectricalSampleCount; i++) { + recentMaximumTotalCurrentAmps = + Math.max(recentMaximumTotalCurrentAmps, recentTotalCurrentSamplesAmps[i]); + } + + return recentElectricalSampleCount == 0 ? 0.0 : recentMaximumTotalCurrentAmps; } - private static double unitRange(double value, double minimumValue, double maximumValue) { + /** + * Maps a value into the inclusive unit interval, where {@code minimumValue} becomes {@code 0.0} + * and {@code maximumValue} becomes {@code 1.0}. + */ + private static double mapToUnitRange(double value, double minimumValue, double maximumValue) { if (maximumValue <= minimumValue) { return value >= maximumValue ? 1.0 : 0.0; } From a7d452e4c2f7736b7978bd66339b7eaa36e03f74 Mon Sep 17 00:00:00 2001 From: Mond-Mann <63756512+Mond-Mann@users.noreply.github.com> Date: Thu, 26 Mar 2026 01:44:02 +0000 Subject: [PATCH 06/10] style: Apply Spotless fixes --- .../robot/subsystems/BrownoutProtection.java | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/BrownoutProtection.java b/src/main/java/frc/robot/subsystems/BrownoutProtection.java index 66501596..99f872b1 100644 --- a/src/main/java/frc/robot/subsystems/BrownoutProtection.java +++ b/src/main/java/frc/robot/subsystems/BrownoutProtection.java @@ -116,7 +116,9 @@ public void periodic() { double voltageHeadroomFraction = mapToUnitRange(recentMinimumBatteryVoltageVolts, minimumVoltage, startLimitingVoltage); double currentHeadroomFraction = - 1.0 - mapToUnitRange(recentMaximumTotalCurrentAmps, startLimitingCurrentAmps, maximumCurrentAmps); + 1.0 + - mapToUnitRange( + recentMaximumTotalCurrentAmps, startLimitingCurrentAmps, maximumCurrentAmps); // 1.0 means there is enough electrical headroom for full performance. 0.0 means outputs // should be clamped to their configured minimum scaling factors. double allowedPerformanceFraction = @@ -134,8 +136,7 @@ public void periodic() { MathUtil.interpolate( minimumPriorityMechanismScalingFactor, 1.0, allowedPerformanceFraction); double reducedMechanismScalingFactor = - MathUtil.interpolate( - minimumReducedMechanismScalingFactor, 1.0, allowedPerformanceFraction); + MathUtil.interpolate(minimumReducedMechanismScalingFactor, 1.0, allowedPerformanceFraction); double driveVelocityScalingFactor = priorityMode == PriorityMode.DRIVING @@ -174,14 +175,10 @@ public void periodic() { DogLog.log("BrownoutProtection/VoltageHeadroomFraction", voltageHeadroomFraction); DogLog.log("BrownoutProtection/CurrentHeadroomFraction", currentHeadroomFraction); DogLog.log("BrownoutProtection/AllowedPerformanceFraction", allowedPerformanceFraction); - DogLog.log( - "BrownoutProtection/DriveVelocityScalingFactor", driveVelocityScalingFactor); - DogLog.log( - "BrownoutProtection/ShooterAimingScalingFactor", shooterAimingScalingFactor); - DogLog.log( - "BrownoutProtection/IntakeMotionScalingFactor", intakeMotionScalingFactor); - DogLog.log( - "BrownoutProtection/BeltFloorVoltageScalingFactor", beltFloorVoltageScalingFactor); + DogLog.log("BrownoutProtection/DriveVelocityScalingFactor", driveVelocityScalingFactor); + DogLog.log("BrownoutProtection/ShooterAimingScalingFactor", shooterAimingScalingFactor); + DogLog.log("BrownoutProtection/IntakeMotionScalingFactor", intakeMotionScalingFactor); + DogLog.log("BrownoutProtection/BeltFloorVoltageScalingFactor", beltFloorVoltageScalingFactor); DogLog.log( "BrownoutProtection/IntakeRollerVoltageScalingFactor", intakeRollerVoltageScalingFactor); } @@ -200,7 +197,8 @@ private PriorityMode determinePriorityMode() { boolean intakeActive = intakeExtension.getCurrentCommand() != null - || intakeRollers.getAppliedVoltage().in(Volts) > ACTIVE_MECHANISM_VOLTAGE_THRESHOLD_VOLTS + || intakeRollers.getAppliedVoltage().in(Volts) + > ACTIVE_MECHANISM_VOLTAGE_THRESHOLD_VOLTS || beltFloor.getMotorVoltage().in(Volts) > ACTIVE_MECHANISM_VOLTAGE_THRESHOLD_VOLTS; return intakeActive ? PriorityMode.INTAKING : PriorityMode.DRIVING; From 008d7affbdbfc06f6484a85ba4d4a6cfe4c27867 Mon Sep 17 00:00:00 2001 From: Mond Date: Wed, 25 Mar 2026 19:26:45 -0700 Subject: [PATCH 07/10] removed motionmagicjerk in postion control setters --- src/main/java/frc/robot/subsystems/hood/ShooterHood.java | 1 - .../frc/robot/subsystems/intakeextension/IntakeExtension.java | 3 +-- src/main/java/frc/robot/subsystems/turret/Turret.java | 1 - 3 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java index d78c20ff..af87aea9 100644 --- a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java +++ b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java @@ -416,7 +416,6 @@ private void setPositionControl(Angle position) { targetPosition.in(Rotations), getScaledMotionMagicCruiseVelocity(), getScaledMotionMagicAcceleration()) - .withJerk(ShooterHoodConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicJerk) .withFeedForward(Math.cos(getPosition().in(Radians)) * kG)); } } diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java index 7165103a..9fd0b07e 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java @@ -209,8 +209,7 @@ private void setPositionControl(Distance position) { new DynamicMotionMagicVoltage( position.in(Meters), getScaledMotionMagicCruiseVelocity(), - getScaledMotionMagicAcceleration()) - .withJerk(IntakeExtensionConstants.MOTOR_CONFIGURATION.MotionMagic.MotionMagicJerk)); + getScaledMotionMagicAcceleration())); } /** diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 1d1d4582..aff93436 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -608,7 +608,6 @@ private void setPositionControl(Angle targetAngle) { targetAngle.in(Rotations), getScaledMotionMagicCruiseVelocity(), getScaledMotionMagicAcceleration()) - .withJerk(config.MotionMagic.MotionMagicJerk) .withFeedForward( targetAngle.lt(TurretConstants.MIN_KW_ANGLE) ? -TurretConstants.kW From 33d00b0c57c15f7de35bd9b4aa21b006495cb57d Mon Sep 17 00:00:00 2001 From: Mond Date: Wed, 25 Mar 2026 19:34:48 -0700 Subject: [PATCH 08/10] reverted changes in individualized files --- .vscode/settings.json | 2 +- simgui-ds.json | 18 ++++++++---------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 42bffd52..cdcc56da 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,6 +58,6 @@ "edu.wpi.first.math.**.struct.*", ], "java.dependency.enableDependencyCheckup": false, - "java.jdt.ls.vmargs": "-Xmx16G", + "java.jdt.ls.vmargs": "-Xmx8G", "java.compile.nullAnalysis.mode": "disabled" } diff --git a/simgui-ds.json b/simgui-ds.json index d58b23a0..c3c4b0c1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,11 +4,6 @@ "visible": false } }, - "Keyboard 1 Settings": { - "window": { - "visible": true - } - }, "System Joysticks": { "window": { "visible": false @@ -34,8 +29,8 @@ "incKey": 44 }, { - "decKey": 74, - "incKey": 76 + "decKey": 81, + "incKey": 69 } ], "axisCount": 5, @@ -50,7 +45,7 @@ 77, -1, -1, - 340 + 47 ], "povConfig": [ { @@ -68,7 +63,10 @@ }, { "axisConfig": [ - {}, + { + "decKey": 74, + "incKey": 76 + }, { "decKey": 73, "incKey": 75 @@ -127,4 +125,4 @@ "guid": "Keyboard1" } ] -} +} \ No newline at end of file From 9fb1f10ec92192b0b4837252f61965f2e529f0ea Mon Sep 17 00:00:00 2001 From: Mond Date: Sun, 29 Mar 2026 13:25:05 -0700 Subject: [PATCH 09/10] patches --- .../lib/swerve/MotionSwerveDrive.java | 35 ----- .../swerve/commands/DriveToStateCommand.java | 124 ++++-------------- .../lib/swerve/motion/VelocityMotion.java | 65 +-------- .../lib/swerve/pathplanner/PathPlanner.java | 2 +- .../team6962/lib/swerve/util/FieldLogger.java | 13 +- src/main/java/frc/robot/Preferences.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 20 +-- .../frc/robot/controls/TeleopControls.java | 7 +- .../robot/subsystems/BrownoutProtection.java | 22 +++- 9 files changed, 75 insertions(+), 217 deletions(-) diff --git a/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java b/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java index 1cadfed4..b09b358b 100644 --- a/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java +++ b/src/main/java/com/team6962/lib/swerve/MotionSwerveDrive.java @@ -25,7 +25,6 @@ import com.team6962.lib.swerve.util.FieldLogger; import com.team6962.lib.swerve.util.SwerveComponent; import dev.doglog.DogLog; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Twist2d; @@ -83,7 +82,6 @@ public class MotionSwerveDrive implements AutoCloseable { private BaseStatusSignal[] statusSignals; private SwerveDriveKinematics kinematics; private SwerveMotionManager motionManager; - private volatile double velocityScale = 1.0; /** * Creates a new MotionSwerveDrive with the specified drivetrain configuration. @@ -380,21 +378,6 @@ public boolean isNear(Pose2d target, Distance translationTolerance, Angle angula && angularError <= angularTolerance.in(Radians); } - /** - * Checks if the robot is within a translational tolerance of a target pose. - * - * @param target The target pose to check against - * @param translationTolerance Maximum allowed translation error from target position - * @return {@code true} if within the translational tolerance, {@code false} otherwise - */ - public boolean isNear(Pose2d target, Distance translationTolerance) { - Pose2d current = getPosition2d(); - - double linearError = current.getTranslation().getDistance(target.getTranslation()); - - return linearError <= translationTolerance.in(Meters); - } - /** * Gets the current field-relative velocity of the robot as a {@link ChassisSpeeds} object. * @@ -503,24 +486,6 @@ public void updateMotion() { motionManager.update(); } - /** - * Sets runtime scale that is applied to requested drivetrain velocities. - * - * @param scale the velocity scale to apply - */ - public void setVelocityScale(double scale) { - velocityScale = MathUtil.clamp(scale, 0.0, 1.0); - } - - /** - * Gets runtime scale applied to requested drivetrain velocities. - * - * @return the velocity scale - */ - public double getVelocityScale() { - return velocityScale; - } - /** * Applies a motion to be executed during the next control loop cycle. * diff --git a/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java b/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java index d32147cb..7062ad8f 100644 --- a/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java +++ b/src/main/java/com/team6962/lib/swerve/commands/DriveToStateCommand.java @@ -1,10 +1,8 @@ package com.team6962.lib.swerve.commands; import static edu.wpi.first.units.Units.Hertz; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Seconds; import com.team6962.lib.control.MotionProfile; import com.team6962.lib.control.ProfiledController; @@ -13,15 +11,11 @@ import com.team6962.lib.math.AngleMath; import com.team6962.lib.math.TranslationalVelocity; import com.team6962.lib.swerve.CommandSwerveDrive; -import dev.doglog.DogLog; -import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.numbers.N2; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; @@ -153,35 +147,8 @@ public DriveToStateCommand(CommandSwerveDrive swerveDrive, State target) { this.swerveDrive = swerveDrive; this.target = target; - if (target.translation != null) { - addRequirements(swerveDrive.useTranslation()); - } - - if (target.angle != null) { - addRequirements(swerveDrive.useRotation()); - } - } - - @Override - public void initialize() { - // Reset state - motionProfilesFinished = false; - - createControllers(); - - // Generate initial motion profiles - createMotionProfiles(); - - DogLog.log( - "Drivetrain/DriveToState/FinalTarget", - new Pose2d(target.translation, new Rotation2d(target.angle))); - DogLog.log("Drivetrain/DriveToState/FinalVelocityX", target.translationalVelocity.x); - DogLog.log("Drivetrain/DriveToState/FinalVelocityY", target.translationalVelocity.y); - DogLog.log("Drivetrain/DriveToState/FinalAngularVelocity", target.angularVelocity); - } - - /** Creates controllers for translation and rotation using the configured constraints. */ - private void createControllers() { + // Initialize translation and rotation controllers with PID constants + // and motion profile constraints if (target.translation != null) { translationController = new TranslationController( @@ -190,6 +157,8 @@ private void createControllers() { swerveDrive.getConstants().Driving.TranslationFeedbackKD, swerveDrive.getConstants().Driving.getTranslationConstraints(), Hertz.of(50)); + + addRequirements(swerveDrive.useTranslation()); } if (target.angle != null) { @@ -200,9 +169,20 @@ private void createControllers() { swerveDrive.getConstants().Driving.AngleFeedbackKD, new TrapezoidalProfile(swerveDrive.getConstants().Driving.getRotationConstraints()), Hertz.of(50)); + + addRequirements(swerveDrive.useRotation()); } } + @Override + public void initialize() { + // Reset state + motionProfilesFinished = false; + + // Generate initial motion profiles + createMotionProfiles(); + } + /** Creates motion profiles for translation and rotation controllers. */ private void createMotionProfiles() { if (translationController != null) { @@ -247,38 +227,16 @@ public void execute() { createMotionProfiles(); } - // Current target pose and speeds for logging + // Current target pose for logging Pose2d currentTarget = new Pose2d(); - ChassisSpeeds currentSpeeds = new ChassisSpeeds(); // Calculate and apply velocity commands for translation and rotation if (translationController != null) { - TranslationalVelocity currentTranslationalVelocity = + TranslationalVelocity outputTranslationalVelocity = translationController.calculate( swerveDrive.getPosition2d().getTranslation(), swerveDrive.getTranslationalVelocity()); - currentSpeeds = - new ChassisSpeeds( - currentTranslationalVelocity.x.in(MetersPerSecond), - currentTranslationalVelocity.y.in(MetersPerSecond), - currentSpeeds.omegaRadiansPerSecond); - - TranslationalVelocity profileTranslationalVelocity = translationController.sample(0); - - TranslationalVelocity nextTranslationalVelocity = translationController.sample(0.02); - - Vector acceleration = - nextTranslationalVelocity - .toVector() - .minus(profileTranslationalVelocity.toVector()) - .div(0.02); - - currentTranslationalVelocity = - currentTranslationalVelocity.plus( - new TranslationalVelocity(acceleration) - .times(swerveDrive.getConstants().Driving.AutoLinearAccelerationScalar)); - - swerveDrive.applyVelocityMotion(currentTranslationalVelocity); + swerveDrive.applyVelocityMotion(outputTranslationalVelocity); // Add translation to current target pose currentTarget = @@ -287,32 +245,14 @@ public void execute() { } if (headingController != null) { - MotionProfile.State angularState = - new MotionProfile.State( - swerveDrive.getYaw().in(Radians), swerveDrive.getYawVelocity().in(RadiansPerSecond)); - - AngularVelocity currentAngularVelocity = - RadiansPerSecond.of(headingController.calculate(angularState)); - - currentSpeeds = - new ChassisSpeeds( - currentSpeeds.vxMetersPerSecond, - currentSpeeds.vyMetersPerSecond, - currentAngularVelocity.in(RadiansPerSecond)); + AngularVelocity outputAngularVelocity = + RadiansPerSecond.of( + headingController.calculate( + new MotionProfile.State( + swerveDrive.getYaw().in(Radians), + swerveDrive.getYawVelocity().in(RadiansPerSecond)))); - AngularVelocity profileAngularVelocity = RadiansPerSecond.of(headingController.sample(0)); - - AngularVelocity nextAngularVelocity = RadiansPerSecond.of(headingController.sample(0.02)); - - AngularAcceleration angularAcceleration = - nextAngularVelocity.minus(profileAngularVelocity).div(Seconds.of(0.02)); - - currentAngularVelocity = - currentAngularVelocity.plus( - angularAcceleration.times( - Seconds.of(swerveDrive.getConstants().Driving.AutoAngularAccelerationScalar))); - - swerveDrive.applyVelocityMotion(currentAngularVelocity); + swerveDrive.applyVelocityMotion(outputAngularVelocity); // Add rotation to current target pose currentTarget = @@ -323,20 +263,6 @@ public void execute() { // Log current target pose swerveDrive.getFieldLogger().getField().getObject("Current Target").setPose(currentTarget); - - DogLog.log("Drivetrain/DriveToState/ProfilePose", currentTarget); - DogLog.log( - "Drivetrain/DriveToState/ProfileVelocityX", - currentSpeeds.vxMetersPerSecond, - MetersPerSecond); - DogLog.log( - "Drivetrain/DriveToState/ProfileVelocityY", - currentSpeeds.vyMetersPerSecond, - MetersPerSecond); - DogLog.log( - "Drivetrain/DriveToState/ProfileAngularVelocity", - currentSpeeds.omegaRadiansPerSecond, - RadiansPerSecond); } @Override diff --git a/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java b/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java index a1195dd5..917c4529 100644 --- a/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java +++ b/src/main/java/com/team6962/lib/swerve/motion/VelocityMotion.java @@ -1,11 +1,7 @@ package com.team6962.lib.swerve.motion; -import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Hertz; import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.NewtonMeters; -import static edu.wpi.first.units.Units.Newtons; -import static edu.wpi.first.units.Units.Ohms; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -25,12 +21,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Force; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Resistance; -import edu.wpi.first.units.measure.Torque; -import edu.wpi.first.units.measure.Voltage; /** * A swerve motion that drives the robot at a specified field-relative velocity. @@ -64,35 +55,18 @@ public class VelocityMotion implements SwerveMotion { /** Whether this motion has a non-zero rotation component. */ private final boolean hasRotation; - /** The feedforward forces for each swerve module, which can be null if not used. */ - private final Force[] forceFeedforwards; - /** * Creates a new VelocityMotion with the specified field-relative velocity. * * @param velocity The target field-relative chassis speeds - * @param forceFeedforwards The feedforward forces for each swerve module, which can be null if - * not used * @param swerveDrive The swerve drive to control */ - public VelocityMotion( - ChassisSpeeds velocity, Force[] forceFeedforwards, MotionSwerveDrive swerveDrive) { + public VelocityMotion(ChassisSpeeds velocity, MotionSwerveDrive swerveDrive) { this.velocity = velocity; this.hasTranslation = Math.abs(velocity.vxMetersPerSecond) > 0.01 || Math.abs(velocity.vyMetersPerSecond) > 0.01; this.hasRotation = Math.abs(velocity.omegaRadiansPerSecond) > 0.01; this.swerveDrive = swerveDrive; - this.forceFeedforwards = forceFeedforwards; - } - - /** - * Creates a new VelocityMotion with the specified field-relative velocity. - * - * @param velocity The target field-relative chassis speeds - * @param swerveDrive The swerve drive to control - */ - public VelocityMotion(ChassisSpeeds velocity, MotionSwerveDrive swerveDrive) { - this(velocity, null, swerveDrive); } /** @@ -114,14 +88,6 @@ public SwerveMotion fuseWith(SwerveMotion other) { "Cannot fuse two VelocityMotions with overlapping translation or rotation components."); } - if (!otherVelocityMotion.hasRotation && !otherVelocityMotion.hasTranslation) { - return this; - } - - if (!hasRotation && !hasTranslation) { - return otherVelocityMotion; - } - return new VelocityMotion( SwerveKinematicsUtil.addChassisSpeeds(velocity, otherVelocityMotion.velocity), swerveDrive); @@ -141,23 +107,15 @@ public SwerveMotion fuseWith(SwerveMotion other) { */ @Override public void update(double deltaTimeSeconds) { - double velocityScale = swerveDrive.getVelocityScale(); - ChassisSpeeds scaledVelocity = - new ChassisSpeeds( - velocity.vxMetersPerSecond * velocityScale, - velocity.vyMetersPerSecond * velocityScale, - velocity.omegaRadiansPerSecond * velocityScale); - - if (Math.abs(scaledVelocity.omegaRadiansPerSecond) < 0.01 - && Math.abs(scaledVelocity.vxMetersPerSecond) < 0.01 - && Math.abs(scaledVelocity.vyMetersPerSecond) < 0.01) { + if (Math.abs(velocity.omegaRadiansPerSecond) < 0.01 + && Math.abs(velocity.vxMetersPerSecond) < 0.01 + && Math.abs(velocity.vyMetersPerSecond) < 0.01) { brake(); return; } ChassisSpeeds robotRelativeVelocity = - ChassisSpeeds.fromFieldRelativeSpeeds( - scaledVelocity, new Rotation2d(swerveDrive.getHeading())); + ChassisSpeeds.fromFieldRelativeSpeeds(velocity, new Rotation2d(swerveDrive.getHeading())); SwerveModuleState[] states = swerveDrive.getKinematics().toSwerveModuleStates(robotRelativeVelocity); @@ -195,17 +153,6 @@ public void update(double deltaTimeSeconds) { LinearVelocity driveVelocity = MetersPerSecond.of(state.speedMetersPerSecond); Angle steerAngle = state.angle.getMeasure(); - Force ffForce = forceFeedforwards != null ? forceFeedforwards[i] : Newtons.of(0); - Torque ffWheelTorque = swerveDrive.getConstants().getWheelRadius(i).times(ffForce); - Torque ffMotorTorque = ffWheelTorque.div(swerveDrive.getConstants().DriveMotor.GearReduction); - double ffMotorTorqueNm = ffMotorTorque.in(NewtonMeters); - double motorKT = swerveDrive.getConstants().DriveMotor.SimulatedMotor.KtNMPerAmp; - double ffCurrentAmps = ffMotorTorqueNm / motorKT; - Current ffCurrent = Amps.of(ffCurrentAmps); - Resistance windingResistance = - Ohms.of(swerveDrive.getConstants().DriveMotor.SimulatedMotor.rOhms); - Voltage ffVoltage = ffCurrent.times(windingResistance); - module.setControl( new VelocityControlRequest( WheelMath.toAngular(driveVelocity, swerveDrive.getConstants().getWheelRadius(i)) @@ -215,7 +162,6 @@ public void update(double deltaTimeSeconds) { .withSlot(driveConstants.VelocitySlot) .withUpdateFreqHz(updateFrequencyHz) .withUseTimesync(useTimesync) - .withAdditionalFeedforward(ffVoltage) .toControlRequest(), new PositionControlRequest(steerAngle.in(Rotations)) .withMotionProfileType(steerConstants.PositionControlMotionProfile) @@ -242,7 +188,6 @@ public void logTelemetry(String basePath) { DogLog.log(basePath + "LinearVelocityX", velocity.vxMetersPerSecond); DogLog.log(basePath + "LinearVelocityY", velocity.vyMetersPerSecond); DogLog.log(basePath + "AngularVelocity", velocity.omegaRadiansPerSecond); - DogLog.log(basePath + "VelocityScale", swerveDrive.getVelocityScale()); } /** Sets all motors to neutral/brake mode */ diff --git a/src/main/java/com/team6962/lib/swerve/pathplanner/PathPlanner.java b/src/main/java/com/team6962/lib/swerve/pathplanner/PathPlanner.java index e0c5b103..4eef2a8c 100644 --- a/src/main/java/com/team6962/lib/swerve/pathplanner/PathPlanner.java +++ b/src/main/java/com/team6962/lib/swerve/pathplanner/PathPlanner.java @@ -68,7 +68,7 @@ public PathPlanner(CommandSwerveDrive drivetrain) { } } - drivetrain.applyMotion(new VelocityMotion(appliedSpeeds, forces, drivetrain)); + drivetrain.applyMotion(new VelocityMotion(appliedSpeeds, drivetrain)); }, new PPHolonomicDriveController( new PIDConstants( diff --git a/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java b/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java index 31bb2a70..f14a101a 100644 --- a/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java +++ b/src/main/java/com/team6962/lib/swerve/util/FieldLogger.java @@ -69,10 +69,13 @@ public void logTelemetry(String basePath) { } Pose2d robotPose = localization.getPosition2d(); - Pose2d[] modulePoses = null; + + previousRobotPose = robotPose; + + field.setRobotPose(robotPose); if (!constants.Timing.MinimizeLogging) { - modulePoses = new Pose2d[4]; + Pose2d[] modulePoses = new Pose2d[4]; SwerveModuleState[] moduleStates = odometry.getStates(); for (int i = 0; i < 4; i++) { @@ -84,13 +87,7 @@ public void logTelemetry(String basePath) { modulePoses[i] = robotPose.plus(relativePose.minus(new Pose2d())); } - } - - previousRobotPose = robotPose; - - field.setRobotPose(robotPose); - if (modulePoses != null) { field.getObject("Swerve Modules").setPoses(modulePoses); } } diff --git a/src/main/java/frc/robot/Preferences.java b/src/main/java/frc/robot/Preferences.java index 59daffe6..675472cb 100644 --- a/src/main/java/frc/robot/Preferences.java +++ b/src/main/java/frc/robot/Preferences.java @@ -1,6 +1,6 @@ package frc.robot; -import frc.robot.constants.CompetitionBotSimConstants; +import frc.robot.constants.CompetitionBotConstants; import frc.robot.constants.RobotConstants; public class Preferences { @@ -27,5 +27,5 @@ public class Preferences { * corresponds to the robot being simulated. This is used to determine which subsystems exist on * the simulated robot and to provide the correct configuration values for those subsystems. */ - public static final RobotConstants simulatedRobot = new CompetitionBotSimConstants(); + public static final RobotConstants simulatedRobot = new CompetitionBotConstants(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 518397e1..e9cd50d0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -76,15 +76,6 @@ public RobotContainer() { turret = new Turret(); intakeExtension = new IntakeExtension(); hopper = new Hopper(); - brownoutProtection = - new BrownoutProtection( - swerveDrive, - shooterRollers, - turret, - shooterHood, - intakeExtension, - hopper.getBeltFloor(), - intakeRollers); aprilTagVision = new AprilTagVision(swerveDrive, constants.getAprilTagVisionConstants()); fuelClumpLocalization = @@ -96,6 +87,17 @@ public RobotContainer() { teleopControls = new TeleopControls(this); teleopControls.configureBindings(); + brownoutProtection = + new BrownoutProtection( + swerveDrive, + teleopControls.getTeleopSwerveCommand(), + shooterRollers, + turret, + shooterHood, + intakeExtension, + hopper.getBeltFloor(), + intakeRollers); + driveStraightAuto = new DriveStraightAuto(this); autonomous = new Autonomous(this); diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index 299995bc..a474ca12 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -41,6 +41,7 @@ public class TeleopControls { private CommandXboxController operator = new CommandXboxController(1); private Distance shootingTestDistance = Inches.of(206); private AutoDepot autoDepot; + private XBoxTeleopSwerveCommand teleopSwerveCommand; private boolean fineControl = false; private AngularVelocity flywheelVelocity = ShooterRollersConstants.FIXED_FLYWHEEL_VELOCITY; @@ -97,7 +98,7 @@ public void configureBindings() { Trigger teleopEnabledTrigger = new Trigger(() -> RobotState.isTeleop() && RobotState.isEnabled()); - XBoxTeleopSwerveCommand teleopSwerveCommand = + teleopSwerveCommand = new XBoxTeleopSwerveCommand( robot.getSwerveDrive(), robot.getConstants().getTeleopSwerveConstants()); @@ -392,4 +393,8 @@ private Command fineControlEnableRumble() { private Command fineControlDisableRumble() { return Commands.sequence(rumble(operator, RumbleType.kBothRumble, 1).withTimeout(1.0)); } + + public XBoxTeleopSwerveCommand getTeleopSwerveCommand() { + return teleopSwerveCommand; + } } diff --git a/src/main/java/frc/robot/subsystems/BrownoutProtection.java b/src/main/java/frc/robot/subsystems/BrownoutProtection.java index 99f872b1..0cd119b9 100644 --- a/src/main/java/frc/robot/subsystems/BrownoutProtection.java +++ b/src/main/java/frc/robot/subsystems/BrownoutProtection.java @@ -1,11 +1,14 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.swerve.CommandSwerveDrive; +import com.team6962.lib.swerve.commands.XBoxTeleopSwerveCommand; import dev.doglog.DogLog; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.RobotController; @@ -21,6 +24,9 @@ /** * Dynamically slows subsystems when electrical headroom gets tight, prioritizing shooter/feed while * shooting, intake while intaking, and drivetrain otherwise. + * + * Drive limits are applied as hard velocity caps directly in {@link XBoxTeleopSwerveCommand} so + * autonomous paths are unaffected */ public class BrownoutProtection extends SubsystemBase { private static final int ELECTRICAL_SAMPLE_WINDOW = 8; @@ -28,6 +34,7 @@ public class BrownoutProtection extends SubsystemBase { private static final double ACTIVE_SHOOTER_SPEED_THRESHOLD_RPS = 5.0; private final CommandSwerveDrive swerveDrive; + private final XBoxTeleopSwerveCommand teleopSwerveCommand; private final ShooterRollers shooterRollers; private final Turret turret; private final ShooterHood shooterHood; @@ -57,6 +64,7 @@ private enum PriorityMode { public BrownoutProtection( CommandSwerveDrive swerveDrive, + XBoxTeleopSwerveCommand teleopSwerveCommand, ShooterRollers shooterRollers, Turret turret, ShooterHood shooterHood, @@ -64,6 +72,7 @@ public BrownoutProtection( BeltFloor beltFloor, IntakeRollers intakeRollers) { this.swerveDrive = swerveDrive; + this.teleopSwerveCommand = teleopSwerveCommand; this.shooterRollers = shooterRollers; this.turret = turret; this.shooterHood = shooterHood; @@ -159,7 +168,16 @@ public void periodic() { ? prioritizedMechanismScalingFactor : reducedMechanismScalingFactor; - swerveDrive.setVelocityScale(driveVelocityScalingFactor); + // Apply drive limits as hard velocity caps in teleop command only + // Auton paths go through PathPlanner/VelocityMotion, are never affected + double maxLinearVelocityMetersPerSecond = + swerveDrive.getConstants().Driving.MaxLinearVelocity.in(MetersPerSecond); + double maxAngularVelocityRadiansPerSecond = + swerveDrive.getConstants().Driving.MaxAngularVelocity.in(RadiansPerSecond); + teleopSwerveCommand.addDynamicVelocityLimits( + MetersPerSecond.of(driveVelocityScalingFactor * maxLinearVelocityMetersPerSecond), + RadiansPerSecond.of(driveVelocityScalingFactor * maxAngularVelocityRadiansPerSecond)); + turret.setMotionProfileConstraintScale(shooterAimingScalingFactor); shooterHood.setMotionProfileConstraintScale(shooterAimingScalingFactor); intakeExtension.setMotionProfileConstraintScale(intakeMotionScalingFactor); @@ -247,4 +265,4 @@ private static double mapToUnitRange(double value, double minimumValue, double m return MathUtil.clamp((value - minimumValue) / (maximumValue - minimumValue), 0.0, 1.0); } -} +} \ No newline at end of file From a85e7d176c498bc0e959d9c7add8bd1dcccdaff4 Mon Sep 17 00:00:00 2001 From: Mond Date: Sun, 29 Mar 2026 13:39:56 -0700 Subject: [PATCH 10/10] changes --- src/main/java/frc/robot/RobotContainer.java | 19 ++++--- .../frc/robot/controls/TeleopControls.java | 56 +++++++------------ .../robot/subsystems/BrownoutProtection.java | 37 ++++-------- 3 files changed, 41 insertions(+), 71 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e9cd50d0..9cbb0d33 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -88,15 +88,16 @@ public RobotContainer() { teleopControls.configureBindings(); brownoutProtection = - new BrownoutProtection( - swerveDrive, - teleopControls.getTeleopSwerveCommand(), - shooterRollers, - turret, - shooterHood, - intakeExtension, - hopper.getBeltFloor(), - intakeRollers); + new BrownoutProtection( + swerveDrive, + teleopControls.getTeleopSwerveCommand(), + teleopControls.getShootingActiveTrigger(), + teleopControls.getIntakingActiveTrigger(), + turret, + shooterHood, + intakeExtension, + hopper.getBeltFloor(), + intakeRollers); driveStraightAuto = new DriveStraightAuto(this); autonomous = new Autonomous(this); diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index a474ca12..6f9f3a43 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -48,6 +48,9 @@ public class TeleopControls { private double tunableHoodAngle = 0; private double tunableRollerVelocity = 0; + private Trigger shootingActiveTrigger; + private Trigger intakingActiveTrigger; + public TeleopControls(RobotContainer robot) { this.robot = robot; // this.autoClimb = new AutoClimb(robot); @@ -248,11 +251,11 @@ public void configureBindings() { // Intake extension and retraction - WORKS Trigger intakeRetract = operator.rightStick(); - Trigger intakeExtend = + intakingActiveTrigger = intakeRetract.negate().and(RobotState::isTeleop).and(RobotState::isEnabled); intakeRetract.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().retract()); - intakeExtend.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().extend()); + intakingActiveTrigger.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().extend()); // Automatically load fuel into the kicker when there is fuel in the hopper - WORKS, but not // fully tested @@ -295,39 +298,12 @@ public void configureBindings() { robot.getSwerveDrive().getPosition2d().getX() < TrenchDriving.OBSTACLES_CENTER_X.in(Meters)); - Trigger autoshootTrigger = - new Trigger(RobotState::isTeleop) - .and(RobotState::isEnabled) - .and(inAllianceZone) - .and(operator.a().negate()) - .and(operator.y().negate()) - .and(() -> !fineControl); - - autoshootTrigger.whileTrue(autoShoot); - - AutoShoot autoPass = - new AutoShoot( - robot.getSwerveDrive(), - robot.getTurret(), - robot.getShooterHood(), - robot.getShooterRollers(), - robot.getPassFunctions(), - () -> - robot.getSwerveDrive().getPosition2d().getY() > AutoShoot.HUB_TRANSLATION.getY() - ? AutoShoot.PASS_RIGHT_TRANSLATION - : AutoShoot.PASS_LEFT_TRANSLATION, - () -> tunableHoodAngle == 0 ? null : Degrees.of(tunableHoodAngle), - () -> tunableRollerVelocity == 0 ? null : RotationsPerSecond.of(tunableRollerVelocity)); - - Trigger autoPassTrigger = - new Trigger(RobotState::isTeleop) - .and(RobotState::isEnabled) - .and(inAllianceZone.negate()) - .and(operator.a().negate()) - .and(operator.y().negate()) - .and(() -> !fineControl); - - autoPassTrigger.whileTrue(autoPass); + shootingActiveTrigger = + new Trigger(RobotState::isTeleop) + .and(RobotState::isEnabled) + .and(operator.a().negate()) + .and(operator.y().negate()) + .and(() -> !fineControl); Trigger shootButtonsTrigger = operator.rightTrigger().or(driver.back()); @@ -344,7 +320,7 @@ public void configureBindings() { .whileTrue( teleopSwerveCommand.limitVelocity( MetersPerSecond.of(1.5), RotationsPerSecond.of(0.5))) // Temporary values - .and(autoPass.isReadyToShoot()) + .and(autoShoot.isReadyToShoot()) .whileTrue(robot.getHopper().feed()); // ShooterFunctions functions = robot.getHubFunctions(); @@ -397,4 +373,12 @@ private Command fineControlDisableRumble() { public XBoxTeleopSwerveCommand getTeleopSwerveCommand() { return teleopSwerveCommand; } + + public Trigger getShootingActiveTrigger() { + return shootingActiveTrigger; + } + + public Trigger getIntakingActiveTrigger() { + return intakingActiveTrigger; + } } diff --git a/src/main/java/frc/robot/subsystems/BrownoutProtection.java b/src/main/java/frc/robot/subsystems/BrownoutProtection.java index 0cd119b9..6185ab60 100644 --- a/src/main/java/frc/robot/subsystems/BrownoutProtection.java +++ b/src/main/java/frc/robot/subsystems/BrownoutProtection.java @@ -3,8 +3,8 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; + +import java.util.function.BooleanSupplier; import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.swerve.CommandSwerveDrive; @@ -18,7 +18,6 @@ import frc.robot.subsystems.hopper.beltfloor.BeltFloor; import frc.robot.subsystems.intakeextension.IntakeExtension; import frc.robot.subsystems.intakerollers.IntakeRollers; -import frc.robot.subsystems.shooterrollers.ShooterRollers; import frc.robot.subsystems.turret.Turret; /** @@ -30,12 +29,11 @@ */ public class BrownoutProtection extends SubsystemBase { private static final int ELECTRICAL_SAMPLE_WINDOW = 8; - private static final double ACTIVE_MECHANISM_VOLTAGE_THRESHOLD_VOLTS = 1.0; - private static final double ACTIVE_SHOOTER_SPEED_THRESHOLD_RPS = 5.0; private final CommandSwerveDrive swerveDrive; private final XBoxTeleopSwerveCommand teleopSwerveCommand; - private final ShooterRollers shooterRollers; + private final BooleanSupplier shootingActive; + private final BooleanSupplier intakingActive; private final Turret turret; private final ShooterHood shooterHood; private final IntakeExtension intakeExtension; @@ -65,7 +63,8 @@ private enum PriorityMode { public BrownoutProtection( CommandSwerveDrive swerveDrive, XBoxTeleopSwerveCommand teleopSwerveCommand, - ShooterRollers shooterRollers, + BooleanSupplier shootingActive, + BooleanSupplier intakingActive, Turret turret, ShooterHood shooterHood, IntakeExtension intakeExtension, @@ -73,7 +72,8 @@ public BrownoutProtection( IntakeRollers intakeRollers) { this.swerveDrive = swerveDrive; this.teleopSwerveCommand = teleopSwerveCommand; - this.shooterRollers = shooterRollers; + this.shootingActive = shootingActive; + this.intakingActive = intakingActive; this.turret = turret; this.shooterHood = shooterHood; this.intakeExtension = intakeExtension; @@ -202,24 +202,9 @@ public void periodic() { } private PriorityMode determinePriorityMode() { - boolean shooterActive = - shooterRollers.getCurrentCommand() != null - || Math.abs(shooterRollers.getMotorVoltage().in(Volts)) - > ACTIVE_MECHANISM_VOLTAGE_THRESHOLD_VOLTS - || Math.abs(shooterRollers.getAngularVelocity().in(RotationsPerSecond)) - > ACTIVE_SHOOTER_SPEED_THRESHOLD_RPS; - - if (shooterActive) { - return PriorityMode.SHOOTING; - } - - boolean intakeActive = - intakeExtension.getCurrentCommand() != null - || intakeRollers.getAppliedVoltage().in(Volts) - > ACTIVE_MECHANISM_VOLTAGE_THRESHOLD_VOLTS - || beltFloor.getMotorVoltage().in(Volts) > ACTIVE_MECHANISM_VOLTAGE_THRESHOLD_VOLTS; - - return intakeActive ? PriorityMode.INTAKING : PriorityMode.DRIVING; + if (shootingActive.getAsBoolean()) return PriorityMode.SHOOTING; + if (intakingActive.getAsBoolean()) return PriorityMode.INTAKING; + return PriorityMode.DRIVING; } private void recordElectricalSample(double batteryVoltageVolts, double totalCurrentAmps) {