diff --git a/src/main/java/frc/game/GameState.java b/src/main/java/frc/game/GameState.java index 6562aa43..34d1921a 100644 --- a/src/main/java/frc/game/GameState.java +++ b/src/main/java/frc/game/GameState.java @@ -116,6 +116,17 @@ public static Optional getAlliance() { return DriverStation.getAlliance(); } + /** + * Returns true if the given pose is inside our alliance zone (within allianceZone_x_len of our + * end wall). Returns false when alliance is unknown. + */ + public static boolean isInMyAllianceZone(Pose2d pose) { + Alliance alliance = getMyAlliance(); + if (alliance == Alliance.Blue) return Field.Region.BlueZone.contains(pose); + if (alliance == Alliance.Red) return Field.Region.RedZone.contains(pose); + return false; + } + public static void logValues() { getMyAlliance(); getAutoWinner(); @@ -131,6 +142,10 @@ public static void logValues() { Logger.recordOutput("GameState/IsMyHubActive", isMyHubActive()); } + public static Pose3d getMyHub() { + return Robot.getAlliance() == Alliance.Red ? Field.redHubCenter : Field.blueHubCenter; + } + public static Pose3d getTarget(Pose2d robotPose) { if (Robot.getAlliance() == Alliance.Red) { if (robotPose.getMeasureX().gt(Field.redHubCenter.getMeasureX())) { diff --git a/src/main/java/frc/lib/LoggedCompressor.java b/src/main/java/frc/lib/LoggedCompressor.java index 8e47b0f5..5a583836 100644 --- a/src/main/java/frc/lib/LoggedCompressor.java +++ b/src/main/java/frc/lib/LoggedCompressor.java @@ -2,20 +2,34 @@ import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; -public class LoggedCompressor extends Compressor { +public class LoggedCompressor extends SubsystemBase { + private final Compressor compressor; private final String key; public LoggedCompressor(PneumaticsModuleType moduleType, String logKey) { - super(moduleType); + compressor = new Compressor(moduleType); this.key = logKey; } + public void disable() { + compressor.disable(); + } + + public void enableDigital() { + compressor.enableDigital(); + } + + public boolean isEnabled() { + return compressor.isEnabled(); + } + public void log() { - Logger.recordOutput(key + "/Enabled", isEnabled()); - Logger.recordOutput(key + "/PressureSwitch", getPressureSwitchValue()); - Logger.recordOutput(key + "/CurrentAmps", getCurrent()); - Logger.recordOutput(key + "/PressurePSI", getPressure()); + Logger.recordOutput(key + "/Enabled", compressor.isEnabled()); + Logger.recordOutput(key + "/PressureSwitch", compressor.getPressureSwitchValue()); + Logger.recordOutput(key + "/CurrentAmps", compressor.getCurrent()); + Logger.recordOutput(key + "/PressurePSI", compressor.getPressure()); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7eb374fa..2307ce6d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -136,6 +136,23 @@ public static final class CANHD { } } + public static final class PowerConstants { + /** Fixed overhead current (RoboRIO, radio, PDH, coprocessors) used in battery simulation. */ + public static final double kElectronicsOverheadAmps = 4.5; + + /** + * PDH total current above which the compressor is inhibited (prevents inrush during high-demand + * periods). Matches the 150A threshold from the VACHE power analysis. + */ + public static final double kCompressorInhibitAmps = 150.0; + + /** + * PDH total current below which the compressor load inhibit clears. Hysteresis gap prevents + * rapid enable/disable cycling near the inhibit threshold. + */ + public static final double kCompressorResumeAmps = 100.0; + } + public static final class PneumaticChannels { // hopper public static final int hopperForward = 14; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a1801a16..a22707d7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -141,8 +141,11 @@ public class Robot extends LoggedRobot { private LoggedCompressor compressor; private PneumaticsSimulator pneumaticsSimulator; - // Battery simulation constants - private static final double ELECTRONICS_OVERHEAD_AMPS = 4.5; // RoboRIO + radio + PDH + misc + // Flywheel policy: true = flywheel permitted by driver input. Overridden by bindZorroDriver; + // defaults to always-enabled for Xbox/keyboard drivers that lack a dial. + private java.util.function.BooleanSupplier flywheelPolicy = () -> true; + + // Battery simulation private final LinearFilter vBusFilter = LinearFilter.singlePoleIIR(0.04, Robot.defaultPeriodSecs); public Robot() { @@ -329,14 +332,24 @@ public Robot() { feeder.setDefaultCommand(Commands.startEnd(feeder::stop, () -> {}, feeder).withName("Stop")); intake.setDefaultCommand(intake.getDefaultCommand()); - launcher.setDefaultCommand( - launcher - .initializeHoodCommand() - .andThen( - new RunCommand( - () -> launcher.aim(GameState.getTarget(drive.getPose()).getTranslation()), - launcher) - .withName("Aim at hub"))); + // Compressor default: enable when PDH load is low, disable when high (hysteresis via + // thresholds). + // Shot commands interrupt this by requiring the compressor subsystem via deadlineWith. + if (compressor != null) { + compressor.setDefaultCommand( + Commands.run( + () -> { + double pdh = powerDistribution.getTotalCurrent(); + if (pdh > Constants.PowerConstants.kCompressorInhibitAmps) { + compressor.disable(); + } else if (pdh < Constants.PowerConstants.kCompressorResumeAmps) { + compressor.enableDigital(); + } + // Between thresholds: maintain current state (hysteresis). + }, + compressor) + .withName("Auto")); + } } /** This function is called periodically during all modes. */ @@ -418,6 +431,7 @@ public void disabledPeriodic() { public void autonomousInit() { if (hopper != null) hopper.getRetractCommand().schedule(); drive.setDefaultCommand(Commands.runOnce(drive::stop, drive).withName("Stop")); + launcher.setDefaultCommand(createAutoLauncherDefault()); autoSelector.scheduleAuto(); leds.clear(); } @@ -435,6 +449,8 @@ public void teleopInit() { if (hopper != null && !hopper.isDeployed() && !hopper.isStowed()) hopper.getRetractCommand().schedule(); autoSelector.cancelAuto(); + flywheelPolicy = () -> true; // reset before binding; overridden below if Zorro is driver + launcher.setDefaultCommand(createTeleopLauncherDefault()); ControllerSelector.getInstance().scan(true); leds.clear(); } @@ -488,7 +504,7 @@ public void simulationPeriodic() { feeder.getSimCurrentDrawAmps(), intake.getSimCurrentDrawAmps(), pneumaticsSimulator.getCompressorCurrentAmps(), - ELECTRONICS_OVERHEAD_AMPS)))); + Constants.PowerConstants.kElectronicsOverheadAmps)))); } private void configureControlPanelBindings() { @@ -508,6 +524,10 @@ private void configureControlPanelBindings() { public DriverController bindZorroDriver(int port) { var zorroDriver = new CommandZorroController(port); + flywheelPolicy = + () -> + DriverStation.isFMSAttached() + || zorroDriver.axisGreaterThan(Axis.kLeftDial.value, 0.5).getAsBoolean(); var controller = new DriverController() { @@ -560,20 +580,6 @@ public boolean getFieldRelativeInput() { // Desaturate turret and advance feeder zorroDriver.AIn().whileTrue(createDesaturateAndShootCommand(controller)); - // Launcher - Trigger launcherEnabled = zorroDriver.axisGreaterThan(Axis.kLeftDial.value, 0.5).debounce(0.1); - launcherEnabled - .or(() -> DriverStation.isFMSAttached()) - .whileTrue( - launcher - .initializeHoodCommand() - .andThen( - new RunCommand( - () -> - launcher.aim(GameState.getTarget(drive.getPose()).getTranslation()), - launcher) - .withName("Aim at hub"))); - // Intake zorroDriver.HIn().whileTrue(intake.getDeployCommand()); @@ -750,7 +756,7 @@ public void bindXboxOperator(int port, DriverController driver) { // xboxOperator.y().and(() -> hopper.isDeployed()).whileTrue(intake.getReverseCommand()); // Feeder - xboxOperator.a().whileTrue(feeder.getSpinForwardCommand()); + xboxOperator.a().whileTrue(shootWithCompressorInhibit()); xboxOperator.x().whileTrue(feeder.getReverseCommand()); @@ -791,6 +797,48 @@ public static long getUSBStorageFreeSpace() { return new java.io.File("/U").getFreeSpace(); } + private Command createAutoLauncherDefault() { + return launcher + .initializeHoodCommand() + .andThen( + new RunCommand( + () -> launcher.aim(GameState.getMyHub().getTranslation(), true), launcher) + .withName("Aim at hub")); + } + + private Command createTeleopLauncherDefault() { + return launcher + .initializeHoodCommand() + .andThen( + new RunCommand( + () -> + launcher.aim( + GameState.getTarget(drive.getPose()).getTranslation(), + flywheelPolicy.getAsBoolean() + && GameState.isMyHubActive() + && GameState.isInMyAllianceZone(drive.getPose())), + launcher) + .withName("Aim at hub")); + } + + /** + * Returns the shoot command with compressor inhibited and flywheel force-enabled for its + * duration. Force-enables the flywheel (overriding hub/zone policy), waits for it to reach speed, + * then advances the feeder. Compressor is disabled for the full shot cycle to prevent 6-20A + * interference (VACHE power analysis recommendation). + */ + private Command shootWithCompressorInhibit() { + var innerCmd = + Commands.sequence( + Commands.runOnce(() -> launcher.setFlywheelForceEnabled(true)), + Commands.waitUntil(launcher::isFlywheelAtSpeed), + feeder.getSpinForwardCommand()) + .finallyDo(() -> launcher.setFlywheelForceEnabled(false)); + if (compressor == null) return innerCmd; + return innerCmd.deadlineWith( + Commands.run(compressor::disable, compressor).withName("Shot inhibit")); + } + private Command createDesaturateAndShootCommand(DriverController driver) { return Commands.parallel( DriveCommands.joystickDrive( @@ -809,7 +857,7 @@ private Command createDesaturateAndShootCommand(DriverController driver) { allianceSelector::fieldRotated) .withName("Desaturate turret"), Commands.sequence( - Commands.waitUntil(launcher::isTurretDesaturated), feeder.getSpinForwardCommand())); + Commands.waitUntil(launcher::isTurretDesaturated), shootWithCompressorInhibit())); } private static void logHIDs() { @@ -840,6 +888,7 @@ private void logScheduler() { logSubsystem("Feeder", feeder); if (hopper != null) logSubsystem("Hopper", hopper); logSubsystem("Intake", intake); + if (compressor != null) logSubsystem("Compressor", compressor); logAlerts(); } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index dcb6062f..748bb7bf 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -17,6 +17,8 @@ public static final class SpindexerConstants { // Motor public static final double motorReduction = 1.0; + // Hard secondary current limit to prevent 149A stall events (VACHE analysis) + public static final int kSecondaryCurrentLimit = 80; public static final LinearVelocity maxTangentialVelocity = MetersPerSecond.of( NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) @@ -39,6 +41,8 @@ public static final class KickerConstants { // Motor public static final double motorReduction = 1.0; + // Hard secondary current limit to prevent 149A stall events (VACHE analysis) + public static final int kSecondaryCurrentLimit = 80; public static final LinearVelocity maxTangentialVelocity = MetersPerSecond.of( NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java index a1b7e430..5a902b7c 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java @@ -41,6 +41,7 @@ public KickerIOSpark() { .inverted(false) .idleMode(IdleMode.kBrake) .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) + .secondaryCurrentLimit(kSecondaryCurrentLimit) .voltageCompensation(RobotConstants.kNominalVoltage); config diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java index 5b6403f8..d67a3481 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java @@ -41,6 +41,7 @@ public SpindexerIOSpark() { .inverted(false) .idleMode(IdleMode.kBrake) .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) + .secondaryCurrentLimit(kSecondaryCurrentLimit) .voltageCompensation(RobotConstants.kNominalVoltage); config diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f8e53da6..afb4a536 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -27,6 +27,9 @@ public static class RollerConstants { public static final Slot1Configs velocityTorqueCurrentGains = new Slot1Configs().withKP(5).withKI(0.0).withKD(0.0).withKS(2.5); + // Stator current limit; reduced from default 100A — observed VACHE peaks were 56A + public static final int kStatorCurrentLimit = 60; + public static final double maxAcceleration = 4000.0; public static final double maxJerk = 40000.0; diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java index 7619a050..0bee629d 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java @@ -42,9 +42,9 @@ public class RollerIOTalonFX implements RollerIO { public RollerIOTalonFX(RollerConfig rollerConfig) { motor = new TalonFX(rollerConfig.port, rollerConfig.bus); config = new TalonFXConfiguration(); - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.kDefaultStatorCurrentLimit; - config.TorqueCurrent.PeakReverseTorqueCurrent = -KrakenX60Constants.kDefaultStatorCurrentLimit; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.kDefaultStatorCurrentLimit; + config.TorqueCurrent.PeakForwardTorqueCurrent = kStatorCurrentLimit; + config.TorqueCurrent.PeakReverseTorqueCurrent = -kStatorCurrentLimit; + config.CurrentLimits.StatorCurrentLimit = kStatorCurrentLimit; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.kDefaultSupplyCurrentLimit; config.CurrentLimits.SupplyCurrentLimitEnable = true; diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java index 9ea59578..dd58b138 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -56,9 +56,9 @@ public FlywheelIOTalonFX() { .withNeutralMode(NeutralModeValue.Brake); config.Slot0 = velocityVoltageGains; config.Slot1 = velocityTorqueCurrentGains; - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.kDefaultStatorCurrentLimit; - config.TorqueCurrent.PeakReverseTorqueCurrent = -KrakenX60Constants.kDefaultStatorCurrentLimit; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.kDefaultStatorCurrentLimit; + config.TorqueCurrent.PeakForwardTorqueCurrent = kStatorCurrentLimit; + config.TorqueCurrent.PeakReverseTorqueCurrent = -kStatorCurrentLimit; + config.CurrentLimits.StatorCurrentLimit = kStatorCurrentLimit; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.kDefaultSupplyCurrentLimit; config.CurrentLimits.SupplyCurrentLimitEnable = true; diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index cc4e621c..c8b9764e 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants; import frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.FlywheelScaling; import java.util.ArrayList; import java.util.function.Supplier; @@ -68,6 +69,14 @@ public class Launcher extends SubsystemBase { private double ballisticSimTimer = 0.0; private double ballisticLogTimer = 0.0; + // Flywheel enable state + // policyEnabled is set by aim() callers based on hub-active / alliance-zone checks. + // forceEnabled is set by shot commands to override the policy and spin up before shooting. + // hoodInitialized gates isFlywheelAtSpeed() to prevent shooting during hood initialization. + private boolean flywheelForceEnabled = false; + private boolean hoodInitialized = false; + private double flywheelSetpointMetersPerSec = 0.0; // last computed setpoint, even while disabled + // Turret desaturation private final PIDController headingController = new PIDController(1.5, 0.0, 0.1); @@ -175,7 +184,11 @@ public double getSimCurrentDrawAmps() { return flywheelInputs.currentAmps * 2 + turretInputs.currentAmps + hoodInputs.currentAmps; } - public void aim(Translation3d target) { + /** + * @param policyEnabled whether hub-active / alliance-zone conditions permit the flywheel to run. + * Ignored when {@link #setFlywheelForceEnabled} is active (e.g. during a shot command). + */ + public void aim(Translation3d target, boolean policyEnabled) { long aimStart = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Get vector from static target to turret @@ -187,10 +200,17 @@ public void aim(Translation3d target) { Math.hypot(vectorTurretBaseToTarget.getX(), vectorTurretBaseToTarget.getY()); Rotation2d dynamicImpactAngle = getImpactAngle(horizontalDistance); - // Set flywheel speed assuming a motionless robot + // Compute nominal flywheel setpoint (always needed for turret aim direction) var v0_nominal = getV0Nominal(vectorTurretBaseToTarget, dynamicImpactAngle, nominalKey); var flywheelSetpoint = MetersPerSecond.of(flywheelSetpointfromBallistics(v0_nominal.getNorm())); - flywheelIO.setVelocity(flywheelSetpoint); + flywheelSetpointMetersPerSec = flywheelSetpoint.in(MetersPerSecond); + + // Spin flywheel only when policy or force-enable permits + if (policyEnabled || flywheelForceEnabled) { + flywheelIO.setVelocity(flywheelSetpoint); + } else { + flywheelIO.setOpenLoop(Volts.of(0.0)); + } long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Get translation velocities (m/s) of the turret caused by motion of the chassis @@ -205,12 +225,19 @@ public void aim(Translation3d target) { double initialSpeedMetersPerSec = ballisticsFromFlywheelSetpoint(flywheelInputs.velocityMetersPerSec); - // Replan shot using actual initial shot speed speed - var v0_total = getV0Replanned(vectorTurretBaseToTarget, initialSpeedMetersPerSec, replannedKey); + // Replan shot using actual flywheel speed; fall back to nominal direction when the flywheel is + // off or still spinning up so the turret continues tracking the target rather than returning + // early due to a near-zero velocity vector. + Translation3d v0_forAim; + if (initialSpeedMetersPerSec < 0.5) { + v0_forAim = v0_nominal; // flywheel off / spinning up — aim using ideal direction + } else { + v0_forAim = getV0Replanned(vectorTurretBaseToTarget, initialSpeedMetersPerSec, replannedKey); + } long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Point turret to align velocity vectors - var v0_flywheel = v0_total.minus(v_base); + var v0_flywheel = v0_forAim.minus(v_base); // Check if v0_flywheel has non-zero horizontal component double v0_horizontal = Math.hypot(v0_flywheel.getX(), v0_flywheel.getY()); @@ -262,7 +289,7 @@ public void aim(Translation3d target) { fuelNominal.add( new BallisticObject(turretBasePose.getTranslation(), v0_nominal, target.getMeasureZ())); fuelReplanned.add( - new BallisticObject(turretBasePose.getTranslation(), v0_total, target.getMeasureZ())); + new BallisticObject(turretBasePose.getTranslation(), v0_forAim, target.getMeasureZ())); fuelActual.add( new BallisticObject(turretBasePose.getTranslation(), v0_actual, target.getMeasureZ())); } @@ -302,6 +329,23 @@ public Rotation2d getHorizontalAimAngle() { return horizontalAimAngle; } + /** + * Called by shot commands to override hub/zone policy and force the flywheel on so it can spin up + * before the feeder advances. Must be cleared (false) when the shot command ends. + */ + public void setFlywheelForceEnabled(boolean force) { + flywheelForceEnabled = force; + } + + /** True when the flywheel is spinning at its current setpoint within the at-speed tolerance. */ + @AutoLogOutput(key = "Launcher/FlywheelAtSpeed") + public boolean isFlywheelAtSpeed() { + if (!hoodInitialized) return false; + if (flywheelSetpointMetersPerSec < 1e-6) return false; + double error = Math.abs(flywheelInputs.velocityMetersPerSec - flywheelSetpointMetersPerSec); + return error < flywheelSetpointMetersPerSec * FlywheelConstants.kAtSpeedToleranceFraction; + } + public double getTurretDesaturationDelta() { return headingController.calculate(-turretInputs.oversaturationLessMargin, 0); } @@ -548,6 +592,7 @@ public Command initializeHoodCommand() { return new StartEndCommand( // initialize () -> { + hoodInitialized = false; hoodIO.configureSoftLimits(false); hoodIO.setOpenLoop(Volts.of(1.0)); }, @@ -555,6 +600,7 @@ public Command initializeHoodCommand() { () -> { hoodIO.configureSoftLimits(true); hoodIO.resetEncoder(); + hoodInitialized = true; }, // requirements this) diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 7fb9e4d2..4d42e718 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -111,6 +111,13 @@ public static final class FlywheelScaling { public static final Distance wheelRadius = Inches.of(1.5); + // Stator current limit; reduced from default 100A — spin-up peaks observed at 40-60A/motor + public static final int kStatorCurrentLimit = 60; + + // At-speed tolerance: flywheel actual speed must be within this fraction of its setpoint + // before the feeder is allowed to advance during a shot cycle. + public static final double kAtSpeedToleranceFraction = 0.05; + // Velocity Controller public static final double maxAcceleration = 4000.0; public static final double maxJerk = 40000.0;