diff --git a/.gitignore b/.gitignore index d4de2d76..9bfac946 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,5 @@ simgui-ds.json simgui-window.json simgui.json networktables.json +bin/main/choreo/README.md +bin/main/frc/robot/README.md diff --git a/src/main/deploy/choreo/README.md b/src/main/deploy/choreo/README.md new file mode 100644 index 00000000..c5785b33 --- /dev/null +++ b/src/main/deploy/choreo/README.md @@ -0,0 +1,23 @@ +## Autonomous Paths +This folder is where all the paths that we made for autonomous pathing exist. Such paths were made using [Choreo](https://choreo.autos/), a path making software. Choreo is how we make all of our autonomous paths, and this year, we opted to make seperate paths, and chain them together for our routines. + +### How to make your own paths! +If you would like to see what the paths look like for yourself, or even make your own, lucky for you its something thats very simple! First things first, you need to make sure that Choreo is installed([link for convienience](https://github.com/SleipnirGroup/Choreo/releases)), then navigate the menu (**LOCATED IN THE TOP LEFT**) to the *open project* tab. From there, navigate to where this cloned repository exists on *your* computer, and navigate to this folder. Select the ```Rebuilt.chor``` file and you're good to go! You can see the other paths there, and make your own from there as well. + + +![Choreo Logo](images/choreoLogo.png) + +### Example of one of our autons +```java + addMultiAuton(kLeftTrenchTwoCycleBumpReturn, + new AdaptableAutonInfo(AutonK.kLeftOneBumpReturn, AutonK.kSOTMTimeout, true, 0), + new AdaptableAutonInfo(AutonK.kLeftTwoBumpToTrench, AutonK.kSOTMTimeout, true, 0), + new AdaptableAutonInfo(AutonK.kLeftTwoBumpReturn, AutonK.kSOTMTimeout, true, 0), + new AdaptableAutonInfo(AutonK.kLeftTwoBumpToTrench, AutonK.kSOTMTimeout, true, 0), + new AdaptableAutonInfo(AutonK.kLeftTwoGoOut, AutonK.kSOTMTimeout, true, 0)); +``` +Note that we chain 5 paths together to create an auton, in which we run the RIGHT_one_bumpReturn, the RIGHT_two_bumpToTrench, the RIGHT_two_bumpReturn, followed by the RIGHT_two_bumpToTrench, finishing with the RIGHT_two_goOut. All of these paths exist as traj's in this folder, and that is where the code pulls from; such paths are, once again, made in choreo. So in order to make a new auton, you must make a plethora of new paths, and make it so that they chain together using the logic seen above. + +### Example of one of our paths (LEFT_one_bumpReturn) +We start at the GREEN waypoint (waypoint one for those of you who are colorblind), and end at the RED waypoint (waypoint 7). The arrow on the square represents the front of our robot, and this year, that is our intake (counter-intuitive, tell me about it :sob:). +![Choreo Path](images/choreoPath.png) diff --git a/src/main/deploy/choreo/images/choreoLogo.png b/src/main/deploy/choreo/images/choreoLogo.png new file mode 100644 index 00000000..f88b9020 Binary files /dev/null and b/src/main/deploy/choreo/images/choreoLogo.png differ diff --git a/src/main/deploy/choreo/images/choreoPath.png b/src/main/deploy/choreo/images/choreoPath.png new file mode 100644 index 00000000..8cf5b828 Binary files /dev/null and b/src/main/deploy/choreo/images/choreoPath.png differ diff --git a/src/main/java/choreo/README.md b/src/main/java/choreo/README.md new file mode 100644 index 00000000..aa8d197c --- /dev/null +++ b/src/main/java/choreo/README.md @@ -0,0 +1,47 @@ +# Choreo + +This folder contains our wrapper and utility classes for integrating [Choreo](https://choreo.autos/) into our robot code. Choreo is the path-planning software we use to generate and follow autonomous trajectories. + +## Why we forked/duplicated Choreo's library + +We duplicated Choreo's library classes locally so we could experiment with **pose correction** during path following. The idea was that if the robot drifts off its expected pose mid-path, we would pause the Choreo timer, drive the robot back to the correct pose, and then resume the timer once the robot is back on track. We weren't able to fully get this working in time, but the groundwork is here. +```java + if(kUsePoseCorrection) { + if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) > 0.2) { + activeTimer.stop(); + outOfBounds.set(true); + } else if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) < 0.05) { + activeTimer.start(); + outOfBounds.set(false); + } + } +``` + +## Structure + +### auto/ +Handles autonomous routine selection and execution. + +- `AutoChooser.java`: lets drivers select an auto routine from the dashboard +- `AutoFactory.java`: constructs auto routines by chaining trajectory segments together +- `AutoRoutine.java`: represents a full autonomous routine made up of multiple trajectories +- `AutoTrajectory.java`: wraps a single Choreo trajectory for command-based execution + +### trajectory/ +Core trajectory representation and sampling logic. + +- `Trajectory.java`: the main trajectory class, holds a list of samples and handles time-based lookups +- `TrajectorySample.java`: a single sampled state along a trajectory (pose, velocity, etc.) +- `SwerveSample.java`: trajectory sample specific to swerve drivetrains +- `DifferentialSample.java`: trajectory sample specific to differential drivetrains +- `EventMarker.java`: represents a timed event trigger within a trajectory + +### util/ +Utility and configuration classes. + +- `Choreo.java`: main entry point for loading trajectories from deploy files +- `ChoreoAlert.java`: handles driver station alerts related to Choreo +- `ChoreoAllianceFlipUtil.java`: flips trajectories for red/blue alliance mirroring +- `ChoreoArrayUtil.java`: array helper methods used internally +- `FieldDimensions.java`: field size constants used for alliance flipping +- `TrajSchemaVersion.java`: version tracking for the trajectory file format diff --git a/src/main/java/choreo/auto/AutoTrajectory.java b/src/main/java/choreo/auto/AutoTrajectory.java index ab2c2074..88e1b1e3 100644 --- a/src/main/java/choreo/auto/AutoTrajectory.java +++ b/src/main/java/choreo/auto/AutoTrajectory.java @@ -10,6 +10,7 @@ import static choreo.util.ChoreoAlert.allianceNotReady; import static edu.wpi.first.wpilibj.Alert.AlertType.kError; import static edu.wpi.first.wpilibj.Alert.AlertType.kWarning; +import static frc.robot.Constants.*; import choreo.Choreo.TrajectoryLogger; import choreo.auto.AutoFactory.AllianceContext; @@ -178,13 +179,19 @@ private void cmdExecute() { return; } var sample = sampleOpt.get(); - // if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) > 0.2) { - // activeTimer.stop(); - // outOfBounds.set(true); - // } else if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) < 0.05) { - // activeTimer.start(); - // outOfBounds.set(false); - // } + /* + * The entire idea of this block is to see that if we are outside of a certain range (0.2 meters in this case), we would pause the autonomous timer - * the timer just tells the robot where it should be, its nothing to + * do with pose based, as choreo is time based * - until the robot reaches the pose (0.05 meters within tolerance in this case). + */ + if(kUsePoseCorrection) { + if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) > 0.2) { + activeTimer.stop(); + outOfBounds.set(true); + } else if (poseSupplier.get().getTranslation().getDistance(sample.getPose().getTranslation()) < 0.05) { + activeTimer.start(); + outOfBounds.set(false); + } + } if (sample instanceof SwerveSample swerveSample) { var swerveController = (Consumer) this.controller; swerveController.accept(swerveSample); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d16f44ca..4e885838 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -57,15 +57,61 @@ import frc.util.AllianceFlipUtil; import frc.util.VisionUtil; +/* + * Constants + * + * This file is the single source of truth for every tunable value, hardware ID, + * and configuration object in the robot code. Keeping everything here makes it + * easy to update a value in one place without hunting through multiple files. + * + * It's organized into nested static classes, one per subsystem or topic: + * MotorK — motor physical constants (free speeds, used for RPS calculations) + * WpiK — WPILib utility constants + * ShooterK — flywheel, turret, and hood settings + TalonFX configs + * VisionK — camera positions relative to the robot + * FieldK — field dimensions and April Tag layout + * RobotK — robot physical dimensions and misc settings + * SuperstructureK — log tab name for Superstructure + * IntakeK — intake arm + roller settings + TalonFX configs + * IndexerK — spindexer + tunnel settings, speed ratios, TalonFX configs + * TurretK — turret encoder offsets and gear tooth counts + * AutonK — auton timeouts and Choreo trajectory file names + * + * Quick note on CTRE motor configs: + * Each motor has a TalonFXConfiguration object built up from sub-configs. + * The PID gains (Slot0Configs) use this terminology: + * kS — static friction compensation (minimum voltage to move the motor at all) + * kV — velocity feedforward (voltage per RPS) + * kA — acceleration feedforward (voltage per RPS/s) + * kP — proportional gain (more error → more correction) + * kI — integral gain (corrects sustained steady-state error over time) + * kD — derivative gain (dampens oscillation) + * SensorToMechanismRatio in FeedbackConfigs accounts for gear reduction so the + * motor controller reports mechanism position/speed directly, not motor shaft position. + * + * CAN bus note: + * kRioBus — default RoboRIO CAN bus (lower bandwidth, used for intake) + * kCanivoreBus — high-speed CANivore bus ("fd"), used for indexer + drivetrain + * kShooterBus — dedicated CAN bus for the shooter subsystem ("shooter") + */ public class Constants { + // Global feature flags — toggle these without touching subsystem code. public static final boolean kDebugLoggingEnabled = false; public static final boolean kDataLoggingEnabled = true; + public static final boolean kUsePoseCorrection = false; public static final double kSimPeriodicUpdateInterval = 0.020; + // CAN bus identifiers. Motors are assigned to specific buses based on bandwidth needs. public static final CANBus kRioBus = CANBus.roboRIO(); public static final CANBus kCanivoreBus = new CANBus("fd"); public static final CANBus kShooterBus = new CANBus("shooter"); + + // ============================================================= + // MOTOR CONSTANTS + // Free speed is the no-load max speed of each motor variant. + // These are used to compute max RPS for velocity scaling. + // ============================================================= public static final class MotorK { public static final double kX60MaxRadPerSec = DCMotor.getKrakenX60(1).freeSpeedRadPerSec; public static final AngularVelocity kX60MaxVelocity = RadiansPerSecond.of(kX60MaxRadPerSec); @@ -77,30 +123,47 @@ public static final class MotorK { public static final double kX44FOCMaxRadPerSec = DCMotor.getKrakenX44Foc(1).freeSpeedRadPerSec; public static final AngularVelocity kX44FOCMaxVelocity = RadiansPerSecond.of(kX44FOCMaxRadPerSec); } + + + // ============================================================= + // WPILib UTILITY CONSTANTS + // ============================================================= public static class WpiK { public static final ChassisSpeeds kZeroChassisSpeeds = new ChassisSpeeds(0, 0, 0); } + + + // ============================================================= + // SHOOTER CONSTANTS + // Flywheel, turret, and hood settings. + // ============================================================= public static class ShooterK { public static final String kLogTab = "Shooter"; - public static final Rotation2d kTurretAngleOffset = Rotation2d.fromRotations(0.106 + 0.0067); //4.87 //0.132324 // was 0.12, decreased 0.014 (~5deg) to fix consistent rightward aim error + + // ---- turret geometry ---- + // The turret is offset from the robot's center. These constants describe + // where it is and how it's rotated relative to the robot frame. + // kTurretAngleOffset corrects for the turret's mechanical zero not being + // aligned with the robot's forward direction. + public static final Rotation2d kTurretAngleOffset = Rotation2d.fromRotations(0.106 + 0.0067); public static final Rotation3d kTurretAngleOffset3d = new Rotation3d(kTurretAngleOffset); public static final Translation3d kTurretTranslation = new Translation3d(Inches.of(-4.744), Inches.of(-4.239), Inches.of(15.769)); public static final Transform3d kTurretTransformNoRotation = new Transform3d(kTurretTranslation, Rotation3d.kZero); public static final Transform3d kTurretTransform = new Transform3d(kTurretTranslation, kTurretAngleOffset3d); - public static final Distance kInchesAboveFunnel = Inches.of(20);// distance the ball must travel above the funnel opening to arc correctly into the hub + + // How far above the funnel opening the ball must travel to arc correctly into the hub. + public static final Distance kInchesAboveFunnel = Inches.of(20); + + // Turret position used when "barfing" (shooting at low speed to clear jams). public static final Angle kTurretBarfPos = Rotations.of(-0.113); public static final boolean kUseStaticShot = false; public static final boolean kAllowDriverRPSTweak = false; - // private static final Pose3dLogger log_turretTransform = WaltLogger.logPose3d(kLogTab, "TurretTransformRaw"); - // static { - // log_turretTransform.accept(kTurretTransform); - // } - public static final Distance kFlywheelRadius = Inches.of(1.5); - // Precomputed doubles for hot-path shot calc (avoid measure allocations) + // Pre-computed doubles for the shot calculator hot path. + // Using doubles directly avoids creating Measure objects on every loop iteration. public static final double kTurretOffsetX_m = kTurretTransform.getTranslation().getX(); public static final double kTurretOffsetY_m = kTurretTransform.getTranslation().getY(); public static final double kTurretAngleOffsetRad = kTurretAngleOffset.getRadians(); @@ -110,69 +173,72 @@ public static class ShooterK { public static final double kFunnelRadiusIn = FieldConstants.Hub.funnelRadius.in(Inches); public static final double kFunnelHeightPlusAboveIn = FieldConstants.Hub.funnelHeight.plus(kInchesAboveFunnel).in(Inches); - // Lateral bias compensation: balls drift left/right as a function of turret angle relative to robot. + // Lateral bias compensation: balls drift left/right as a function of turret angle. // sin(turretRelAngle) = 0 at 0/180°, +1 at 90° (left bias), -1 at 270° (right bias). - // This gain (in rotations) is subtracted * sin to counter the bias. Tune on robot. - public static final double kTurretLateralBiasGainRots = 0;//-0.005 + // This gain (in rotations) is subtracted * sin to counter the drift. Tune on robot. + public static final double kTurretLateralBiasGainRots = 0; public static final int kHopperCapacity = 55; //TODO: find true max public static final double kGravity = MetersPerSecondPerSecond.of(9.81).in(InchesPerSecondPerSecond); - //TODO: work out where our passing spots should be..? - // public static final Translation3d kPassingSpotRight = new Translation3d( - // Meters.of(2), Meters.of(1.5), Meters.zero()); - // public static final Translation3d kPassingSpotLeft = new Translation3d( - // Meters.of(2), Meters.of(6.5), Meters.zero()); + // Passing zone: the X coordinate beyond which the robot can pass to a partner. public static final Distance kPassingX = Meters.of(3.5); public static final double kPassingXAsDouble = kPassingX.in(Meters); + // No-pass zone: a region around the hub where passing is blocked. public static final double kNoPassZoneTopX = FieldConstants.Hub.blueInnerCenterPoint.getX() + 2; public static final double kNoPassZoneRightY = Meters.of(3.2).baseUnitMagnitude(); public static final double kNoPassZoneLeftY = FieldConstants.fieldWidth - 3.2; - + public static final double kShooterTimeout = 1.0; - public static final double kBallDetectedDebounceTime = 1.2; //0.9; + // How long ball detection must be triggered before we count it as a real shot (debounce). + public static final double kBallDetectedDebounceTime = 1.2; - /* MOTOR CONSTANTS */ - public static final double kShooterMoI = 0.000349 * 2.5; //J for 5 3" 0.53lb flywheels + // ---- motor constants ---- + public static final double kShooterMoI = 0.000349 * 2.5; // J for 5x 3" 0.53 lb flywheels public static final double kTurretMoI = 0.104506595; - public static final double kShooterGearing = 1/1; - public static final double kTurretGearing = 41.66666666/1; - public static final double kHoodGearing = 25.0/1; + public static final double kShooterGearing = 1.0 / 1; + public static final double kTurretGearing = 41.66666666 / 1; + public static final double kHoodGearing = 25.0 / 1; public static final int kPeakShooterVolts = 16; - public static final Angle kTurretMaxRotsFromHome = Rotations.of(0.55); //0.75 rots in each direction from home + // ---- turret limits ---- + // The turret can rotate ±0.55 rotations from its home position. + // Software limits prevent it from wrapping the wiring. + public static final Angle kTurretMaxRotsFromHome = Rotations.of(0.55); public static final Angle kTurretMinRots = Rotations.of(-kTurretMaxRotsFromHome.in(Rotations)); public static final Angle kTurretMaxRots = Rotations.of(kTurretMaxRotsFromHome.in(Rotations)); public static final double kTurretMaxErrD = Rotations.of(0.05).in(Rotations); public static final double kTurretMaxErrDSpin = Rotations.of(0.4).in(Rotations); - public static final double kTurretMaxNotAbleToPassRange = 0.23; //sorry for this horrible name i dont know a better one lol - public static final double kTurretMinNotAbleToPassRange = -0.23; //sorry for this horrible name i dont know a better one lol + // Range where the turret can't pass through (cable wrap / mechanical blockage). + public static final double kTurretMaxNotAbleToPassRange = 0.23; + public static final double kTurretMinNotAbleToPassRange = -0.23; + // ---- shooter RPS targets ---- public static final AngularVelocity kShooterMaxRPS = MotorK.kX44MaxVelocity.div(kShooterGearing); public static final double kShooterMaxRPSd = kShooterMaxRPS.in(RotationsPerSecond); - public static final AngularVelocity kShooterRPS = kShooterMaxRPS.times(0.65); //Kraken X44 Max RPM: 7758 + public static final AngularVelocity kShooterRPS = kShooterMaxRPS.times(0.65); public static final double kShooterRPSd = 42.90 + 1.25; - public static final AngularVelocity kShooterAutonCloseRPS = kShooterMaxRPS.times(0.60); //auton pose is closer to the hub than teleop scoring + public static final AngularVelocity kShooterAutonCloseRPS = kShooterMaxRPS.times(0.60); // auton is closer to hub public static final AngularVelocity kShooterAuton_EndSweep_RPS = kShooterMaxRPS.times(0.70); // end of sweep paths public static final AngularVelocity kShooterBarfRPS = kShooterMaxRPS.times(0.37); public static final AngularVelocity kShooterZeroRPS = RotationsPerSecond.zero(); public static final AngularVelocity kShooterSpunUpMinimum = RotationsPerSecond.of(10); - public static final Time kShooterSpunUpTimeout = Seconds.of(0.64); //double expected spinup time + public static final Time kShooterSpunUpTimeout = Seconds.of(0.64); // 2x expected spinup time as a safety margin public static final double kShooterSpunUpMinimumD = kShooterSpunUpMinimum.in(RotationsPerSecond); - public static final double kDriverRPSIncreaseD = 2.0; //biggest cope of the century + public static final double kDriverRPSIncreaseD = 2.0; - //---HOOD CONSTANTS + // ---- hood constants ---- public static final double kHoodMoI = 0.00027505; - public static final Angle kHoodAbsoluteMinRots = Rotations.of(0.0); //ABSOLUTE MIN - private static final Angle kHoodAbsoluteMaxRots = Rotations.of(1.174805); //ABSOLUTE MAX + public static final Angle kHoodAbsoluteMinRots = Rotations.of(0.0); + private static final Angle kHoodAbsoluteMaxRots = Rotations.of(1.174805); public static final Angle kHoodMaxDegs = Degrees.of(kHoodAbsoluteMaxRots.in(Degrees)); public static final Angle kHoodLockDegs = Degrees.of(kHoodMaxDegs.times(0.75).in(Degrees)); public static final double kHoodRotsd = 0.08; @@ -181,26 +247,26 @@ public static class ShooterK { public static final double kHoodMaxErrD = Rotations.of(0.01).in(Rotations); public static final double kHoodAtPosTimeout = 0.1; - //double versions + // double versions for use in hot-path calculations public static final double kHoodMinRots_double = 0.0; public static final double kHoodMaxRots_double = kHoodAbsoluteMaxRots.in(Rotations); public static final double kPhysicalHoodMinPosition_double = 0; public static final double kPhysicalHoodMaxPosition_double = 48; - //TODO: ensure this is the home value - // public static final Angle kHoodHomePosition = Degrees.of(10); public static final Angle kHoodTrenchPosition = Degrees.of(5); + // Custom DC motor model for the hood (NEO 550 with specific characteristics). public static final DCMotor khoodDCMotorGearbox = new DCMotor( - 6, - 0.047, - 2.5, - 0.2, - 24.0855, - 1 + 6, // nominal voltage + 0.047, // stall torque (N·m) + 2.5, // stall current (A) + 0.2, // free current (A) + 24.0855,// free speed (rad/s) + 1 // number of motors ); - /* HOMING */ + // ---- hood homing ---- + // The hood also uses current-sense homing (same principle as the intake arm). public static final Current kWireTugMinAmps = Amps.of(8); public static final double kWireTugMinSecs = 0.125; public static final double kHoodHomingVoltage = -0.75; @@ -208,21 +274,22 @@ public static class ShooterK { public static final Angle kHomePosition = Rotations.of(-0.2175); public static final Angle kInitPosition = Rotations.of(-0.145); - /* IDS */ + // ---- CAN IDs ---- public static final int kShooterA_CANID = 21; public static final int kShooterB_CANID = 20; public static final int kTurretCANID = 12; public static final int kHoodCANID = 22; - /* CONFIGS */ - // TODO: Check what more configs would be necessary - private static final Slot0Configs kShooterASlot0Configs = new Slot0Configs() //Note to self (hrehaan) (and saarth cuz i did the same thing): the default PID sets ZERO volts to a motor, which makes all sim effectively useless cuz the motor has ZERO supplyV - .withKS(0.37) - .withKV(0.1) + // ---- TalonFX configurations ---- + // Slot 0: main shooting PID (used during normal operation) + // Slot 1: alternate gains (used in specific scenarios, e.g. high-speed passing) + private static final Slot0Configs kShooterASlot0Configs = new Slot0Configs() + .withKS(0.37) // static friction: minimum voltage to overcome stiction + .withKV(0.1) // velocity FF: volts per RPS .withKA(0) - .withKP(0.5) + .withKP(0.5) // proportional: corrects speed error .withKI(0) - .withKD(0); // kP was causing the werid sinusoid behavior, kS and kA were adding inconsistency with the destination values + .withKD(0); private static final CurrentLimitsConfigs kShooterACurrentLimitConfigs = new CurrentLimitsConfigs() .withStatorCurrentLimit(80) .withSupplyCurrentLimit(50) @@ -251,15 +318,15 @@ public static class ShooterK { .withFeedback(kShooterAFeedbackConfigs) .withVoltage(kShooterAVoltageConfigs); - - + // Shooter B uses the same config as A but with a different inversion. private static final MotorOutputConfigs kShooterBOutputConfigs = new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Coast); public static final TalonFXConfiguration kShooterBTalonFXConfiguration = kShooterATalonFXConfiguration.clone() .withMotorOutput(kShooterBOutputConfigs); - //---HOOD + // ---- hood TalonFXS configuration ---- + // The hood uses a TalonFXS (for NEO 550 compatibility) instead of a TalonFX. private static final Slot0Configs kHoodSlot0Configs = new Slot0Configs() .withKP(29) .withKI(0) @@ -267,7 +334,7 @@ public static class ShooterK { .withKS(0.5) .withKV(4) .withKA(0) - .withKG(0); + .withKG(0); // gravity FF (not needed here since hood axis is horizontal) private static final CurrentLimitsConfigs kHoodCurrentLimitConfig = new CurrentLimitsConfigs() .withStatorCurrentLimit(30) .withSupplyCurrentLimit(15) @@ -286,14 +353,17 @@ public static class ShooterK { .withMotorArrangement(MotorArrangementValue.NEO550_JST); private static final ExternalFeedbackConfigs kHoodFeedbackConfigs = new ExternalFeedbackConfigs() .withSensorToMechanismRatio(kHoodGearing); + + // Software limits prevent the hood from driving past its physical travel range. public static final SoftwareLimitSwitchConfigs kHoodSoftLimitConfigs = new SoftwareLimitSwitchConfigs() .withForwardSoftLimitThreshold(kHoodAbsoluteMaxRots.minus(Rotations.of(0.05))) .withReverseSoftLimitThreshold(kHoodAbsoluteMinRots.plus(Rotations.of(0.05))) .withForwardSoftLimitEnable(true) .withReverseSoftLimitEnable(true); + // Version with limits disabled — used during homing so the hood can reach its hard stop. public static final SoftwareLimitSwitchConfigs kHoodSoftLimitConfigsNoEnable = kHoodSoftLimitConfigs .withForwardSoftLimitEnable(false) - .withReverseSoftLimitEnable(false); + .withReverseSoftLimitEnable(false); public static final TalonFXSConfiguration kHoodTalonFXSConfiguration = new TalonFXSConfiguration() .withSlot0(kHoodSlot0Configs) .withCurrentLimits(kHoodCurrentLimitConfig) @@ -311,26 +381,26 @@ public static class ShooterK { .withCommutation(kHoodCommutationConfigs) .withSoftwareLimitSwitch(kHoodSoftLimitConfigsNoEnable); - //---TURRET + // ---- turret TalonFX configuration ---- private static final Slot0Configs kTurretSlot0Configs = new Slot0Configs() .withKS(0) .withKV(5) .withKA(0.02) - .withKP(300) //3 - testing values in Pheonix Tuner + .withKP(300) .withKI(0) - .withKD(5); // OLD: kP was too low making the slope less steep, kS kV and kA were causing rlly weird behavior (jumping up/down way further than targeted position) + .withKD(5); private static final CurrentLimitsConfigs kTurretCurrentLimitConfigs = new CurrentLimitsConfigs() .withStatorCurrentLimit(55) .withSupplyCurrentLimit(55) .withSupplyCurrentLowerLimit(15) - .withSupplyCurrentLowerTime(1.0) // drop to 15A after 1 second + .withSupplyCurrentLowerTime(1.0) // drop to 15A after 1 second of high current .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimitEnable(true); private static final MotorOutputConfigs kTurretOutputConfigs = new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake); private static final MotionMagicConfigs kTurretMotionMagicConfigs = new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(110) //TODO: update MMV Configs + .withMotionMagicCruiseVelocity(110) //TODO: update cruise velocity after re-characterization .withMotionMagicAcceleration(20) .withMotionMagicJerk(0); private static final SoftwareLimitSwitchConfigs kTurretSoftwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs() @@ -352,12 +422,14 @@ public static class ShooterK { .withFeedback(kTurretFeedbackConfigs) .withVoltage(kTurretVoltageConfigs); + // Turret absolute encoder (CANcoder) configuration. public static final MagnetSensorConfigs kEncoderAMagnetSensorConfigs = new MagnetSensorConfigs() .withMagnetOffset(TurretK.kEncAMagnetOffset); public static final CANcoderConfiguration kEncoderAConfiguration = new CANcoderConfiguration() .withMagnetSensor(kEncoderAMagnetSensorConfigs); - //Left, Center (Climb), Center (Hub), Right - Driver POV + // Fixed shooter pose overrides — used when auton shooting from known positions. + // [Left, Center (Climb), Center (Hub), Right] from driver POV. public static final Pose2d kShooterOverridePose[] = { AllianceFlipUtil.apply(new Pose2d(FieldK.kFieldLengthMeters / 6, FieldK.kFieldWidthMeters * 2 / 3, new Rotation2d(0))), AllianceFlipUtil.apply(new Pose2d(Units.inchesToMeters(156.61 - 115.05 + 10), FieldK.kFieldWidthMeters / 2, new Rotation2d(0))), @@ -366,111 +438,105 @@ public static class ShooterK { }; } + + // ============================================================= + // VISION CONSTANTS + // Camera positions relative to the robot center (used by PhotonVision). + // Coordinates follow: ONSHAPE X = OUR Y, ONSHAPE Y = OUR X — don't mix these up. + // ============================================================= public static class VisionK { - // public static final Camera[] kCameras = new Camera[4]; - // private static final String kSimCameraSimVisualNames = /"VisionEstimation"; //suffixed to each camera name - // ONSHAPE X IS OUR Y -- ONSHAPE Y IS OUR X !!! NOTE THIS PLEASE DO NOT FORGET - public static final Transform3d kFrontLeftCTR = VisionUtil.transformToRobo(8.875, 12.18175, 20.45, 180, -20, 45); - public static final Transform3d kFrontRightCTR = VisionUtil.transformToRobo(8.875, -12.18175, 20.45, 180, -20, -45); - public static final Transform3d kBackLeftCTR = VisionUtil.transformToRobo(-11.375, 11.875, 20.5625, 0, -20, 135); - public static final Transform3d kBackRightCTR = VisionUtil.transformToRobo(-12.455, -12.055, 18.25, 180,-20, -135); - //Initialize cameras - // static { - // kCameras[0] = new Camera( - // new SimCameraProperties(), - // "FrontLeft", - // kSimCameraSimVisualNames, - // Camera.transformToRobo(10.413, 12.394, 28.844, 0, -10, 45) - // ); - // kCameras[0].setProps("ThriftyCam", 0, 0, 0, 0); - - // kCameras[1] = new Camera( - // new SimCameraProperties(), - // "FrontRight", - // kSimCameraSimVisualNames, - // Camera.transformToRobo(10.413, -12.394, 28.844, 0, -10, 315) - // ); - // kCameras[1].setProps("ThriftyCam", 0, 0, 0, 0); - - // kCameras[2] = new Camera( - // new SimCameraProperties(), - // "BackLeft", - // kSimCameraSimVisualNames, - // Camera.transformToRobo(-11.894, 12.394, 28.844, 0, -10, 135) - // ); - // kCameras[2].setProps("ThriftyCam", 0, 0, 0, 0); - - // kCameras[3] = new Camera( - // new SimCameraProperties(), - // "BackRight", - // kSimCameraSimVisualNames, - // Camera.transformToRobo(-11.894, -12.394, 28.844, 0, -10, 225) - // ); - // kCameras[3].setProps("ThriftyCam", 0, 0, 0, 0); - // } + public static final Transform3d kFrontLeftCTR = VisionUtil.transformToRobo(8.875, 12.18175, 20.45, 180, -20, 45); + public static final Transform3d kFrontRightCTR = VisionUtil.transformToRobo(8.875, -12.18175, 20.45, 180, -20, -45); + public static final Transform3d kBackLeftCTR = VisionUtil.transformToRobo(-11.375, 11.875, 20.5625, 0, -20, 135); + public static final Transform3d kBackRightCTR = VisionUtil.transformToRobo(-12.455, -12.055, 18.25, 180, -20, -135); } + + // ============================================================= + // FIELD CONSTANTS + // Field dimensions and the April Tag layout used for vision pose estimation. + // ============================================================= public static class FieldK { - // take with a grain of salt - pulled from field dimensions (welded) - public static final double kFieldLengthMeters = Units.inchesToMeters(651.22); + // Field dimensions pulled from the 2026 Rebuilt field spec (welded version). + public static final double kFieldLengthMeters = Units.inchesToMeters(651.22); public static final double kFieldWidthMeters = Units.inchesToMeters(317.69); - public static final Pose2d kLeftResetPose = new Pose2d(0.478, 8 - 0.392, Rotation2d.kZero); - public static final Pose2d kRightResetPose = new Pose2d(0.478, 0.392, Rotation2d.kZero); + // Reset poses for each alliance side (used when pressing the reset-pose button). + public static final Pose2d kLeftResetPose = new Pose2d(0.478, 8 - 0.392, Rotation2d.kZero); + public static final Pose2d kRightResetPose = new Pose2d(0.478, 0.392, Rotation2d.kZero); + // April Tag layout with trench tags excluded. We remove them because the trench + // area has poor camera coverage and including those tags hurts pose accuracy. public static final AprilTagFieldLayout kTagLayout; - - //Ignore trench April Tags static { - HashSet excludedAprilTagsID = new HashSet<> (Arrays.asList(1, 6, 7, 12, 17, 22, 23, 28)); + HashSet excludedAprilTagsID = new HashSet<>(Arrays.asList(1, 6, 7, 12, 17, 22, 23, 28)); AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); - List tags = new ArrayList<> (fieldLayout.getTags()); + List tags = new ArrayList<>(fieldLayout.getTags()); tags.removeIf(tag -> excludedAprilTagsID.contains(tag.ID)); kTagLayout = new AprilTagFieldLayout(tags, fieldLayout.getFieldLength(), fieldLayout.getFieldWidth()); } } + + // ============================================================= + // ROBOT CONSTANTS + // Physical dimensions and misc settings. + // ============================================================= public static class RobotK { public static final String kLogTab = "Robot"; public static final int kMiniPCChannel = 14; - // real values - public static final Distance kRobotFullWidth = Inches.of(33.6875); + public static final Distance kRobotFullWidth = Inches.of(33.6875); public static final Distance kRobotFullLength = Inches.of(32.6875); - public static final Distance kBumperHeight = Inches.of(4.5); + public static final Distance kBumperHeight = Inches.of(4.5); + + // Max drive speed while actively intaking (keeps the ball from bouncing off). public static final double kRobotSpeedIntakingLimit = 0.31; + // Max drive speed while evading defense. public static final double kRobotEvasionLimit = 1.5; } + + // ============================================================= + // SUPERSTRUCTURE CONSTANTS + // ============================================================= public static class SuperstructureK { public static final String kLogTab = "Superstructure"; } + + // ============================================================= + // INTAKE CONSTANTS + // Arm and roller motor settings + TalonFX configurations. + // ============================================================= public static class IntakeK { public static final String kLogTab = "Intake"; - /* MOTOR CONSTANTS */ - public static final double kIntakeArmMOI = 0.0209; - public static final double kIntakeArmGearing = 125/1; + // ---- physical constants (used for simulation) ---- + public static final double kIntakeArmMOI = 0.0209; // moment of inertia (kg·m²) + public static final double kIntakeArmGearing = 125.0 / 1; // 125:1 reduction - public static final double kIntakeRollersMOI = 0.0001; // 0.00343880857 - public static final double kIntakeRollersGearing = 12.0/30; + public static final double kIntakeRollersMOI = 0.0001; + public static final double kIntakeRollersGearing = 12.0 / 30; // 0.4:1 (rollers spin faster than motor) - public static final AngularVelocity kIntakeRollersMaxRPS = MotorK.kX60FOCMaxVelocity.div(kIntakeRollersGearing); - public static final AngularVelocity kIntakeRollersShootRPS = kIntakeRollersMaxRPS.times(0.2); - public static final AngularVelocity kIntakeRollersShimmyRPS = kIntakeRollersMaxRPS.times(0.2); - public static final double kIntakeRollersBarfVolts = -12; + // ---- roller speed targets ---- + public static final AngularVelocity kIntakeRollersMaxRPS = MotorK.kX60FOCMaxVelocity.div(kIntakeRollersGearing); + public static final AngularVelocity kIntakeRollersShootRPS = kIntakeRollersMaxRPS.times(0.2); + public static final AngularVelocity kIntakeRollersShimmyRPS = kIntakeRollersMaxRPS.times(0.2); + public static final double kIntakeRollersBarfVolts = -12; public static final double kIntakeRollersIntakeVolts = 11; public static final double kIntakeRollersShimmyVolts = 5; - /* IDS */ - public static final int kIntakeArmCANID = 40; + // ---- CAN IDs ---- + public static final int kIntakeArmCANID = 40; public static final int kIntakeRollersA_CANID = 41; public static final int kIntakeRollersB_CANID = 42; - /* CONFIGS */ - // IntakeArm Motor + // ---- TalonFX configurations ---- + + // Intake arm: MotionMagic position control with soft current limiting. + // Low current limits because the arm is geared heavily and doesn't need much torque. private static final CurrentLimitsConfigs kIntakeArmCurrentLimitConfigs = new CurrentLimitsConfigs() .withStatorCurrentLimit(20) .withSupplyCurrentLimit(20) @@ -478,7 +544,7 @@ public static class IntakeK { .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimitEnable(true); private static final Slot0Configs kIntakeArmSlot0Configs = new Slot0Configs() - .withKS(1.5) + .withKS(1.5) // high static friction — arm is heavy and geared .withKV(0) .withKA(0) .withKP(50) @@ -504,7 +570,8 @@ public static class IntakeK { .withVoltage(kIntakeArmVoltageConfigs) .withFeedback(kIntakeArmFeedbackConfigs); - // IntakeRollers Motors + // Intake rollers A: velocity control. Higher current limits since rollers + // need to grab and accelerate balls quickly. private static final CurrentLimitsConfigs kIntakeRollersACurrentLimitConfigs = new CurrentLimitsConfigs() .withStatorCurrentLimit(55) .withSupplyCurrentLimit(35) @@ -513,7 +580,7 @@ public static class IntakeK { .withSupplyCurrentLimitEnable(true); private static final Slot0Configs kIntakeRollersASlot0Configs = new Slot0Configs() .withKS(0) - .withKV(0.048) // 0.488599348534 + .withKV(0.048) // kV tuned to match voltage-to-velocity relationship .withKA(0) .withKP(0.05) .withKI(0) @@ -524,8 +591,8 @@ public static class IntakeK { public static final FeedbackConfigs kIntakeRollersAFeedbackConfigs = new FeedbackConfigs() .withSensorToMechanismRatio(kIntakeRollersGearing); private static final VoltageConfigs kIntakeRollersAVoltageConfigs = new VoltageConfigs() - .withPeakForwardVoltage(12) //1.2 - .withPeakReverseVoltage(-12); //-1.2 + .withPeakForwardVoltage(12) + .withPeakReverseVoltage(-12); public static final TalonFXConfiguration kIntakeRollersAConfiguration = new TalonFXConfiguration() .withCurrentLimits(kIntakeRollersACurrentLimitConfigs) .withSlot0(kIntakeRollersASlot0Configs) @@ -533,6 +600,7 @@ public static class IntakeK { .withFeedback(kIntakeRollersAFeedbackConfigs) .withVoltage(kIntakeRollersAVoltageConfigs); + // Roller B mirrors A but with opposite inversion (they face each other on the robot). public static final MotorOutputConfigs kIntakeRollersBMotorOutputConfigs = new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Coast); @@ -540,47 +608,66 @@ public static class IntakeK { .withMotorOutput(kIntakeRollersBMotorOutputConfigs); } + + // ============================================================= + // INDEXER CONSTANTS + // Spindexer + tunnel motor settings and speed ratio math. + // ============================================================= public static class IndexerK { public static final String kLogTab = "Indexer"; - - /* IDS */ - //TODO: Make ids accurate + + // ---- CAN IDs ---- + //TODO: verify IDs match physical robot wiring public static final int kSpindexerCANID = 10; - public static final int kTunnelCANID = 11; + public static final int kTunnelCANID = 11; - public static final double kSpindexerGearing = 5.0 ; //5 - public static final double kTunnelGearing = 20.0/18.0; + // ---- gear reductions ---- + public static final double kSpindexerGearing = 5.0; // 5:1 + public static final double kTunnelGearing = 20.0 / 18.0; // ~1.11:1 + // ---- moments of inertia (for simulation) ---- public static final double kSpindexerMOI = 0.00166190059; - public static final double kTunnelMOI = 0.000215968064; - - public static final AngularVelocity kSpindexerMaxRPS = MotorK.kX60MaxVelocity.div(kSpindexerGearing); - public static final AngularVelocity kSpindexerIntakeRPS = kSpindexerMaxRPS.times(-0.10); + public static final double kTunnelMOI = 0.000215968064; + + // ---- spindexer speed targets ---- + public static final AngularVelocity kSpindexerMaxRPS = MotorK.kX60MaxVelocity.div(kSpindexerGearing); + public static final AngularVelocity kSpindexerIntakeRPS = kSpindexerMaxRPS.times(-0.10); // negative = reverse for intake public static final AngularVelocity kSpindexerShootRPS = kSpindexerMaxRPS.times(0.85); - public static final double kSpindexerMaxRPSD = kSpindexerMaxRPS.in(RotationsPerSecond); + public static final double kSpindexerMaxRPSD = kSpindexerMaxRPS.in(RotationsPerSecond); public static final double kSpindexerShootRPSD = kSpindexerShootRPS.in(RotationsPerSecond); public static final double kSpindexerIntakeRPSD = kSpindexerIntakeRPS.in(RotationsPerSecond); - public static final AngularVelocity kTunnelMaxRPS = MotorK.kX60FOCMaxVelocity.div(kTunnelGearing); - public static final AngularVelocity kTunnelShootRPS = kTunnelMaxRPS.times(0.77); //9V - public static final double kTunnelMaxRPSD = kTunnelMaxRPS.in(RotationsPerSecond); + // ---- tunnel speed targets ---- + public static final AngularVelocity kTunnelMaxRPS = MotorK.kX60FOCMaxVelocity.div(kTunnelGearing); + public static final AngularVelocity kTunnelShootRPS = kTunnelMaxRPS.times(0.77); + public static final double kTunnelMaxRPSD = kTunnelMaxRPS.in(RotationsPerSecond); public static final double kTunnelShootRPSD = kTunnelShootRPS.in(RotationsPerSecond); public static final AngularVelocity kTunnelSpunUpMinimum = RotationsPerSecond.of(10); public static final double kTunnelSpunUpMinimumD = 10.0; - public static final Time kTunnelSpunUpTimeout = Seconds.of(1); //double expected spinup time - - /* VELOCITY RATIO (shooter RPS → indexer RPS) */ + public static final Time kTunnelSpunUpTimeout = Seconds.of(1); + + // ---- speed ratio constants ---- + // These physical wheel radii are used to derive indexer speed from shooter speed. + // The goal: match surface speeds at every ball handoff point so the ball flows + // smoothly without getting slowed or jerked. + // + // Derivation (shooter → tunnel): + // surface speed = radius × angular velocity + // r_shooter × ω_shooter = r_tunnel_pulley × ω_tunnel + // → ratio = (r_bigFlywheel + r_smallFlywheel) / (2 × r_tunnelPulley) + // (average of the two flywheel radii since the ball contacts both) public static final double kR_bigFlywheel = ShooterK.kFlywheelRadiusM; // 0.0381 m - public static final double kR_smallFlywheel = 0.0215; // m - public static final double kR_tunnelPulley = 0.018; // m + public static final double kR_smallFlywheel = 0.0215; // m + public static final double kR_tunnelPulley = 0.018; // m public static final double kR_spindexerFloor = 6.5 * 0.0254; // 0.1651 m public static final double kTunnelFromShooterRatio = (kR_bigFlywheel + kR_smallFlywheel) / (2.0 * kR_tunnelPulley); public static final double kSpindexerFromShooterRatio = (kR_bigFlywheel + kR_smallFlywheel) / (2.0 * kR_spindexerFloor); - /* CONFIGS */ - //TODO: Make transfer configs accurate + // ---- TalonFX configurations ---- + + // Spindexer: velocity control, coast on stop. private static final Slot0Configs kSpindexerSlot0Configs = new Slot0Configs() .withKS(0.420) .withKV(0.560) @@ -601,8 +688,8 @@ public static class IndexerK { private static final FeedbackConfigs kSpindexerFeedbackConfigs = new FeedbackConfigs() .withSensorToMechanismRatio(kSpindexerGearing); private static final VoltageConfigs kSpindexerVoltageConfigs = new VoltageConfigs() - .withPeakForwardVoltage(16) //1.2 - .withPeakReverseVoltage(-16); //-1.2 + .withPeakForwardVoltage(16) + .withPeakReverseVoltage(-16); public static final TalonFXConfiguration kSpindexerTalonFXConfiguration = new TalonFXConfiguration() .withSlot0(kSpindexerSlot0Configs) .withCurrentLimits(kSpindexerCurrentLimitConfigs) @@ -610,6 +697,7 @@ public static class IndexerK { .withFeedback(kSpindexerFeedbackConfigs) .withVoltage(kSpindexerVoltageConfigs); + // Tunnel: velocity control with FOC, coast on stop. private static final Slot0Configs kTunnelSlot0Configs = new Slot0Configs() .withKS(0.2) .withKV(0.1337) @@ -630,8 +718,8 @@ public static class IndexerK { private static final FeedbackConfigs kTunnelFeedbackConfigs = new FeedbackConfigs() .withSensorToMechanismRatio(kTunnelGearing); private static final VoltageConfigs kTunnelVoltageConfigs = new VoltageConfigs() - .withPeakForwardVoltage(16) //1.2 - .withPeakReverseVoltage(-16); //-1.2 + .withPeakForwardVoltage(16) + .withPeakReverseVoltage(-16); public static final TalonFXConfiguration kTunnelTalonFXConfiguration = new TalonFXConfiguration() .withSlot0(kTunnelSlot0Configs) .withCurrentLimits(kTunnelCurrentLimitConfigs) @@ -640,118 +728,142 @@ public static class IndexerK { .withVoltage(kTunnelVoltageConfigs); } + + // ============================================================= + // TURRET CONSTANTS + // Encoder offsets and gear tooth counts for the turret position tracking system. + // ============================================================= public static class TurretK { public static final String kLogTab = "Turret"; - + + // Gear tooth counts for the turret's LCM (Least Common Multiple) absolute position tracking. + // The combination of gear tooth counts creates a unique pattern used to find absolute position. public static final double kGearZeroToothCount = 100; - public static final double kGearOneToothCount = 10; - public static final double kGearTwoToothCount = 19; + public static final double kGearOneToothCount = 10; + public static final double kGearTwoToothCount = 19; + + // The turret's LCM reading when it's at the mechanical home position. + // Measured empirically — log "turretLCMPos" and read the value when the turret is at home. + public static final double kLCMAtHomeRots = 0.251; - public static final double kLCMAtHomeRots = 0.251; // measure: turretLCMPos log value when turret is at home + // CANcoder magnet offset: corrects for the encoder not being physically zero-aligned. public static final double kEncAMagnetOffset = 0.320556640625; - public static final double kEncBOffset = 0.529614; // measure: encB reading when turret is at encA=0 //NEW ONE + + // Secondary encoder offset: the reading of encoder B when encoder A reads zero. + // Used as a cross-check for the LCM position calculation. + public static final double kEncBOffset = 0.529614; } + + + // ============================================================= + // AUTON CONSTANTS + // Timeouts and Choreo trajectory file names. + // ============================================================= public static class AutonK { public static final String kLogTab = "Auton"; - public static final Pose2d kRightNeutralPose = new Pose2d(Meters.of(6.924767017364502), + // ---- reference poses ---- + // Used for auton starting position reset and neutral-zone aiming. + public static final Pose2d kRightNeutralPose = new Pose2d(Meters.of(6.924767017364502), Meters.of(2.251265048980713), new Rotation2d(0)); - public static final Pose2d kRightDepotPose = new Pose2d(Meters.of(1.1576627492904663), + public static final Pose2d kRightDepotPose = new Pose2d(Meters.of(1.1576627492904663), Meters.of(5.958622932434082), new Rotation2d(Math.PI)); - - public static final Pose2d kLeftNeutralPose = new Pose2d(Meters.of(6.924767017364502), + public static final Pose2d kLeftNeutralPose = new Pose2d(Meters.of(6.924767017364502), Meters.of(5.437880039215088), new Rotation2d(0)); - public static final double kIntakeTimeout = 7.5; - public static final double kShootingTimeout = 4; //12 - public static final double kSOTMTimeout = 100; //12 + // ---- timeouts ---- + // How long each auton action is allowed to take before giving up and moving on. + // These are safety cutoffs — in ideal conditions the action ends earlier. + public static final double kIntakeTimeout = 7.5; + public static final double kShootingTimeout = 4; + public static final double kSOTMTimeout = 100; // effectively unlimited — SOTM doesn't block on shot confirmation public static final double kSweepShootingTimeout = 20; + // Delay (seconds) between segments when following another robot. public static final double kFollowDelay = 2; - /* OLD PATHS */ - //---RIGHT FIRST CYCLES - public static final String kRightOneJab = "RIGHT_one_jab"; - public static final String kRightOneTrench = "RIGHT_one_trench"; - public static final String kRightOneDefense = "RIGHT_one_defense"; - public static final String kRightOneReverse = "RIGHT_one_reverse"; + // ---- trajectory file names ---- + // These strings are the file names of the Choreo trajectory JSON files + // (without the .traj extension). They're used by WaltAdaptableAutonFactory + // and AutonChooser to load paths. + // + // Naming convention: + // SIDE_cycle_description + // e.g. "RIGHT_one_jab" = right-side start, first cycle, jab path + // "LEFT_two_sweep" = left-side start, second cycle, sweep path + + /* OLD PATHS (kept for reference / regression testing) */ + public static final String kRightOneJab = "RIGHT_one_jab"; + public static final String kRightOneTrench = "RIGHT_one_trench"; + public static final String kRightOneDefense = "RIGHT_one_defense"; + public static final String kRightOneReverse = "RIGHT_one_reverse"; - //---RIGHT SECOND CYCLES public static final String kRightTwoSotmDepot = "RIGHT_two_sotmDepot"; - public static final String kRightTwoDepot = "RIGHT_two_depot"; - public static final String kRightTwoSweep = "RIGHT_two_sweep"; - public static final String kRightTwoPassing = "RIGHT_two_passing"; - public static final String kRightTwoJab = "RIGHT_two_jab"; - public static final String kRightTwoReverse = "RIGHT_two_reverse"; - - //---LEFT FIRST CYCLES - public static final String kLeftOneJab = "LEFT_one_jab"; - public static final String kLeftOneTrench = "LEFT_one_trench"; - public static final String kLeftOneDefense = "LEFT_one_defense"; - public static final String kLeftOneReverse = "LEFT_one_reverse"; - - //---LEFT SECOND CYCLES + public static final String kRightTwoDepot = "RIGHT_two_depot"; + public static final String kRightTwoSweep = "RIGHT_two_sweep"; + public static final String kRightTwoPassing = "RIGHT_two_passing"; + public static final String kRightTwoJab = "RIGHT_two_jab"; + public static final String kRightTwoReverse = "RIGHT_two_reverse"; + + public static final String kLeftOneJab = "LEFT_one_jab"; + public static final String kLeftOneTrench = "LEFT_one_trench"; + public static final String kLeftOneDefense = "LEFT_one_defense"; + public static final String kLeftOneReverse = "LEFT_one_reverse"; + public static final String kLeftTwoSotmDepot = "LEFT_two_sotmDepot"; - public static final String kLeftTwoDepot = "LEFT_two_depot"; - public static final String kLeftTwoSweep = "LEFT_two_sweep"; - public static final String kLeftTwoPassing = "LEFT_two_passing"; - public static final String kLeftTwoJab = "LEFT_two_jab"; - public static final String kLeftTwoReverse = "LEFT_two_reverse"; - - //---MISC - public static final String kRightOneCircle = "RIGHT_one_circle"; - public static final String kLeftOneSweepAndDepot = "LEFT_one_sweepAndDepot"; - public static final String kLeftThreeDepotToBump = "LEFT_three_depotToBump"; + public static final String kLeftTwoDepot = "LEFT_two_depot"; + public static final String kLeftTwoSweep = "LEFT_two_sweep"; + public static final String kLeftTwoPassing = "LEFT_two_passing"; + public static final String kLeftTwoJab = "LEFT_two_jab"; + public static final String kLeftTwoReverse = "LEFT_two_reverse"; + + public static final String kRightOneCircle = "RIGHT_one_circle"; + public static final String kLeftOneSweepAndDepot = "LEFT_one_sweepAndDepot"; + public static final String kLeftThreeDepotToBump = "LEFT_three_depotToBump"; public static final String kRightThreeDepotToBump = "RIGHT_three_depotToBump"; - //---STRESS TEST - public static final String kRightStressTestLong = "RIGHT_stress_test_long"; + public static final String kRightStressTestLong = "RIGHT_stress_test_long"; public static final String kRightStressTestOverlap = "RIGHT_stress_test_overlap"; - + /* NEW PATHS */ - //---BUMP RETURN PATHS - public static final String kRightOneBumpReturn = "RIGHT_one_bumpReturn"; + public static final String kRightOneBumpReturn = "RIGHT_one_bumpReturn"; public static final String kRightOneBumpReturnFollow = "RIGHT_one_bumpReturnFollow"; - public static final String kLeftOneBumpReturn = "LEFT_one_bumpReturn"; - public static final String kLeftOneBumpReturnFollow = "LEFT_one_bumpReturnFollow"; - public static final String kRightTwoBumpReturn = "RIGHT_two_bumpReturn"; - public static final String kLeftTwoBumpReturn = "LEFT_two_bumpReturn"; - public static final String kRightTwoBumpToTrench = "RIGHT_two_bumpToTrench"; - public static final String kLeftTwoBumpToTrench = "LEFT_two_bumpToTrench"; - - //---TRENCH RETURN PATHS - public static final String kRightOneTrenchReturn = "RIGHT_one_trenchReturn"; - public static final String kLeftOneTrenchReturn = "LEFT_one_trenchReturn"; - public static final String kRightTwoTrenchReturn = "RIGHT_two_trenchReturn"; - public static final String kLeftTwoTrenchReturn = "LEFT_two_trenchReturn"; + public static final String kLeftOneBumpReturn = "LEFT_one_bumpReturn"; + public static final String kLeftOneBumpReturnFollow = "LEFT_one_bumpReturnFollow"; + public static final String kRightTwoBumpReturn = "RIGHT_two_bumpReturn"; + public static final String kLeftTwoBumpReturn = "LEFT_two_bumpReturn"; + public static final String kRightTwoBumpToTrench = "RIGHT_two_bumpToTrench"; + public static final String kLeftTwoBumpToTrench = "LEFT_two_bumpToTrench"; + + public static final String kRightOneTrenchReturn = "RIGHT_one_trenchReturn"; + public static final String kLeftOneTrenchReturn = "LEFT_one_trenchReturn"; + public static final String kRightTwoTrenchReturn = "RIGHT_two_trenchReturn"; + public static final String kLeftTwoTrenchReturn = "LEFT_two_trenchReturn"; public static final String kRightOneBumpTrenchReturn = "RIGHT_one_bumpReverseToTrench"; - public static final String kLeftOneBumpTrenchReturn = "LEFT_one_bumpReverseToTrench"; - - //---OUTPOST PATHS - public static final String kRightOneTrenchToOutpost = "RIGHT_one_trenchToOutpost"; - public static final String kRightTwoTrenchToOutpost = "RIGHT_two_trenchToOutpost"; - public static final String kRightTwoOutpostToTrench = "RIGHT_two_outpostToTrench"; - public static final String kRightOneBumpToOutpost = "RIGHT_one_bumpToOutpost"; - public static final String kRightTwoBumpToOutpost = "RIGHT_two_bumpToOutpost"; - public static final String kRightTwoOutpostToBump = "RIGHT_two_outpostToBump"; - - //---DEPOT PATHS - public static final String kLeftOneTrenchToDepot = "LEFT_one_trenchToDepot"; - public static final String kLeftTwoTrenchToDepot = "LEFT_two_trenchToDepot"; - public static final String kLeftTwoDepotToTrench = "LEFT_two_depotToTrench"; - public static final String kLeftOneBumpToDepot = "LEFT_one_bumpToDepot"; - public static final String kLeftTwoBumpToDepot = "LEFT_two_bumpToDepot"; - public static final String kLeftTwoDepotToBump = "LEFT_two_depotToBump"; - - //---MISC - public static final String kRightOneSelfPass = "RIGHT_one_selfPass"; - public static final String kLeftOneSelfPass = "LEFT_one_selfPass"; - public static final String kRightTwoGoOut = "RIGHT_two_goOut"; - public static final String kLeftTwoGoOut = "LEFT_two_goOut"; - public static final String kRightBumpPreload = "RIGHT_one_bumpPreload"; - public static final String kLeftBumpPreload = "LEFT_one_bumpPreload"; + public static final String kLeftOneBumpTrenchReturn = "LEFT_one_bumpReverseToTrench"; + + public static final String kRightOneTrenchToOutpost = "RIGHT_one_trenchToOutpost"; + public static final String kRightTwoTrenchToOutpost = "RIGHT_two_trenchToOutpost"; + public static final String kRightTwoOutpostToTrench = "RIGHT_two_outpostToTrench"; + public static final String kRightOneBumpToOutpost = "RIGHT_one_bumpToOutpost"; + public static final String kRightTwoBumpToOutpost = "RIGHT_two_bumpToOutpost"; + public static final String kRightTwoOutpostToBump = "RIGHT_two_outpostToBump"; + + public static final String kLeftOneTrenchToDepot = "LEFT_one_trenchToDepot"; + public static final String kLeftTwoTrenchToDepot = "LEFT_two_trenchToDepot"; + public static final String kLeftTwoDepotToTrench = "LEFT_two_depotToTrench"; + public static final String kLeftOneBumpToDepot = "LEFT_one_bumpToDepot"; + public static final String kLeftTwoBumpToDepot = "LEFT_two_bumpToDepot"; + public static final String kLeftTwoDepotToBump = "LEFT_two_depotToBump"; + + public static final String kRightOneSelfPass = "RIGHT_one_selfPass"; + public static final String kLeftOneSelfPass = "LEFT_one_selfPass"; + public static final String kRightTwoGoOut = "RIGHT_two_goOut"; + public static final String kLeftTwoGoOut = "LEFT_two_goOut"; + public static final String kRightBumpPreload = "RIGHT_one_bumpPreload"; + public static final String kLeftBumpPreload = "LEFT_one_bumpPreload"; public static final String kRightTrenchPreload = "RIGHT_one_trenchPreload"; - public static final String kLeftTrenchPreload = "LEFT_one_trenchPreload"; + public static final String kLeftTrenchPreload = "LEFT_one_trenchPreload"; public static final String kCenterPreload = "CENTER_one_preload"; } diff --git a/src/main/java/frc/robot/README.md b/src/main/java/frc/robot/README.md new file mode 100644 index 00000000..31fc0db1 --- /dev/null +++ b/src/main/java/frc/robot/README.md @@ -0,0 +1,82 @@ +# /robot + +This is the main area for the ReBuilt Season. Typically, everything here is all you'll need, as everything here controls how the robot behaves during a match, from driving around to shooting game pieces at targets. + +If you're new to the team, welcome! Hopefully this will help you figure out where things live so you're not completely lost staring at the file tree, like I was ~~two years ago~~ all those years ago. + +--- + +## How the code is organized + +The code is split into folders by responsibility. Each folder handles one aspect of the robot. If something breaks, you'll know exactly where to look (and who to blame(thank you gitblame)). + +--- + +## Top-level files + +- `Robot.java`: Main robot class. Subsystems, commands, and bindings all get wired up here. This will have almost every concept that you would need to understand how to program in FRC! :wink: +- `Main.java`: Program entry point. You ~~probably~~ never need to touch this. +- `Constants.java`: Robot-wide constants (motor ports, PID values, speed limits, etc.). If you're looking for a magic number, it's probably here. NO MAGIC NUMBERS OUTSIDE OF HERE!!!! +- `FieldConstants.java`: Field dimensions and target positions. All thanks to our goats, Mechanical Advantage (6328) + +### [autons/](autons/): Autonomous routines + +- `WaltAdaptableAutonFactory.java`: Builds auto routines by chaining Choreo trajectory segments together. + +--- + +### [dashboards/](dashboards/): Driver station UI + +Controls what drivers see on their dashboard laptop. + +- `AutonChooser.java`: Dropdown that lets drivers pick which auto routine to run before a match. + +--- + +### [generated/](generated/): Auto-generated files + +Created by external tools, not written by hand. Don't edit these directly unless you know what you're doing LOL + +- `TunerConstants.java`: Swerve drive tuning constants from CTRE's Tuner X (THANK GOD FOR TUNERX). + +--- + +### [subsystems/](subsystems/): Robot mechanisms + +Each file represents a physical mechanism on the robot(for the most part) and the code that controls it. This is where most of the action happens. + +#### [subsystems/shooter/](subsystems/shooter/): Shooting mechanism + +The fun part. I had SO much fun on this part :D + +- `Shooter.java`: Master control over the entire shooter subsystem. +- `Hood.java`: Adjustable hood angle and other variety. +- `Turret.java`: Turret rotation and tracking. +- `TurretVisualizer.java`: 3D visualization for turret state. +- `FuelSim.java`: Game piece physics simulation for testing. + +##### [subsystems/shooter/calc](subsystems/shooter/calc): Shot calculations + +this is where it hit the fan :scream: :scream: :scream: + +- `ShotCalcMath.java`: Shot math and distance-based calculations on its own thread. +- `ShotCalculator.java`: Other shot math and distance-based calculations. + +#### Other subsystems + +- `Intake.java`: Picks up game pieces from the ground. +- `Indexer.java`: Feeds game pieces from the intake into the shooter. The middleman. +- `Superstructure.java`: Coordinates intake, indexer, and shooter together so nothing fires when it shouldn't. +- `Swerve.java`: Swerve drivetrain. Makes the robot go vroom in any direction. + +--- + +### [vision/](vision/): Target tracking + +Uses cameras to localize where we are on the field. + +- `WaltCamera.java`: Camera wrapper. +- `Detection.java`: Detects game pieces. +- `VisionSim.java`: Simulates vision for testing. + +--- diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5cf9470f..dccf3ee3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,7 +20,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import choreo.auto.AutoFactory; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.AngularVelocity; @@ -60,28 +59,29 @@ import frc.util.WaltLogger.DoubleLogger; import frc.util.WaltLogger.Pose2dLogger; +/* + * Robot - top-level TimedRobot. wires up all subsystems, controllers, triggers, and auton. + * All button binds live in configureBindings(). Sim wiring is in simulationPeriodic(). + * + * Startup profiling: kFieldInitStart captures a timestamp before field initializers run so + * the constructor can print how long subsystem/HW construction took. + */ public class Robot extends TimedRobot { - // Captured before any field initializers run (subsystem construction, etc.) + // ---- CONSTANTS ---- + // captured before any field initializers run (subsystem construction, etc.) private static final long kFieldInitStart = System.nanoTime(); - - /* CLASS VARIABLES */ - //---CONSTANTS private final LinearVelocity kMaxTranslationSpeed = TunerConstants.kSpeedAt12Volts; // kSpeedAt12Volts desired top speed private final AngularVelocity kMaxAngularRate = RotationsPerSecond.of(1.05); // 3/4 of a rotation per second max angular velocity - - // Pre-computed doubles for driveCommand hot path (avoids .times() measure allocations every tick) + // pre-computed doubles for driveCommand hot path (avoids .times() measure allocations every tick) private final double kMaxTranslationMps = kMaxTranslationSpeed.in(MetersPerSecond); private final double kMaxAngularRps = kMaxAngularRate.in(RadiansPerSecond); - private double m_visionSeenLastSec = Utils.getCurrentTimeSeconds(); - private final BooleanLogger log_visionSeenPastSecond = new BooleanLogger(kLogTab, "VisionSeenLastSec"); - - /* Setting up bindings for necessary control of the swerve drive platform */ + // ---- SWERVE REQUESTS ---- private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(kMaxTranslationSpeed.times(0.1)).withRotationalDeadband(kMaxAngularRate.times(0.1)) // Add a 10% deadband .withDriveRequestType(DriveRequestType.Velocity); // Use open-loop control for drive motors - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + // private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); // private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); // private final SwerveRequest.RobotCentric tuneDrive = new SwerveRequest.RobotCentric() @@ -90,22 +90,20 @@ public class Robot extends TimedRobot { // private final Telemetry logger = new Telemetry(kMaxTranslationSpeed.in(MetersPerSecond)); + // ---- RATE LIMITERS ---- private final SlewRateLimiter limit_driverX = new SlewRateLimiter(0.30); private final SlewRateLimiter limit_driverY = new SlewRateLimiter(0.30); private final SlewRateLimiter limit_driverYawRate = new SlewRateLimiter(0.30); - //---CONTROLLERS + // ---- CONTROLLERS ---- private final CommandXboxController m_driver = new CommandXboxController(0); private final CommandXboxController m_manipulator = new CommandXboxController(1); - // Cached so the drive default-command lambda doesn't allocate a Trigger every tick. - private final Trigger trg_driverSlow = m_driver.leftTrigger(); - - //---INIT SUBSYSTEMS + // ---- SUBSYSTEMS ---- public final Swerve m_drivetrain = TunerConstants.createDrivetrain(); private final Shooter m_shooter = new Shooter( - () -> m_drivetrain.getState().Pose, + () -> m_drivetrain.getState().Pose, () -> m_drivetrain.getStateCopy(), () -> m_drivetrain.getChassisSpeeds()); @@ -115,57 +113,54 @@ public class Robot extends TimedRobot { // private final WaltVisualSim m_visualSim; private final Superstructure m_superstructure = new Superstructure(m_intake, m_indexer, m_shooter); - //---AUTONS + // ---- AUTONS ---- private final AutoFactory m_autoFactory = m_drivetrain.createAutoFactory(); private final WaltAdaptableAutonFactory m_adpatableAutonFactory = new WaltAdaptableAutonFactory(m_superstructure, m_autoFactory, m_intake, m_shooter, m_drivetrain); - //---VISION + // ---- HARDWARE ---- private PowerDistribution m_PDH = new PowerDistribution(); // private final NetworkPinger m_radioPinger = new NetworkPinger("Radio", "10.29.74.1", 0.2, 10); // private final NetworkPinger m_coprocessorPinger = new NetworkPinger("Coprocessor", "10.29.74.11", 0.2, 10); // private final VisionSim m_visionSim = new VisionSim(); - /* TRIGGERS */ + // ---- TRIGGERS ---- // private Trigger trg_optimalPrefireTime = new Trigger(HubShiftUtil.optimalPrefireTime()); // private Trigger trg_comebackTime = new Trigger(HubShiftUtil.comebackTime()); private final Trigger trg_snappingBack = new Trigger(m_shooter.m_turret.isSnappingBack()); private final Trigger trg_driverOverride = m_driver.b(); private final Trigger trg_manipOverride = m_manipulator.b(); - //---DRIVER BUTTONS + // driver private final Trigger trg_shoot = m_driver.rightTrigger().and(trg_driverOverride.negate()); private final Trigger trg_emergencyBarf = m_driver.rightTrigger().and(trg_driverOverride); private final Trigger trg_unjam = m_driver.rightBumper(); private final Trigger trg_resetPoseLeft = m_driver.leftBumper().and(trg_driverOverride.and(m_driver.povLeft())); private final Trigger trg_resetPoseRight = m_driver.leftBumper().and(trg_driverOverride.and(m_driver.povRight())); - private final Trigger trg_lockShooting = m_driver.povRight(); private final Trigger trg_unlockShooting = m_driver.povDown(); + private final Trigger trg_driverSlow = m_driver.leftTrigger(); - //---MANIPULATOR BUTTONS + // manipulator private final Trigger trg_intake = m_manipulator.rightTrigger().and(trg_manipOverride.negate()); private final Trigger trg_retractIntake = m_manipulator.rightBumper().and(trg_manipOverride.negate()); private final Trigger trg_intakeShimmy = m_manipulator.leftBumper(); - private final Trigger trg_emergencyIntakeOnlyBarf = m_manipulator.rightTrigger().and(trg_manipOverride); - - private final Trigger trg_homeIntake = m_manipulator.x().and(trg_manipOverride); + private final Trigger trg_homeIntake = m_manipulator.x().and(trg_manipOverride); private final Trigger trg_homeHood = m_manipulator.start().and(trg_manipOverride); private final Trigger trg_reseedTurret = m_manipulator.y().and(trg_manipOverride); - //---MISC TRGs + // mode private final Trigger trg_limitFPS = RobotModeTriggers.disabled(); private final Trigger trg_unlimitFps = RobotModeTriggers.autonomous().or(RobotModeTriggers.teleop()); - - + // ---- LOGGERS ---- + private final BooleanLogger log_visionSeenPastSecond = new BooleanLogger(kLogTab, "VisionSeenLastSec"); private final DoubleLogger log_miniPCCurrent = WaltLogger.logDouble(kLogTab, "MiniPC current"); private final DoubleLogger log_rioBusVoltage = WaltLogger.logDouble(kLogTab, "RioBusVoltage"); private final BooleanLogger log_rioBrownout = WaltLogger.logBoolean(kLogTab, "RioBrownout"); private final DoubleLogger log_pdhCurrentTotal = WaltLogger.logDouble(kLogTab, "PDHCurrTotal"); private final BooleanLogger log_isDSAttatched = WaltLogger.logBoolean(kLogTab, "isDSAttatched"); private final Pose2dLogger log_robotPose = WaltLogger.logPose2d("Drive", "Pose", true); - private final DoubleLogger log_autonTime = WaltLogger.logDouble("Auton", "autonTime"); //NOT DISPLAYING @@ -178,11 +173,19 @@ public class Robot extends TimedRobot { // private final DoubleLogger log_remainingFudgedTime = WaltLogger.logDouble("Util/Shift", "currentFudgedTime"); // private final BooleanLogger log_isActiveFudged = WaltLogger.logBoolean("Util/Shift", "isActiveFudged"); + // ---- RUNTIME STATE ---- + private double m_visionSeenLastSec = Utils.getCurrentTimeSeconds(); private final Tracer m_periodicTracer = new Tracer(); private final PerformanceMonitor m_perfMonitor = new PerformanceMonitor(false); - private final Command m_preheaterCommand; + private final Command m_preheaterCommand; // assigned in constructor after AutonChooser is ready + private final Timer m_fpsLimitTimer = new Timer(); + private final Timer lastGotTagMsmtTimer = new Timer(); + private final Timer m_disableChangeDelayTimer = new Timer(); + + // ============================================================= + // CONSTRUCTOR + // ============================================================= - /* CONSTRUCTOR */ public Robot() { long t0 = System.nanoTime(); long tPrev = t0; @@ -233,8 +236,6 @@ public Robot() { PhotonCamera.setVersionCheckEnabled(false); LiveWindow.disableAllTelemetry(); - - // MANUAL HOMING IS BEING USED // addPeriodic(m_shooter::fastPeriodic, 0.0025); @@ -253,10 +254,13 @@ public Robot() { System.out.printf("[INIT PROFILE] CONSTRUCTOR TOTAL: %7.1f ms%n", (tEnd - t0) * 1e-6); } - /* COMMANDS */ + // ============================================================= + // DRIVE COMMAND + // ============================================================= + /** - * * @param speedMult how much you want to limit speed as a decimal percentage of kMaxTranslation. 1 does nothing + * @param rotationMult how much you want to limit rotational-speed as a decimal percentage of kMaxAngularRPS. 1 does nothing. * @return swerve drive command */ private Command driveCommand(double speedMult, double rotationMult) { @@ -289,7 +293,10 @@ private Command driveCommand(double speedMult, double rotationMult) { // m_manipulator.setRumble(type, intensity); // } - //---BINDINGS + // ============================================================= + // BINDINGS + // ============================================================= + private void configureBindings() { /* SET UP */ m_drivetrain.setDefaultCommand(driveCommand(RobotK.kRobotSpeedIntakingLimit, RobotK.kRobotEvasionLimit)); @@ -300,7 +307,7 @@ private void configureBindings() { m_drivetrain.applyRequest(() -> idle).ignoringDisable(true) ); - trg_limitFPS.onTrue(WaltCamera.setFpsLimitCmd(true)); + trg_limitFPS.onTrue(WaltCamera.setFpsLimitCmd(true)); trg_unlimitFps.onTrue(WaltCamera.setFpsLimitCmd(false)); /* BUTTON BIDNDS */ @@ -355,7 +362,7 @@ private void configureBindings() { // () -> // HubShiftUtil.getShiftedShiftInfo().active() // || m_shooter.getCurrentGoal().equals(ShooterGoal.PASSING)); - + // trg_optimalPrefireTime.whileTrue( // Commands.run(() -> setBothRumble(RumbleType.kBothRumble, 0.5)).finallyDo(() -> setBothRumble(RumbleType.kBothRumble, 0)) // ); @@ -365,7 +372,7 @@ private void configureBindings() { // ); // m_drivetrain.registerTelemetry(logger::telemeterize); //UNUSED - runs at 250hz which is burning CPU - + //-used when the shooter couldn't shoot while aiming close to the hopper wall // trg_turretInShootRange.whileFalse(Commands.run(() -> m_driver.setRumble(RumbleType.kBothRumble, 0.3)) // .finallyDo(() -> m_driver.setRumble(RumbleType.kBothRumble, 0)) @@ -389,7 +396,10 @@ private void configureTestBindings() { // ); } - /* PERIODICS */ + // ============================================================= + // ROBOT PERIODIC + // ============================================================= + @Override public void robotPeriodic() { m_perfMonitor.loopStart(); @@ -457,14 +467,13 @@ public void robotPeriodic() { // ) // ); - // m_periodicTracer.printEpochs(); m_perfMonitor.loopEnd(); } - private final Timer m_fpsLimitTimer = new Timer(); - private final Timer lastGotTagMsmtTimer = new Timer(); - private final Timer m_disableChangeDelayTimer = new Timer(); + // ============================================================= + // MODE LIFECYCLE + // ============================================================= @Override public void disabledInit() { @@ -525,7 +534,7 @@ public void teleopExit() {} @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); - + CommandScheduler.getInstance().schedule( Commands.sequence( m_drivetrain.runOnce(m_drivetrain::seedFieldCentric), @@ -569,6 +578,10 @@ public void testPeriodic() {} @Override public void testExit() {} + // ============================================================= + // SIMULATION + // ============================================================= + @Override public void simulationInit() { // FuelSim.getInstance().start(); diff --git a/src/main/java/frc/robot/autons/WaltAdaptableAutonFactory.java b/src/main/java/frc/robot/autons/WaltAdaptableAutonFactory.java index 02743435..65156953 100644 --- a/src/main/java/frc/robot/autons/WaltAdaptableAutonFactory.java +++ b/src/main/java/frc/robot/autons/WaltAdaptableAutonFactory.java @@ -1,7 +1,5 @@ package frc.robot.autons; -import static frc.robot.Constants.IntakeK.kIntakeRollersIntakeVolts; - import java.util.Set; import java.util.function.Supplier; @@ -23,132 +21,269 @@ import frc.util.WaltLogger.Pose2dArrayLogger; import frc.util.WaltLogger.StringLogger; +/* + * WaltAdaptableAutonFactory + * + * This class is responsible for building all of our autonomous routines. + * Rather than writing a brand new command sequence for every single auton we + * want to run, this factory takes a list of trajectory segments + settings and + * assembles the full routine automatically — hence "adaptable". + * + * Quick vocabulary for new programmers: + * Trajectory — a pre-planned path the robot drives along, created in the + * Choreo desktop app and saved as a JSON file on the robot. + * Event marker / waypoint — a named timestamp placed inside Choreo. + * When the robot's path timer hits that timestamp, a Trigger + * fires and we run a command (e.g. start intaking, start shooting). + * Trigger — a WPILib class that watches a boolean condition and runs + * commands when that condition becomes true or false. + * SOTM — "Shoot On The Move". Instead of stopping the robot to shoot, + * SOTM mode lets the robot keep driving while the shooter is active. + * + * The two main entry points are: + * adaptableAuton(...) — builds a routine that follows a single path + * multiAdaptableAuton(...) — builds a routine that chains multiple paths together + * + * AutonChooser.java is where the actual routines are defined (which paths to use, + * what timeouts, SOTM or not, etc.) and put on the dashboard for the driver to pick. + */ public class WaltAdaptableAutonFactory { - private final Superstructure m_superstructure; - public final AutoFactory m_autoFactory; - private final Intake m_intake; - private final Shooter m_shooter; - private final Swerve m_drivetrain; - - public Timer autonTimer = new Timer(); - // state logger - // private final DoubleLogger log_autonState = WaltLogger.logDouble(kLogTab, "State"); - - // waypoint constants - private final String kIntakeWaypoint = "intake"; - private final String kStopIntakeWaypoint = "stopIntake"; - private final String kShootWaypoint = "shoot"; - private final String kStopShootWaypoint = "stopShoot"; - private final String kIntakeAndShootWaypoint = "intakeAndShoot"; - private final String kStopIntakeAndShootWaypoint = "stopIntakeAndShoot"; - - // trajectory logger - private final StringLogger log_trajectoryName = new StringLogger(AutonK.kLogTab, "trajectoryName"); - private final Pose2dArrayLogger log_trajectoryPoses = new Pose2dArrayLogger(AutonK.kLogTab, "trajectoryPoses"); - private final BooleanLogger log_isAtStopShoot = new BooleanLogger(AutonK.kLogTab, "isAtStopShoot"); - private final BooleanLogger log_isAtStopIntake = new BooleanLogger(AutonK.kLogTab, "isAtStopIntake"); + // ---- SUBSYSTEMS ---- + // These are passed in through the constructor so we can tell them what to do + // without this class being responsible for creating them. + + private final Superstructure m_superstructure; // controls shooting + intaking at a high level + private final AutoFactory m_autoFactory; // Choreo: loads trajectory files and creates routines + private final Intake m_intake; // intake arm + rollers + private final Shooter m_shooter; // flywheels + shot calculator + private final Swerve m_drivetrain; // swerve drive (used to lock wheels at the end) + + // ---- WAYPOINT NAMES ---- + // These strings must EXACTLY match the event marker names you place in Choreo. + // If a marker is named "intake" in Choreo, it has to be "intake" here too. + + private static final String kIntakeWaypoint = "intake"; + private static final String kStopIntakeWaypoint = "stopIntake"; + private static final String kShootWaypoint = "shoot"; + private static final String kStopShootWaypoint = "stopShoot"; + private static final String kIntakeAndShootWaypoint = "intakeAndShoot"; + private static final String kStopIntakeAndShootWaypoint = "stopIntakeAndShoot"; + + // ---- loggers ---- + // WaltLogger pushes data to NetworkTables, which AdvantageScope can read + // and replay after a match so we can see exactly what happened and when. + + private final StringLogger log_trajectoryName = new StringLogger(AutonK.kLogTab, "trajectoryName"); + private final Pose2dArrayLogger log_trajectoryPoses = new Pose2dArrayLogger(AutonK.kLogTab, "trajectoryPoses"); + private final BooleanLogger log_isAtStopShoot = new BooleanLogger(AutonK.kLogTab, "isAtStopShoot"); + private final BooleanLogger log_isAtStopIntake = new BooleanLogger(AutonK.kLogTab, "isAtStopIntake"); private final DoubleLogger log_autonActionTimes = new DoubleLogger(AutonK.kLogTab, "autonActionTimes"); + private final StringLogger log_autonEventMarker = WaltLogger.logString("Auton", "autonTriggerCall"); - // logic booleans + // ---- state flags ---- + // These get flipped by trajectory event markers at runtime and are exposed + // as Triggers so we can use them as end conditions for running commands. + + // Toggled by the "stopShoot" marker. Acts as a manual override to end shooting + // early — useful when the robot has driven past the point where shooting makes sense + // but the ball sensor hasn't confirmed the shot yet. + // Uses onChange (fires on both the rising and falling edge of the marker window) + // so the flag automatically resets itself once the window closes. private boolean m_isAtStopShoot = false; + + // Goes true while the robot is inside the "stopIntake" marker window, then + // back to false when it leaves. The intake command runs until this goes true. private boolean m_isAtStopIntake = false; - private Trigger trg_isAtStopShoot = new Trigger(() -> m_isAtStopShoot); - private Trigger trg_isAtStopIntake = new Trigger(() -> m_isAtStopIntake); - public WaltAdaptableAutonFactory(Superstructure superstructure, AutoFactory autoFactory, Intake intake, Shooter shooter, Swerve swerve) { + private final Trigger trg_isAtStopShoot = new Trigger(() -> m_isAtStopShoot); + private final Trigger trg_isAtStopIntake = new Trigger(() -> m_isAtStopIntake); + + // ---- auton timer ---- + // Tracks time since auton started. Used to timestamp logged events so we + // know exactly when each action fired during a match. + + public Timer autonTimer = new Timer(); + + + // ============================================================= + // CONSTRUCTOR + // ============================================================= + + public WaltAdaptableAutonFactory( + Superstructure superstructure, + AutoFactory autoFactory, + Intake intake, + Shooter shooter, + Swerve swerve) { m_superstructure = superstructure; - m_autoFactory = autoFactory; - m_intake = intake; - m_shooter = shooter; - m_drivetrain = swerve; + m_autoFactory = autoFactory; + m_intake = intake; + m_shooter = shooter; + m_drivetrain = swerve; } - //---UTILITY METHODS - // private Command logState(double state) { - // return Commands.runOnce(() -> log_autonState.accept(state)); - // } - private Command tp(String message) { + // ============================================================= + // PRIVATE UTILITY METHODS + // ============================================================= + + // Short helper to print a timestamped message when a command runs. + // Used throughout to trace the auton flow in the console/logs. + private Command timedPrint(String message) { return WaltLogger.timedPrintCmd(message); } - private Command waitIntakeHomedCmd() { - return Commands.waitUntil(m_intake.intakeHomedSupp); + // Defers evaluating the string until the command actually *runs*, not when + // it's constructed. This matters for anything that reads a live value like + // a timer — without defer, the value would be captured at build time (t=0). + // Thank you grac + private static Command printLater(Supplier stringSup) { + return Commands.defer(() -> Commands.print(stringSup.get()), Set.of()); } + // Waits for the intake arm to find its home position (mechanical zero) + // before the auton relies on any intake movement. Times out after 5 s so + // a bad homing wont screw our path over private Command homingCmd() { - return Commands.sequence(tp("intakeArmHoming.START"), waitIntakeHomedCmd(), tp("intakeArmHoming.END")) - .withTimeout(5); + return Commands.sequence( + timedPrint("intakeArmHoming.START"), + Commands.waitUntil(m_intake.intakeHomedSupp), + timedPrint("intakeArmHoming.END") + ).withTimeout(5); } - public AutoRoutine preheater() { - System.out.println("PREHEAT MADE"); - return adaptableAuton("PreHeat", new AdaptableAutonInfo("PreHeat", AutonK.kShootingTimeout, false, 0)); + // Builds the command that fires when trajectory[i] finishes and trajectory[i+1] should start. + // + // SOTM mode → drive into the next path right away (optionally after a short delay). + // Shooting is already happening while driving, so no wait needed. + // + // Normal mode → wait for the shooter to confirm the ball has left the robot + // (or for the stopShoot override, or until the timeout expires), + // then optionally delay, then start the next path. + private Command buildTransitionCmd( + Command nextTrajCmd, + double nextDelay, + double shooterTimeout, + boolean shootOnTheMove) { + + // If there's a pre-path delay, prepend it to whatever we're about to start. + Command startNext = nextDelay > 0 + ? Commands.sequence(Commands.waitSeconds(nextDelay), nextTrajCmd) + : nextTrajCmd; + + if (shootOnTheMove) { + return startNext; // SOTM — just go, no waiting needed + } + + // Non-SOTM: race shot confirmation against the timeout so we never + // get stuck waiting if a ball gets stuck or the sensor misses. + Command waitForShot = Commands.race( + Commands.waitUntil(m_shooter.getBallShotDebounceTrg().or(trg_isAtStopShoot)), + Commands.waitSeconds(shooterTimeout) + ); + + return Commands.sequence( + timedPrint("WAITING FOR SHOOTING DONE"), + waitForShot, + startNext + ); } - /** - * Eagerly loads every given trajectory into the AutoFactory's TrajectoryCache. - * After this runs, {@code routine.trajectory(name)} during autonomousInit is a - * HashMap hit instead of a disk read + GSON parse. Call once during robotInit. - */ + // Logs a named waypoint event to NetworkTables so post-match replays in + // AdvantageScope show which markers fired and in what order. + private Command logEventMarker(String markerName) { + return Commands.runOnce(() -> log_autonEventMarker.accept(markerName)); + } + + + // ============================================================= + // PUBLIC UTILITIES + // ============================================================= + + // Eagerly parses and caches every trajectory during robotInit. + // Without this, the first time a trajectory is requested during auton it + // has to be read off the filesystem + parsed from JSON, which takes time. + // After preloading, all lookups are fast HashMap hits instead. + // Call this once during robotInit, before building any auton routines. public void preloadAllTrajectories(String[] names) { var cache = m_autoFactory.cache(); long totalStart = System.nanoTime(); + for (String name : names) { long ts = System.nanoTime(); cache.loadTrajectory(name); long elapsed = System.nanoTime() - ts; - if (elapsed > 5_000_000) { // only log if > 5ms + + // only print slow (> 5 ms) crine because rio cant handle too many prints + if (elapsed > 5_000_000) { System.out.printf("[PRELOAD] %s: %.1f ms%n", name, elapsed * 1e-6); } } + System.out.printf("[PRELOAD] %d trajectories total: %.1f ms%n", names.length, (System.nanoTime() - totalStart) * 1e-6); } - //thank you grac - private static Command printLater(Supplier stringSup) { - return Commands.defer(() -> { - return Commands.print(stringSup.get()); - }, Set.of()); - } - + // Logs an elapsed-time snapshot for a named event. + // Used to timestamp when specific things happened during auton for post-match analysis. public Command logTimer(String epochName, Supplier timerSup) { return printLater(() -> { var timer = timerSup.get(); log_autonActionTimes.accept(autonTimer.get()); return epochName + " at " + timer.get() + " s"; }); - } + } + // Start the auton-wide timer. Call this at the top of autonomousInit. public void startAutonTimer() { autonTimer.start(); } - //---AUTOROUTINE TRAJECTORY HELPER + + // ============================================================= + // TRAJECTORY BUILDER HELPER + // ============================================================= + + // Creates an AutoTrajectory and wires up start/end logging. + // Poses are pulled from the cache (populated by preloadAllTrajectories) + // so we're doing a fast lookup, not re-parsing the JSON file from disk. private AutoTrajectory createTraj(AutoRoutine routine, String name) { AutoTrajectory traj = routine.trajectory(name); - // Go through the factory's TrajectoryCache so this is a HashMap hit after preload, - // instead of a second disk read + GSON parse on top of routine.trajectory(name)'s load. var poses = m_autoFactory.cache().loadTrajectory(name).get().getPoses(); + traj.active().onTrue(Commands.sequence( Commands.runOnce(() -> { log_trajectoryName.accept(name); log_trajectoryPoses.accept(poses); }), - tp("traj.START(" + name + ")") + timedPrint("traj.START(" + name + ")") )); - traj.done().onTrue(tp("traj.END(" + name + ")")); + + traj.done().onTrue(timedPrint("traj.END(" + name + ")")); + return traj; } - //---ADAPTABLE AUTON MAKER + + // ============================================================= + // AUTON ROUTINE BUILDERS + // ============================================================= + + // Builds a "preheat" routine that runs while the robot is still disabled + // before a match. Ensures we dont stall for like 0.4 seconds at the start of auton + public AutoRoutine preheater() { + System.out.println("PREHEAT MADE"); + return adaptableAuton("PreHeat", new AdaptableAutonInfo("PreHeat", AutonK.kShootingTimeout, false, 0)); + } + + // Builds a single-segment auton: follow one trajectory and fire waypoint + // actions at the event markers embedded in it. + // The intake arm homing sequence runs alongside the path so homing doesn't + // cost us any auton time. public AutoRoutine adaptableAuton(String routineName, AdaptableAutonInfo autonInfo) { AutoRoutine routine = m_autoFactory.newRoutine(routineName); - - String path = autonInfo.autonName(); - AutoTrajectory traj = createTraj(routine, path); + AutoTrajectory traj = createTraj(routine, autonInfo.autonName()); routine.active().onTrue( traj.cmd().alongWith(homingCmd()) @@ -159,131 +294,116 @@ public AutoRoutine adaptableAuton(String routineName, AdaptableAutonInfo autonIn return routine; } + // Builds a multi-segment auton by chaining several trajectories end-to-end. + // How the transition between segments works depends on SOTM mode — see buildTransitionCmd. + // After the last segment finishes, the drivetrain locks into an X-brake so the robot + // doesn't slide. + /** + * NEW TERMINOLOGY: COAST OUT -- Coasting out means to let the motors keep their momentum when they stop, instead of + * simply going to zero (stopping abruptly). + */ + // Something to think about is to coast out the swerve at the end of auto, to cover more ground to get closer to + // fuel to pick up faster? + // EX: See Citrus's (1678) autos public AutoRoutine multiAdaptableAuton(String routineName, AdaptableAutonInfo[] autonInfos) { - System.out.println("================== adaptableBuilder Start =================="); - - AutoTrajectory[] autonTrajs = new AutoTrajectory[autonInfos.length]; AutoRoutine routine = m_autoFactory.newRoutine(routineName); + // Step 1: create all trajectory objects before wiring any triggers. + // This ensures every trajectory is in memory before anything tries to + // reference the next one. + AutoTrajectory[] autonTrajs = new AutoTrajectory[autonInfos.length]; for (int i = 0; i < autonInfos.length; i++) { - String path = autonInfos[i].autonName(); - autonTrajs[i] = createTraj(routine, path); + autonTrajs[i] = createTraj(routine, autonInfos[i].autonName()); } - System.out.println("trajs built"); - + // Step 2: attach waypoint triggers to every segment. for (int i = 0; i < autonTrajs.length; i++) { - AutoTrajectory thisTraj = autonTrajs[i]; - var thisInfo = autonInfos[i]; - System.out.println("traj idx " + i + " (" + thisInfo.autonName + ") .done().onTrue() built"); - - setUpTrajTriggers(thisTraj, thisInfo.shooterTimeout(), autonInfos[i].SOTM()); + setUpTrajTriggers(autonTrajs[i], autonInfos[i].shooterTimeout(), autonInfos[i].SOTM()); } - System.out.println("trajTriggers built"); - if (autonInfos[0].delay() != 0) { - routine.active().onTrue( - Commands.sequence( - Commands.waitSeconds(autonInfos[0].delay()), - autonTrajs[0].cmd().alongWith(homingCmd()) - ) - ); - } else { - routine.active().onTrue( - autonTrajs[0].cmd().alongWith(homingCmd()) - ); - } - System.out.println("routine active built"); + // Step 3: start the first trajectory when the routine becomes active. + // Home the intake arm in parallel so we don't waste time waiting for it. + Command firstPath = autonTrajs[0].cmd().alongWith(homingCmd()); + double firstDelay = autonInfos[0].delay(); + routine.active().onTrue( + firstDelay > 0 + ? Commands.sequence(Commands.waitSeconds(firstDelay), firstPath) + : firstPath + ); - System.out.println("traj idx onTrue pre-built"); + // Step 4: chain each segment into the next. + // For every segment except the last: "when this path ends, run the transition into the next one." for (int i = 0; i < autonTrajs.length - 1; i++) { - AutoTrajectory thisTraj = autonTrajs[i]; - var thisInfo = autonInfos[i]; - System.out.println("traj idx " + i + " (" + thisInfo.autonName + ") .done().onTrue() built"); - // thisTraj.active().whileTrue( - // Commands.run(() -> { - // var sample = thisTraj.getRawTrajectory().sampleAt(i, m_isAtStopIntake); - // })) - // ) - if (autonInfos[i + 1].delay() != 0) { - thisTraj.done().onTrue( - thisInfo.SOTM ? Commands.sequence( - Commands.waitSeconds(autonInfos[i + 1].delay()), - autonTrajs[i + 1].cmd()) : - Commands.sequence( - tp("WAITING FOR SHOOTING DONE"), - Commands.race( - Commands.waitUntil(m_shooter.getBallShotDebounceTrg().or(trg_isAtStopShoot)), - Commands.waitSeconds(thisInfo.shooterTimeout()) - ), - tp("TRAJ " + (i + 1) + " DELAY STARTED"), - Commands.waitSeconds(autonInfos[i + 1].delay()), - tp("TRAJ " + (i + 1) + " STARTED"), - autonTrajs[i + 1].cmd() - )); - } else { - thisTraj.done().onTrue( - thisInfo.SOTM ? autonTrajs[i + 1].cmd() : - Commands.sequence( - tp("WAITING FOR SHOOTING DONE"), - Commands.race( - Commands.waitUntil(m_shooter.getBallShotDebounceTrg().or(trg_isAtStopShoot)), - Commands.waitSeconds(thisInfo.shooterTimeout()) - ), - tp("TRAJ " + (i + 1) + " STARTED"), - autonTrajs[i + 1].cmd() - )); - } + Command transition = buildTransitionCmd( + autonTrajs[i + 1].cmd(), + autonInfos[i + 1].delay(), + autonInfos[i].shooterTimeout(), + autonInfos[i].SOTM() + ); + autonTrajs[i].done().onTrue(transition); } + // Step 5: lock wheels once the final segment finishes. autonTrajs[autonTrajs.length - 1].done().onTrue( m_drivetrain.xBrakeCmd() ); - System.out.println("================== full routine built =================="); - return routine; } - private StringLogger log_autonEventMarker = WaltLogger.logString("Auton", "autonTriggerCall"); - - public Command updateAutonEventMarkerLogger(String update) { - return Commands.runOnce(() -> log_autonEventMarker.accept(update)); - } - /* CHECK IF TURRET IS ABLE TO SHOOT FOR PASSING PLEASEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */ - public void setUpTrajTriggers(AutoTrajectory traj, double shooterTimeout, boolean SOTM) { - //---TRIGGER ACTIONS + // ============================================================= + // TRAJECTORY TRIGGER SETUP + // ============================================================= + + // Registers all the waypoint-triggered commands for a single trajectory segment. + // + // Choreo fires a Trigger each time the path timer reaches a named event marker. + // That's how the robot knows *when* to intake, shoot, etc. based on where it is + // along the path — we're not polling position, just reacting to timestamps. + // + // Supported marker names (type these exactly in the Choreo editor): + // "intake" start intaking; runs until stopIntake fires + // "stopIntake" stop the active intake command + // "shoot" start shooting (behavior differs by SOTM flag, see below) + // "stopShoot" force-stop shooting early (override if ball sensor misses) + // "intakeAndShoot" run intake + shooter at the same time + // "stopIntakeAndShoot" stop the simultaneous intake+shoot + // + // shootOnTheMove (SOTM): + // false → robot waits for shot confirmation before moving on (timeout is the safety cutoff) + // true → robot keeps driving while shooting; shooting ends when ball exits or stopShoot fires + public void setUpTrajTriggers(AutoTrajectory traj, double shooterTimeout, boolean shootOnTheMove) { + + // "intake" — start intaking, stop when the stopIntake marker fires traj.atTime(kIntakeWaypoint).onTrue( m_superstructure.intake(() -> false, () -> false).until(trg_isAtStopIntake) ); - // traj.atTime(kIntakeWaypoint).onTrue( - // logTimer("intaking", () -> autonTimer) - // ); - - traj.atTime(kShootWaypoint).and(() -> !SOTM).onTrue( - // stopShoot acts as an override STOP SHOOTING to continue the pathing whereas the debounceTrg can help us move on faster if we're already outta balls + // "shoot" (non-SOTM) — race: shoot until ball exits OR timeout expires. + // The timeout is a safety cutoff so auton never stalls if a ball gets stuck. + traj.atTime(kShootWaypoint).and(() -> !shootOnTheMove).onTrue( Commands.race( m_superstructure.activateOuttakeShotCalc().until(m_shooter.getBallShotDebounceTrg()), Commands.waitSeconds(shooterTimeout) ) - // (m_superstructure.activateOuttakeShotCalc().until(m_shooter.getBallShotDebounceTrg())).withTimeout(shooterTimeout) ); - traj.atTime(kShootWaypoint).and(() -> SOTM).onTrue( - // stopShoot acts as an override STOP SHOOTING to continue the pathing whereas the debounceTrg can help us move on faster if we're already outta balls - m_superstructure.activateOuttakeShotCalc().until(m_shooter.getBallShotDebounceTrg().or(trg_isAtStopShoot)) + // "shoot" (SOTM) — robot keeps driving; shooting ends when ball exits or stopShoot fires + traj.atTime(kShootWaypoint).and(() -> shootOnTheMove).onTrue( + m_superstructure.activateOuttakeShotCalc() + .until(m_shooter.getBallShotDebounceTrg().or(trg_isAtStopShoot)) ); + // "shoot" (both modes) — shimmy the intake to help feed the ball into the shooter traj.atTime(kShootWaypoint).onTrue( m_superstructure.intakeShimmy(() -> true) ); - // traj.atTime(kShootWaypoint).onTrue( - // logTimer(kShootWaypoint, () -> autonTimer) - // ); - + // "stopShoot" — toggle the override flag on both the rising AND falling + // edge of the marker window (onChange fires twice per pass-through). + // This means the flag flips on as the robot enters the window, then + // automatically flips back off as it leaves — a self-resetting override. traj.atTime(kStopShootWaypoint).onChange( Commands.runOnce(() -> { m_isAtStopShoot = !m_isAtStopShoot; @@ -291,13 +411,14 @@ public void setUpTrajTriggers(AutoTrajectory traj, double shooterTimeout, boolea }) ); + // "stopIntake" — true while inside the marker window, false once outside. + // The intake command above uses trg_isAtStopIntake as its end condition. traj.atTime(kStopIntakeWaypoint).onTrue( Commands.runOnce(() -> { m_isAtStopIntake = true; log_isAtStopIntake.accept(m_isAtStopIntake); }) ); - traj.atTime(kStopIntakeWaypoint).onFalse( Commands.runOnce(() -> { m_isAtStopIntake = false; @@ -305,44 +426,50 @@ public void setUpTrajTriggers(AutoTrajectory traj, double shooterTimeout, boolea }) ); + // "intakeAndShoot" / "stopIntakeAndShoot" — run intake + shooter together + // until the stop marker fires. Used for bump/passing actions. traj.atTime(kIntakeAndShootWaypoint).onTrue( - m_superstructure.intake(() -> true, () -> false).until(traj.atTime(kStopIntakeAndShootWaypoint)) - ); - - traj.atTime(kIntakeAndShootWaypoint).onTrue( - m_superstructure.activateOuttakeShotCalc().until(traj.atTime(kStopIntakeAndShootWaypoint)) + m_superstructure.intake(() -> true, () -> false) + .until(traj.atTime(kStopIntakeAndShootWaypoint)) ); - - //---TRIGGER LOGGERS - traj.atTime(kIntakeWaypoint).onTrue( - updateAutonEventMarkerLogger(kIntakeWaypoint) - ); - traj.atTime(kIntakeAndShootWaypoint).onTrue( - updateAutonEventMarkerLogger(kIntakeAndShootWaypoint) + m_superstructure.activateOuttakeShotCalc() + .until(traj.atTime(kStopIntakeAndShootWaypoint)) ); - traj.atTime(kStopIntakeAndShootWaypoint).onTrue( - updateAutonEventMarkerLogger(kStopIntakeAndShootWaypoint) - ); - - traj.atTime(kStopIntakeWaypoint).onTrue( - updateAutonEventMarkerLogger(kStopIntakeWaypoint) - ); + // Log every marker event so post-match AdvantageScope replays show + // which events fired and in what order. + traj.atTime(kIntakeWaypoint).onTrue(logEventMarker(kIntakeWaypoint)); + traj.atTime(kStopIntakeWaypoint).onTrue(logEventMarker(kStopIntakeWaypoint)); + traj.atTime(kShootWaypoint).onTrue(logEventMarker(kShootWaypoint)); + traj.atTime(kStopShootWaypoint).onTrue(logEventMarker(kStopShootWaypoint)); + traj.atTime(kIntakeAndShootWaypoint).onTrue(logEventMarker(kIntakeAndShootWaypoint)); + traj.atTime(kStopIntakeAndShootWaypoint).onTrue(logEventMarker(kStopIntakeAndShootWaypoint)); + } - traj.atTime(kShootWaypoint).onTrue( - updateAutonEventMarkerLogger(kShootWaypoint) - ); - traj.atTime(kStopShootWaypoint).onTrue( - updateAutonEventMarkerLogger(kStopShootWaypoint) - ); - } - + // ============================================================= + // DATA RECORD + // ============================================================= + + /* + * AdaptableAutonInfo + * + * Holds all the settings for one trajectory segment of an auton routine. + * Think of it as the "config card" you hand to the factory for each path. + * + * autonName — file name of the Choreo trajectory (no .traj extension) + * shooterTimeout — max seconds to wait for shot confirmation before giving up + * and moving on (only matters in non-SOTM mode) + * SOTM — Shoot On The Move: true = shoot while driving, + * false = stop and wait for shot confirmation + * delay — seconds to wait before starting this segment (0 = start right away). + * Useful when following another robot or letting defense clear out. + */ public final record AdaptableAutonInfo( - String autonName, - double shooterTimeout, + String autonName, + double shooterTimeout, boolean SOTM, - double delay + double delay ) {} -}; \ No newline at end of file +} diff --git a/src/main/java/frc/robot/dashboards/AutonChooser.java b/src/main/java/frc/robot/dashboards/AutonChooser.java index cd381842..ffa73f40 100644 --- a/src/main/java/frc/robot/dashboards/AutonChooser.java +++ b/src/main/java/frc/robot/dashboards/AutonChooser.java @@ -5,7 +5,6 @@ import java.util.List; import choreo.auto.AutoChooser; -import edu.wpi.first.hal.simulation.AddressableLEDDataJNI; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -14,6 +13,29 @@ import frc.robot.autons.WaltAdaptableAutonFactory; import frc.robot.autons.WaltAdaptableAutonFactory.AdaptableAutonInfo; +/** + * AutonChooser + * + * This class is responsible for deciding which autons we want to run, + * and in what order do those paths go in. + * + * FORMATTING: + * We like to keep things the same around here so nobody can get confused + * on the field and pick the wrong auton ;). We follow a format with how + * these autons are named, and thats in the following format: + * STARTING POSITION: Left or Right, or maybe even center. + * Are we starting in the Trench, on + * the bump, hell maybe even on the hub?! + * Knowing this prior to the match starting allows for the people + * setting the robot on the field know where they need to align it to. + * + * NUMBER OF CYCLES: One cycle, Two Cycle, or even Three. + * + * RETURN POSITION: Are we coming back over the bump? Or under the trench? + * + * MODIFIERS: Things such as a follow auto, delayed auto, going to the depot, + * etc. + */ public class AutonChooser { private static final String kPreheatTrajectory = "PreHeat"; @@ -27,7 +49,14 @@ private record AutonEntry(String name, AdaptableAutonInfo infos) {} private record MultiAutonEntry(String name, AdaptableAutonInfo[] infos) {} private static final List s_multiAutons = new ArrayList<>(); - /* OLD AUTON NAMES */ + // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // AUTON NAMES + // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + + // ============================================================= + // OLD AUTON NAMES + // ============================================================= + //---1.5 CYCLES private final static String kLeftShootAndSweep = "LEFT Sweep 1.5 Cycle"; private final static String kRightShootAndSweep = "RIGHT Sweep 1.5 Cycle"; @@ -53,18 +82,21 @@ private record MultiAutonEntry(String name, AdaptableAutonInfo[] infos) {} private final static String kLeftSweepAndDepot = "LEFT Bump Sweep and Depot"; //---STRESS TEST - private final static String kRightStressTestLong = "RIGHT Long Stress Test"; - private final static String kRightStressTestOverlap = "RIGHT Overlap Stress Test"; - private final static String kRightStressTestTenTimes = "RIGHT Five Times Stress Test"; + // private final static String kRightStressTestLong = "RIGHT Long Stress Test"; + // private final static String kRightStressTestOverlap = "RIGHT Overlap Stress Test"; + // private final static String kRightStressTestTenTimes = "RIGHT Five Times Stress Test"; + + // ============================================================= + // NEW AUTON NAMES + // ============================================================= - /* NEW AUTON NAMES */ //---2 CYCLES private final static String kRightTrenchTwoCycleBumpReturn = "RIGHT Trench 2 Cycle Bump Return"; private final static String kRightTrenchTwoCycleBumpReturnFollow = "RIGHT Trench 2 Cycle Bump Return FOLLOW"; private final static String kLeftTrenchTwoCycleBumpReturn = "LEFT Trench 2 Cycle Bump Return"; private final static String kLeftTrenchTwoCycleBumpReturnFollow = "LEFT Trench 2 Cycle Bump Return FOLLOW"; private final static String kRightTrechTwoCycleTrenchReturn = "RIGHT Trench 2 Cycle Trench Return"; - private final static String kRightTrechTwoCycleTrenchReturnDelay = "RIGHT Trench 2 Cycle Trench Return 5sec DELAY"; + private final static String kRightTrenchTwoCycleTrenchReturnDelay = "RIGHT Trench 2 Cycle Trench Return 5sec DELAY"; private final static String kLeftTrenchTwoCycleTrenchReturn = "LEFT Trench 2 Cycle Trench Return"; private final static String kRightBumpTwoCycleReverseToTrenchPlusTrenchOnly = "RIGHT Bump 2 Cycle Reverse to Trench + Trench ONLY"; private final static String kLeftBumpTwoCycleReverseToTrenchPlusTrenchOnly = "LEFT Bump 2 Cycle Reverse to Trench + Trench ONLY"; @@ -83,13 +115,20 @@ private record MultiAutonEntry(String name, AdaptableAutonInfo[] infos) {} private final static String kCenterPreload = "CENTER Preload"; // private final static String kRightDelayTest = "DELAY TEST - NOT FOR ACTUAL USE"; + // ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // ============================================================================ + // -- UTILITY METHODS + // ============================================================================ public static void initialize(WaltAdaptableAutonFactory adaptableAutonFactory) { m_adaptableAutonFactory = adaptableAutonFactory; m_chooser = new AutoChooser(); s_multiAutons.clear(); - /* OLD AUTON OPTIONS */ + // ============================================================= + // OLD AUTON OPTIONS + // old being relative -- from when we were still bump bot + // ============================================================= //---MAIN AUTONS addMultiAuton(kLeftShootAndSweep, new AdaptableAutonInfo(AutonK.kLeftOneJab, AutonK.kSweepShootingTimeout, false, 0), @@ -165,7 +204,9 @@ public static void initialize(WaltAdaptableAutonFactory adaptableAutonFactory) { new AdaptableAutonInfo(AutonK.kRightOneReverse, AutonK.kShootingTimeout, false, 0), new AdaptableAutonInfo(AutonK.kRightTwoJab, AutonK.kShootingTimeout, false, 0)); - /* NEW AUTON OPTIONS */ + // ============================================================= + // NEW AUTON OPTIONS + // ============================================================= //---2 CYCLES addMultiAuton(kRightTrenchTwoCycleBumpReturn, new AdaptableAutonInfo(AutonK.kRightOneBumpReturn, AutonK.kSOTMTimeout, true, 0), @@ -200,7 +241,7 @@ public static void initialize(WaltAdaptableAutonFactory adaptableAutonFactory) { new AdaptableAutonInfo(AutonK.kRightTwoTrenchReturn, AutonK.kShootingTimeout, false, 0), new AdaptableAutonInfo(AutonK.kRightTwoGoOut, AutonK.kSOTMTimeout, false, 0)); - addMultiAuton(kRightTrechTwoCycleTrenchReturnDelay, + addMultiAuton(kRightTrenchTwoCycleTrenchReturnDelay, new AdaptableAutonInfo(AutonK.kRightOneTrenchReturn, AutonK.kShootingTimeout, false, 5), new AdaptableAutonInfo(AutonK.kRightTwoTrenchReturn, AutonK.kShootingTimeout, false, 0), new AdaptableAutonInfo(AutonK.kRightTwoGoOut, AutonK.kSOTMTimeout, false, 0)); diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index b47832f0..2f25b745 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -7,7 +7,6 @@ import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; -import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj2.command.Command; @@ -27,101 +26,126 @@ import frc.util.WaltLogger.BooleanLogger; import frc.util.WaltLogger.DoubleLogger; +/* + * Indexer + * + * The indexer sits between the intake and the shooter and is responsible for + * moving balls from the hopper into the shooter at the right speed. + * + * It has two motors: + * Spindexer — a spinning disk/floor underneath the ball hopper. It spins + * to rotate balls around and push them toward the tunnel entrance. + * Tunnel — a belt or tube that carries balls linearly from the hopper + * toward the shooter flywheel. It needs to spin fast enough to + * match the ball's exit speed. + * + * The tunnel speed is linked to the shooter flywheel speed via a geometric ratio + * derived from the physical radii of each wheel (see tunnelRPSFromShooter). + * This prevents the ball from being jerked or slowed as it enters the shooter. + */ public class Indexer extends SubsystemBase { - /* CLASS VARIABLES */ - //---MOTORS + CONTROL REQUESTS - private final TalonFX m_spindexer = new TalonFX(kSpindexerCANID, Constants.kCanivoreBus); // X60Foc - private final TalonFX m_tunnel = new TalonFX(kTunnelCANID, Constants.kCanivoreBus); // X60Foc + // ---- motors ---- + + // Both motors are Kraken X60s on the CANivore bus. + // X60Foc = Kraken X60 with Field-Oriented Control enabled (more torque at high speeds). + private final TalonFX m_spindexer = new TalonFX(kSpindexerCANID, Constants.kCanivoreBus); + private final TalonFX m_tunnel = new TalonFX(kTunnelCANID, Constants.kCanivoreBus); + + // ---- control requests ---- + // VelocityVoltage tells the motor controller to hold a specific speed (in RPS) + // using a PID loop + feedforward. We pre-create these and mutate them with + // .withVelocity() to avoid unnecessary object allocation on the hot path. + + // Spindexer uses non-FOC velocity (simpler, lower torque demands). private final VelocityVoltage m_spindexerVelocityRequest = new VelocityVoltage(0).withEnableFOC(false); + // Tunnel uses FOC velocity for smoother, more precise speed matching with the shooter. private final VelocityVoltage m_tunnelVelocityRequest = new VelocityVoltage(0).withEnableFOC(true); - private static final WaltTunable kTunnelRPSOverride = new WaltTunable("/Indexer/Tunnel/tunnelRPSOverride", kTunnelShootRPSD); - private static final WaltTunable kSpindexerRPSOverride = new WaltTunable("/Indexer/Spindexer/spindexerRPSOverride", kSpindexerShootRPSD); - + // CoastOut lets the motor spin freely to a stop instead of actively braking. + // We use this when stopping so the ball's momentum doesn't fight the motor. private final CoastOut m_spindexerMotorIdleReq = new CoastOut(); private final CoastOut m_tunnelMotorIdleReq = new CoastOut(); - /* SIM OBJECTS */ - // private final DCMotorSim m_spindexerSim = new DCMotorSim( - // LinearSystemId.createDCMotorSystem( - // DCMotor.getKrakenX60Foc(1), - // kSpindexerMOI, - // kSpindexerGearing - // ), - // DCMotor.getKrakenX60Foc(1) - // ); - - // private final DCMotorSim m_tunnelSim = new DCMotorSim( - // LinearSystemId.createDCMotorSystem( - // DCMotor.getKrakenX60Foc(1), - // kTunnelMOI, - // kTunnelGearing - // ), - // DCMotor.getKrakenX60Foc(1) - // ); - - - /* TUNABLES */ - private static final WaltTunable kTunnelRatioMultiplier = new WaltTunable("/Indexer/Tunnel/tunnelRatioScalar", 1.0); - private static final WaltTunable kSpindexerRatioMultiplier = new WaltTunable("/Indexer/Spindexer/spindexerRatioScalar", 1.0); - - /* LOGGERS */ + // ---- tunables ---- + // WaltTunable values can be adjusted at runtime through NetworkTables (no redeploy needed). + // If a tunable has been set, getOr() returns the tuned value; otherwise it returns the default. + // Useful during practice to dial in speeds without redeploying. + + private static final WaltTunable kTunnelRPSOverride = new WaltTunable("/Indexer/Tunnel/tunnelRPSOverride", kTunnelShootRPSD); + private static final WaltTunable kSpindexerRPSOverride = new WaltTunable("/Indexer/Spindexer/spindexerRPSOverride", kSpindexerShootRPSD); + private static final WaltTunable kTunnelRatioScalarTuner = new WaltTunable("/Indexer/Tunnel/tunnelRatioScalar", 1.0); + private static final WaltTunable kSpindexerRatioScalarTuner = new WaltTunable("/Indexer/Spindexer/spindexerRatioScalar", 1.0); + + // ---- loggers ---- + private final String kTunnelLogTab = "/Tunnel"; private final String kSpindexerLogTab = "/Spindexer"; private final DoubleLogger log_spindexerRPS = WaltLogger.logDouble(kLogTab + kSpindexerLogTab, "spindexerRPS"); - private final DoubleLogger log_tunnelRPS = WaltLogger.logDouble(kLogTab + kTunnelLogTab, "tunnelRPS"); - + private final DoubleLogger log_tunnelRPS = WaltLogger.logDouble(kLogTab + kTunnelLogTab, "tunnelRPS"); private final DoubleLogger log_desiredSpindexerRPS = WaltLogger.logDouble(kLogTab + kSpindexerLogTab, "desiredRPS"); - private final DoubleLogger log_desiredTunnelRPS = WaltLogger.logDouble(kLogTab + kTunnelLogTab, "desiredRPS"); + private final DoubleLogger log_desiredTunnelRPS = WaltLogger.logDouble(kLogTab + kTunnelLogTab, "desiredRPS"); private final DoubleLogger log_spindexerStatorCurrent = WaltLogger.logDouble(kLogTab + kSpindexerLogTab, "statorCurrent"); private final DoubleLogger log_spindexerSupplyCurrent = WaltLogger.logDouble(kLogTab + kSpindexerLogTab, "supplyCurrent"); + private final DoubleLogger log_tunnelClosedLoopError = WaltLogger.logDouble(kLogTab + kTunnelLogTab, "closedLoopError"); + private final BooleanLogger log_isTunnelSpunUp = WaltLogger.logBoolean(kLogTab + kTunnelLogTab, "spunUp"); + + // ---- status signals ---- + // StatusSignals are CTRE's way of efficiently reading motor data without spamming the CAN bus. + // We register them with SignalManager so they get refreshed at a consistent rate each loop. private final StatusSignal sig_spindexerVelo = m_spindexer.getVelocity(); private final StatusSignal sig_spindexerStatorCurrent = m_spindexer.getStatorCurrent(); - private final StatusSignal sig_spindexerSupplyCurrent = m_spindexer.getSupplyCurrent();; + private final StatusSignal sig_spindexerSupplyCurrent = m_spindexer.getSupplyCurrent(); private final StatusSignal sig_tunnelVelo = m_tunnel.getVelocity(); private final StatusSignal sig_tunnelCLErr = m_tunnel.getClosedLoopError(); - private final DoubleLogger log_tunnelClosedLoopError = WaltLogger.logDouble(kLogTab + kTunnelLogTab, "closedLoopError"); - private final BooleanLogger log_isTunnelSpunUp = WaltLogger.logBoolean(kLogTab + kTunnelLogTab, "spunUp"); + // ---- state ---- private boolean m_isTunnelSpunUp = false; private double m_tunnelVelocityRotPerSec = 0.0; private double m_desiredTunnelRPS = 0.0; private double m_desiredSpindexerRPS = 0.0; - /* CONSTRUCTOR */ + + // ============================================================= + // CONSTRUCTOR + // ============================================================= + public Indexer() { + // Apply the motor configurations defined in Constants.IndexerK. m_spindexer.getConfigurator().apply(kSpindexerTalonFXConfiguration); m_tunnel.getConfigurator().apply(kTunnelTalonFXConfiguration); - SignalManager.register(Constants.kCanivoreBus, sig_spindexerVelo, sig_tunnelVelo, sig_tunnelCLErr, sig_spindexerStatorCurrent, sig_spindexerSupplyCurrent); + // Register all status signals so they're refreshed every loop at the correct rate. + SignalManager.register(Constants.kCanivoreBus, + sig_spindexerVelo, sig_tunnelVelo, sig_tunnelCLErr, + sig_spindexerStatorCurrent, sig_spindexerSupplyCurrent); initSim(); } - //TODO: Change orientation if necessary + // Sets up the simulation models for both motors. + // TODO: Change orientation if mechanical setup changes. private void initSim() { WaltMotorSim.initSimFX(m_spindexer, ChassisReference.CounterClockwise_Positive, MotorType.KrakenX60); WaltMotorSim.initSimFX(m_tunnel, ChassisReference.CounterClockwise_Positive, MotorType.KrakenX60); } - /* COMMANDS */ - //---STARTS AND STOPS + + // ============================================================= + // COMMANDS — START / STOP + // ============================================================= + + // Starts both the tunnel and spindexer at their shoot speeds. public Command startIndexerCmd() { - return Commands.sequence( - startTunnelCmd(), - startSpindexerCmd() - ); + return Commands.sequence(startTunnelCmd(), startSpindexerCmd()); } + // Stops both motors by commanding 0 RPS (which triggers CoastOut in the setters). public Command stopIndexerCmd() { - return Commands.sequence( - stopTunnelCmd(), - stopSpindexerCmd() - ); + return Commands.sequence(stopTunnelCmd(), stopSpindexerCmd()); } public Command startSpindexerCmd() { @@ -136,11 +160,6 @@ public void stopSpindexer() { setSpindexerVelocity(0); } - public void setIndexerFromShooterRPS(DoubleSupplier shooterRPS) { - setTunnelVelocity(tunnelRPSFromShooter(shooterRPS).getAsDouble()); - setSpindexerVelocity(spindexerRPSFromShooter(shooterRPS).getAsDouble()); - } - public Command startTunnelCmd() { return setTunnelVelocityCmd(kTunnelShootRPS); } @@ -153,42 +172,27 @@ public void stopTunnel() { setTunnelVelocity(0); } - private void refreshTunnelState() { - m_tunnelVelocityRotPerSec = sig_tunnelVelo.getValueAsDouble(); - log_tunnelRPS.accept(m_tunnelVelocityRotPerSec); - - log_tunnelClosedLoopError.accept(sig_tunnelCLErr.getValueAsDouble()); - m_isTunnelSpunUp = sig_tunnelCLErr.isNear(0, 3); - if (m_desiredTunnelRPS > 80) { - m_isTunnelSpunUp = sig_tunnelCLErr.isNear(0, 6); - } - - log_isTunnelSpunUp.accept(m_isTunnelSpunUp); - } - - public boolean isTunnelSpunUp() { - return m_isTunnelSpunUp; - } - - public double getTunnelVelocityRotPerSec() { - return m_tunnelVelocityRotPerSec; + // Sets both indexer motors to speeds derived from the shooter's current RPS. + // This keeps the indexer surface speed matched to the flywheel so the ball + // doesn't get jerked when it enters the shooter. + public void setIndexerFromShooterRPS(DoubleSupplier shooterRPS) { + setTunnelVelocity(tunnelRPSFromShooter(shooterRPS).getAsDouble()); + setSpindexerVelocity(spindexerRPSFromShooter(shooterRPS).getAsDouble()); } - public double getDesiredTunnelVelocityRPS() { - return m_desiredTunnelRPS; - } - public double getDesiredSpindexerVelocityRPS() { - return m_desiredSpindexerRPS; - } + // ============================================================= + // SPINDEXER CONTROL + // ============================================================= - //---SPINDEXER + // When stopping (RPS == 0) we switch to CoastOut so the spindexer spins + // down freely public void setSpindexerVelocity(double RPS) { if (RPS == 0) { m_spindexer.setControl(m_spindexerVelocityRequest.withVelocity(0)); m_spindexer.setControl(m_spindexerMotorIdleReq); } else { - RPS = kSpindexerRPSOverride.enabled() ? kSpindexerRPSOverride.get() : RPS; + RPS = kSpindexerRPSOverride.getOr(RPS); // override if tuned at runtime m_spindexer.setControl(m_spindexerVelocityRequest.withVelocity(RPS)); } m_desiredSpindexerRPS = RPS; @@ -199,13 +203,18 @@ public Command setSpindexerVelocityCmd(AngularVelocity RPS) { return runOnce(() -> setSpindexerVelocity(RPS.in(RotationsPerSecond))); } - //---TUNNEL + + // ============================================================= + // TUNNEL CONTROL + // ============================================================= + + // Same coast-on-stop pattern as the spindexer. public void setTunnelVelocity(double RPS) { if (RPS == 0) { m_tunnel.setControl(m_tunnelVelocityRequest.withVelocity(0)); m_tunnel.setControl(m_tunnelMotorIdleReq); } else { - RPS = kTunnelRPSOverride.enabled() ? kTunnelRPSOverride.get() : RPS; + RPS = kTunnelRPSOverride.getOr(RPS); // override if tuned at runtime m_tunnel.setControl(m_tunnelVelocityRequest.withVelocity(RPS)); } m_desiredTunnelRPS = RPS; @@ -216,16 +225,65 @@ public Command setTunnelVelocityCmd(AngularVelocity RPS) { return runOnce(() -> setTunnelVelocity(RPS.in(RotationsPerSecond))); } - //STATICS for conversions + // Polls the tunnel's speed and closed-loop error, then decides if it's "spun up." + // The closed-loop error is how far off the tunnel is from its target speed (in RPS). + // At very high speeds (> 80 RPS), we allow a larger error tolerance since the + // motor is working harder and small deviations matter less. + private void refreshTunnelState() { + m_tunnelVelocityRotPerSec = sig_tunnelVelo.getValueAsDouble(); + log_tunnelRPS.accept(m_tunnelVelocityRotPerSec); + log_tunnelClosedLoopError.accept(sig_tunnelCLErr.getValueAsDouble()); + + m_isTunnelSpunUp = sig_tunnelCLErr.isNear(0, 3); // within 3 RPS of target + if (m_desiredTunnelRPS > 80) { + m_isTunnelSpunUp = sig_tunnelCLErr.isNear(0, 6); // looser tolerance at high speed + } + + log_isTunnelSpunUp.accept(m_isTunnelSpunUp); + } + + public boolean isTunnelSpunUp() { return m_isTunnelSpunUp; } + public double getTunnelVelocityRotPerSec() { return m_tunnelVelocityRotPerSec; } + public double getDesiredTunnelVelocityRPS() { return m_desiredTunnelRPS; } + public double getDesiredSpindexerVelocityRPS() { return m_desiredSpindexerRPS; } + + + // ============================================================= + // SPEED RATIO CONVERSIONS + // ============================================================= + + // The tunnel and spindexer speeds are derived from the shooter flywheel speed + // using the physical radii of each wheel/pulley. The goal is to match surface + // speeds so the ball doesn't get grabbed or slowed when it transitions between + // the indexer and the shooter. + // + // Surface speed = radius × angular velocity + // For the ball to travel smoothly: r_shooter × ω_shooter = r_indexer × ω_indexer + // → ω_indexer = (r_shooter / r_indexer) × ω_shooter + // + // The ratio constants (kTunnelFromShooterRatio, kSpindexerFromShooterRatio) are + // pre-computed in Constants.IndexerK from the physical radii. + // kTunnelRatioScalarTuner allows fine-tuning that ratio at runtime. + public static DoubleSupplier tunnelRPSFromShooter(DoubleSupplier shooterRPS) { - return () -> Math.min(shooterRPS.getAsDouble() * kTunnelFromShooterRatio * (kTunnelRatioMultiplier.enabled() ? kTunnelRatioMultiplier.get() : 1.0), kTunnelMaxRPSD); + return () -> Math.min( + shooterRPS.getAsDouble() * kTunnelFromShooterRatio * kTunnelRatioScalarTuner.getOr(1.0), + kTunnelMaxRPSD + ); } public static DoubleSupplier spindexerRPSFromShooter(DoubleSupplier shooterRPS) { - return () -> Math.min(shooterRPS.getAsDouble() * kSpindexerFromShooterRatio * (kSpindexerRatioMultiplier.enabled() ? kSpindexerRatioMultiplier.get() : 1.0), kSpindexerMaxRPSD); + return () -> Math.min( + shooterRPS.getAsDouble() * kSpindexerFromShooterRatio * kSpindexerRatioScalarTuner.getOr(1.0), + kSpindexerMaxRPSD + ); } - /* PERIODICS */ + + // ============================================================= + // PERIODIC + // ============================================================= + @Override public void periodic() { log_spindexerRPS.accept(sig_spindexerVelo.getValueAsDouble()); @@ -233,10 +291,4 @@ public void periodic() { log_spindexerSupplyCurrent.accept(sig_spindexerSupplyCurrent.getValueAsDouble()); refreshTunnelState(); } - - // @Override - // public void simulationPeriodic() { - // WaltMotorSim.updateSimFX(m_tunnel, m_tunnelSim); - // WaltMotorSim.updateSimFX(m_spindexer, m_spindexerSim); - // } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index f18a6e34..7e1549df 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -19,20 +20,15 @@ import java.util.function.BooleanSupplier; import java.util.function.Consumer; -import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; -import com.ctre.phoenix6.controls.VoltageOut; - import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.util.WaltLogger.BooleanLogger; @@ -42,79 +38,126 @@ import frc.util.SignalManager; import frc.util.WaltLogger; +/* + * Intake + * + * The intake is a ground-level collector that picks up fuel balls during a match. + * It has two parts that work together: + * + * Arm — A pivoting arm that swings down so the rollers can reach balls on + * the ground. It's controlled by a single motor using MotionMagic + * (CTRE's trapezoidal motion profiling) so it moves smoothly to each + * target position without slamming. + * + * Rollers — Two motors that spin to pull the ball in and pass it to the indexer. + * Motor B mirrors motor A (via Follower) but runs in the opposite + * direction so both rollers push the ball the same way. + * + * At the start of every match/auto, the arm needs to home itself — it doesn't + * have an absolute encoder, so it slowly drives into a mechanical hard stop, + * detects the stall via current + velocity sensing, and zeros the encoder there. + * See intakeArmCurrentSenseHoming(). + */ public class Intake extends SubsystemBase { - /* CLASS VARIABLES */ - //---MOTORS + CONTROL REQUESTS - private final TalonFX m_intakeArm = new TalonFX(kIntakeArmCANID); //x60Foc - private final TalonFX m_intakeRollersA = new TalonFX(kIntakeRollersA_CANID); //x60Foc - private final TalonFX m_intakeRollersB = new TalonFX(kIntakeRollersB_CANID); //x60Foc + // ---- motors ---- + + private final TalonFX m_intakeArm = new TalonFX(kIntakeArmCANID); // arm pivot (KrakenX44Foc on Rio bus) + private final TalonFX m_intakeRollersA = new TalonFX(kIntakeRollersA_CANID); // primary roller (KrakenX60Foc) + private final TalonFX m_intakeRollersB = new TalonFX(kIntakeRollersB_CANID); // follower roller (KrakenX60Foc) + + // ---- control requests ---- + // Pre-created and reused every loop to avoid unnecessary object allocation. + // MotionMagicVoltage: moves the arm smoothly to a target position. + // MotionMagic generates a trapezoidal velocity profile internally (accelerate, cruise, decelerate) + // so the arm doesn't slam into positions and cause mechanical stress or brownouts. private MotionMagicVoltage m_MMVReq = new MotionMagicVoltage(0).withEnableFOC(true); + + // VelocityVoltage: holds the rollers at a specific speed (RPS) using closed-loop control. private VelocityVoltage m_VelVoltReq = new VelocityVoltage(0).withEnableFOC(true); + + // VoltageOut: applies raw voltage directly to the arm motor. Used only during homing, + // where we intentionally drive into the hard stop at a low, safe voltage. private VoltageOut m_voltsReq = new VoltageOut(0).withEnableFOC(true); + // A second VoltageOut specifically for zeroing — kept separate from m_voltsReq so + // periodic logging (which reads m_voltsReq.Output) isn't contaminated by homing values. + private VoltageOut m_intakeArmZeroingReq = new VoltageOut(0); + + // ---- status signals ---- + private final StatusSignal sig_intakeArmStatorCurrent = m_intakeArm.getStatorCurrent(); private final StatusSignal sig_intakeArmVelo = m_intakeArm.getVelocity(); private final StatusSignal sig_intakeRollersAVelo = m_intakeRollersA.getVelocity(); private final StatusSignal sig_intakeArmPos = m_intakeArm.getPosition(); private final StatusSignal sig_intakeArmMMAtTarget = m_intakeArm.getMotionMagicAtTarget(); + // ---- homing detection helpers ---- + // These two conditions together confirm the arm has hit the mechanical hard stop: + // 1. Stator current spikes (motor is stalled / fighting the hard stop) + // 2. Arm velocity is near zero (it's not moving anymore) + private BooleanSupplier m_currentSpike = () -> sig_intakeArmStatorCurrent.getValueAsDouble() > 5.0; private BooleanSupplier m_veloIsNearZero = () -> Math.abs(sig_intakeArmVelo.getValueAsDouble()) < 0.005; + // Looser velocity threshold used during the shimmy motion, which has intentional slow movement. private BooleanSupplier m_shimmyVeloIsNearZero = () -> Math.abs(sig_intakeArmVelo.getValueAsDouble()) < 0.05; - - private VoltageOut m_intakeArmZeroingReq = new VoltageOut(0); - private Debouncer m_currentDebouncer = new Debouncer(0.100, DebounceType.kRising); - private Debouncer m_velocityDebouncer = new Debouncer(0.125, DebounceType.kRising); + // Debouncers filter out momentary spikes that don't represent a true stall. + // kRising means the output only goes true after the input has been true for the set duration. + private Debouncer m_currentDebouncer = new Debouncer(0.100, DebounceType.kRising); // 100 ms + private Debouncer m_velocityDebouncer = new Debouncer(0.125, DebounceType.kRising); // 125 ms private boolean m_isIntakeArmHomed = false; + // Public supplier so other classes (e.g. WaltAdaptableAutonFactory) can wait on homing. public final BooleanSupplier intakeHomedSupp = () -> m_isIntakeArmHomed; - /* SIM OBJECTS */ + // ---- simulation models ---- + // WPILib DCMotorSim models approximate the motor's physics during simulation. + // These use the physical MOI and gear ratios from Constants so sim behavior + // roughly matches the real robot. + private final DCMotorSim m_intakeArmSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(1), - kIntakeArmMOI, - kIntakeArmGearing - ), + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60Foc(1), kIntakeArmMOI, kIntakeArmGearing), DCMotor.getKrakenX60Foc(1) ); private final DCMotorSim m_intakeRollersSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(2), - kIntakeRollersMOI, - kIntakeRollersGearing - ), - DCMotor.getKrakenX60Foc(2) // returns gearbox + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60Foc(2), kIntakeRollersMOI, kIntakeRollersGearing), + DCMotor.getKrakenX60Foc(2) ); - /* LOGGERS */ + // ---- loggers ---- + private final DoubleLogger log_intakeArmRots = WaltLogger.logDouble(kLogTab, "intakeArmRots"); private final DoubleLogger log_targetIntakeArmRots = WaltLogger.logDouble(kLogTab, "targetIntakeArmRots"); - private final DoubleLogger log_intakeRollersRPS = WaltLogger.logDouble(kLogTab, "intakeRollersRPS"); private final DoubleLogger log_targetIntakeRollersRPS = WaltLogger.logDouble(kLogTab, "targetIntakeRollersRPS"); - private final BooleanLogger log_isIntakeArmHomed = WaltLogger.logBoolean(kLogTab, "isIntakeArmHomed"); - /* CONSTRUCTOR */ + + // ============================================================= + // CONSTRUCTOR + // ============================================================= + public Intake() { m_intakeArm.getConfigurator().apply(kIntakeArmConfiguration); - m_intakeRollersA.getConfigurator().apply(kIntakeRollersAConfiguration); m_intakeRollersB.getConfigurator().apply(kIntakeRollersBConfiguration); + // Motor B mirrors motor A but spins in the opposite direction so both rollers + // push the ball the same way (they're physically mirrored on the robot). m_intakeRollersB.setControl(new Follower(kIntakeRollersA_CANID, MotorAlignmentValue.Opposed)); - SignalManager.register(kRioBus, sig_intakeArmStatorCurrent, sig_intakeArmVelo, sig_intakeRollersAVelo, sig_intakeArmPos, sig_intakeArmMMAtTarget); + SignalManager.register(kRioBus, + sig_intakeArmStatorCurrent, sig_intakeArmVelo, + sig_intakeRollersAVelo, sig_intakeArmPos, sig_intakeArmMMAtTarget); + // On the real robot, start homing immediately on startup. + // In sim, homing via current sensing doesn't work so we skip it. if (Robot.isReal()) { setDefaultCommand(intakeArmCurrentSenseHoming()); - // setDefaultCommand(intakeArmHome()); } initSim(); @@ -125,13 +168,19 @@ private void initSim() { WaltMotorSim.initSimFX(m_intakeRollersA, ChassisReference.CounterClockwise_Positive, TalonFXSimState.MotorType.KrakenX60); } - /* COMMANDS */ + + // ============================================================= + // ARM POSITION CONTROL + // ============================================================= + + // These set the arm to a named position from the IntakeArmPosition enum. + // Uses MotionMagic for smooth, profiled movement. public void setIntakeArmPos(IntakeArmPosition rots) { - setIntakeArmPos(rots.rots); // rots == IntakeArmPosition.RETRACTED ? 36 : 18 + setIntakeArmPos(rots.rots); } public Command setIntakeArmPosCmd(IntakeArmPosition rots) { - return setIntakeArmPosCmd(rots.rots); // rots == IntakeArmPosition.RETRACTED ? 36 : 18 + return setIntakeArmPosCmd(rots.rots); } public Command setIntakeArmPosCmd(Angle rots) { @@ -142,49 +191,70 @@ public void setIntakeArmPos(Angle rots) { m_intakeArm.setControl(m_MMVReq.withPosition(rots)); } + // True when the arm has stopped moving (velocity near zero), used as a + // "are we there yet" check during shimmy movements. public boolean isIntakeArmAtDest() { - return m_shimmyVeloIsNearZero.getAsBoolean(); + return m_shimmyVeloIsNearZero.getAsBoolean(); } - public void setIntakeArmNeutralMode(NeutralModeValue value) { + public void setIntakeArmNeutralMode(NeutralModeValue value) { m_intakeArm.setNeutralMode(value); } - public Command startIntakeRollers(double volts) { - return setIntakeRollersVelocityCmd(volts); - } - public Command stopIntakeRollers() { - return setIntakeRollersVelocityCmd(0); - } + // ============================================================= + // ROLLER CONTROL + // ============================================================= + // NOTE: despite the parameter name being "volts", this method actually does + // velocity control. The volts value is treated as a 0-12V percentage and + // mapped to a target RPS (0 V = 0 RPS, 12 V = max RPS). This gives a + // convenient voltage-like API while still benefiting from closed-loop control. public void setIntakeRollersVelocity(double volts) { - m_intakeRollersA.setControl(m_VelVoltReq.withVelocity(volts / 12 * kIntakeRollersMaxRPS.in(RotationsPerSecond))); ///kV = 0.488599348534 - // m_intakeRollersA.setControl(m_voltsReq.withOutput(volts)); + m_intakeRollersA.setControl(m_VelVoltReq.withVelocity(volts / 12 * kIntakeRollersMaxRPS.in(RotationsPerSecond))); } public Command setIntakeRollersVelocityCmd(double volts) { return runOnce(() -> setIntakeRollersVelocity(volts)); } - // TESTING TO SEE IF WE CAN JUST SAY 0 AS 0 - public Command intakeArmHome() { - return Commands.parallel( - Commands.sequence( - runOnce(() -> m_intakeArm.setPosition(0)), - - runOnce(() -> m_isIntakeArmHomed = true), - runOnce(() -> log_isIntakeArmHomed.accept(m_isIntakeArmHomed)), + public Command startIntakeRollers(double volts) { + return setIntakeRollersVelocityCmd(volts); + } - runOnce(() -> removeDefaultCommand()) - ) - ); + public Command stopIntakeRollers() { + return setIntakeRollersVelocityCmd(0); } + + // ============================================================= + // HOMING + // ============================================================= + + /* + * intakeArmCurrentSenseHoming + * + * Finds the arm's mechanical zero position without an absolute encoder. + * + * How it works: + * 1. Drive the arm slowly toward the hard stop with a small negative voltage (-3.25 V). + * 2. When the arm hits the stop, the motor stalls: current spikes AND velocity drops to zero. + * 3. Both conditions are debounced (held for 100/125 ms) to filter out false positives + * from momentary bumps or noise during movement. + * 4. Once both conditions are confirmed, zero the encoder at that position, mark homed, + * retract the arm, and remove this as the default command. + * + * The whole thing has a 3 s timeout so a broken sensor or stuck arm doesn't stall the robot. + * + * FunctionalCommand is WPILib's way of building a command inline from four lambda functions: + * init — runs once when the command starts + * execute — runs every loop while the command is active (nothing to do here) + * onEnd — runs once when the command ends (whether it finished or was interrupted) + * isFinished — returns true when the command should stop + */ public Command intakeArmCurrentSenseHoming() { Runnable init = () -> { - m_intakeArm.setControl(m_intakeArmZeroingReq.withOutput(-3.25)); - + m_intakeArm.setControl(m_intakeArmZeroingReq.withOutput(-3.25)); // drive slowly into the hard stop m_isIntakeArmHomed = false; log_isIntakeArmHomed.accept(m_isIntakeArmHomed); }; @@ -192,22 +262,30 @@ public Command intakeArmCurrentSenseHoming() { Runnable execute = () -> {}; Consumer onEnd = (Boolean interrupted) -> { - m_intakeArm.setControl(m_intakeArmZeroingReq.withOutput(0)); - m_intakeArm.setPosition(0); + m_intakeArm.setControl(m_intakeArmZeroingReq.withOutput(0)); // stop applying voltage + m_intakeArm.setPosition(0); // zero the encoder here removeDefaultCommand(); - setIntakeArmPosCmd(IntakeArmPosition.RETRACTED); + setIntakeArmPosCmd(IntakeArmPosition.RETRACTED); // move to safe position m_isIntakeArmHomed = true; log_isIntakeArmHomed.accept(m_isIntakeArmHomed); }; + // Both current spike AND near-zero velocity must be true for their respective debounce + // durations before we declare the arm is at the hard stop. BooleanSupplier isFinished = () -> m_currentDebouncer.calculate(m_currentSpike.getAsBoolean()) && m_velocityDebouncer.calculate(m_veloIsNearZero.getAsBoolean()); - return new FunctionalCommand(init, execute, onEnd, isFinished, this).withTimeout(3).withName("intakeArm homing"); + return new FunctionalCommand(init, execute, onEnd, isFinished, this) + .withTimeout(3) + .withName("intakeArm homing"); } - /* PERIODICS */ + + // ============================================================= + // PERIODIC + // ============================================================= + @Override public void periodic() { log_targetIntakeArmRots.accept(m_MMVReq.Position); @@ -218,12 +296,30 @@ public void periodic() { @Override public void simulationPeriodic() { - WaltMotorSim.updateSimFX(m_intakeArm, m_intakeArmSim); + WaltMotorSim.updateSimFX(m_intakeArm, m_intakeArmSim); WaltMotorSim.updateSimFX(m_intakeRollersA, m_intakeRollersSim); } - /* ENUMS */ - public enum IntakeArmPosition{ + + // ============================================================= + // ENUMS + // ============================================================= + + /* + * IntakeArmPosition + * + * Named positions for the intake arm. All values are stored as both + * degrees (for human readability) and rotations (for motor commands). + * Note: these are mechanism rotations (arm angle), not motor rotations — + * the 125:1 gear ratio is baked into the TalonFX feedback config so the + * controller reports arm angle directly. + * + * RETRACTED — arm is fully up, tucked inside the robot frame. Safe for driving. + * DEPLOYED — arm is fully down to collect balls off the ground. + * SHIMMY — partway down, used to agitate balls already inside the robot. + * SAFE — just above DEPLOYED, used when approaching a ball cautiously. + */ + public enum IntakeArmPosition { RETRACTED(Rotations.of(0.061514).in(Degrees)), DEPLOYED(Rotations.of(0.289062 * 0.86).in(Degrees)), SHIMMY(Rotations.of(0.126025).in(Degrees)), @@ -237,5 +333,4 @@ private IntakeArmPosition(double degs) { this.rots = Rotations.of(this.degs.in(Rotations)); } } - -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index f0a71978..3382cc7e 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -15,22 +14,27 @@ import static frc.robot.Constants.IntakeK.kIntakeRollersShimmyVolts; import java.util.function.BooleanSupplier; -import java.util.function.Supplier; public class Superstructure extends SubsystemBase { - /* SUBSYSTEMS */ + // --- SUBSYSTEM REFERENCES --- private final Intake m_intake; private final Indexer m_indexer; private final Shooter m_shooter; - /* CONSTRUCTOR */ + // ====================================================================== + // CONSTRUCTOR + // ====================================================================== public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { m_intake = intake; m_indexer = indexer; m_shooter = shooter; } - /* BUTTON BIND SEQUENCES */ + // ====================================================================== + // SEQUENCES + // ====================================================================== + + // --- INTAKE --- /** * @param isShooting is if the robot is shooting * @return the intake Command @@ -67,6 +71,9 @@ public Command retractIntake() { ); } + // ~~~~~~~~~~~~~~~~~ + // OUTTAKE + // ~~~~~~~~~~~~~~~~~ // /** // * Turns on spinner and exhaust and sets shooter speed to RPS. // *

@@ -124,6 +131,7 @@ public Command activateOuttakeShotCalc() { }); } + // --- HOOD --- public Command activateHoodShotCalc() { return m_shooter.hoodFromCalc() .finallyDo(() -> { @@ -131,6 +139,7 @@ public Command activateHoodShotCalc() { }); } + // --- FLYWHEEL --- public Command spinUpFlywheel() { return m_shooter.shootFromCalc() .finallyDo(() -> { @@ -138,6 +147,7 @@ public Command spinUpFlywheel() { }); } + // --- HOOD && FLYWHEEL --- public Command hoodAndFlywheelShotCalc() { return Commands.parallel( m_shooter.hoodFromCalc(), @@ -150,7 +160,7 @@ public Command hoodAndFlywheelShotCalc() { } /** - * deactivates the outtake + * Stops the tunnel, spindexer, and the shooter. */ public void deactivateOuttake() { m_indexer.stopSpindexer(); @@ -158,16 +168,26 @@ public void deactivateOuttake() { m_shooter.setShooterVelocity(ShooterK.kShooterZeroRPS); } + // --- FLYWHEEL --- + /** + * Sets the Flywheel to a speed of 0 which will CoastOut + */ public void turnOffFlywheel() { m_shooter.setShooterVelocity(ShooterK.kShooterZeroRPS); } + // --- HOOD --- + /** + * Sets the Hood back to a position that is able to go underneath the trench + */ public void hoodBackToSafe() { m_shooter.m_hood.setHoodPos(ShooterK.kHoodEmergencyRotsD); } /** - * @return the emergency barf command + * Command to get all balls out of the hopper, one way or another. + * This reverses the intake rollers, and shoots out all the balls at a set LOCKED turret pose. + * @return Command above */ public Command emergencyBarf() { return Commands.startEnd( @@ -191,7 +211,7 @@ public Command emergencyBarf() { /** - * @return the emergency barf command + * @return Command that reverses only the intake rollers */ public Command emergencyBarfOnlyIntake() { return Commands.startEnd( @@ -205,6 +225,11 @@ public Command emergencyBarfOnlyIntake() { ); } + /** + * Shimmies the intake up and down while running the intake rollers at a slower speed + * @param isShooting is the robot in a shooting state + * @return the shimmy command + */ public Command intakeShimmy(BooleanSupplier isShooting) { return Commands.repeatingSequence( intake(isShooting, () -> true).withTimeout(0.5), @@ -217,6 +242,8 @@ public Command intakeShimmy(BooleanSupplier isShooting) { } /** + * Runs the indexer in reverse, and if the robot is shooting, then the shooter flywheel WONT reverse, + * otherwise flywheel will reverse * @param isShooting is if the robot is currently shooting * @return the unjam Command */ diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f4199435..80bf16fd 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -46,43 +46,80 @@ import frc.util.WaltLogger.DoubleLogger; import frc.util.WaltLogger.Pose2dLogger; -/** - * CommandSwerveDrivetrain: Class that extends the Phoenix 6 SwerveDrivetrain class - * and implements Subsystem so it can easily be used in command-based projects. +/* + * Swerve * - * Generated by the 2026 Tuner X Swerve Project Generator + * This is our drivetrain subsystem. It extends TunerSwerveDrivetrain (generated + * by CTRE Tuner X) and adds our own behavior on top of it. + * + * Quick primer on swerve drive (skip if you know it already): + * A swerve drivetrain has four independently steered + driven wheel modules. + * Each module can point in any direction, so the robot can strafe, spin, and + * drive at the same time. We use "field-centric" control so pushing "forward" + * on the joystick always moves toward the opposing alliance wall, regardless + * of which way the robot happens to be facing. + * + * What this class adds on top of the CTRE base: + * - Alliance-aware "operator perspective" so field-centric works correctly for + * both Red and Blue (they face opposite directions on the field). + * - Choreo path following: followPath() is called by Choreo each loop tick + * during auton to track a pre-planned trajectory. + * - Helper commands: xBrakeCmd, roboToPose, roboToRotation, etc. + * - Logging of chassis speeds and odometry data to AdvantageScope. + * - SysId support for characterizing the drive motors. + * + * The hardware config (module positions, gear ratios, PID gains, CAN IDs, etc.) + * lives in TunerConstants, generated by Tuner X: * https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html */ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { + + // ---- simulation ---- + // The CTRE drivetrain sim needs to run faster than the main 20 ms robot loop + // so its PID loops behave like they do on the real robot. A Notifier fires + // the sim update at 4 ms on its own thread. private static final double kSimLoopPeriod = 0.004; // 4 ms private Notifier m_simNotifier = null; private double m_lastSimTime; + // Converts individual module states (speed + direction) to/from overall robot speed. + // Used in periodic() to compute and log total chassis speed. public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(getModuleLocations()); + private final StatusSignal sig_gyroRoll = getPigeon2().getRoll(); private final StatusSignal sig_gyroYaw = getPigeon2().getYaw(); - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + // ---- alliance perspective ---- + // Field-centric drive uses a fixed field coordinate frame. + // Blue alliance treats 0° as forward (toward the red wall). + // Red alliance treats 180° as forward (toward the blue wall). + // We update this in periodic() so it re-applies automatically if the robot + // code restarts mid-match. private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ private boolean m_hasAppliedOperatorPerspective = false; - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - // private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - // private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - + // ---- path following PID controllers ---- + // During Choreo auton, these three controllers correct for the robot drifting + // off the planned trajectory. Choreo provides feed-forward velocities from the + // trajectory itself, and these PIDs add a correction term on top based on the + // error between our actual pose and the desired pose. + // X/Y controllers - correct position error in meters + // Theta controller - correct heading error in radians, with continuous input + // so it wraps at ±π (takes the short way around) private final PIDController m_pathXController = new PIDController(4.69, 0, 0); private final PIDController m_pathYController = new PIDController(4.69, 0, 0); private final PIDController m_pathThetaController = new PIDController(4.68, 0, 0); + + // The swerve request that actually sends corrected speeds to the modules. + // Velocity + Position request types give better tracking accuracy than open-loop. private final SwerveRequest.ApplyFieldSpeeds m_pathApplyFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds() .withDriveRequestType(DriveRequestType.Velocity) .withSteerRequestType(SteerRequestType.Position); private final Detection detection = new Detection(); + // ---- loggers ---- private final DoubleLogger log_absoluteRobotSpeed = new WaltLogger.DoubleLogger("Swerve", "absoluteRobotSpeed"); private final DoubleLogger log_vxMPS = new DoubleLogger("Swerve", "vxMPS"); private final DoubleLogger log_vyMPS = new DoubleLogger("Swerve", "vyMPS"); @@ -90,18 +127,24 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private final DoubleLogger log_currentAutonTotalTime = new DoubleLogger("Swerve/Auton", "currentTotalTime"); private final DoubleLogger log_sampleAt_velocityX = new DoubleLogger("Swerve/Auton", "sampleAt_velocityX"); private final DoubleLogger log_sampleAt_velocityY = new DoubleLogger("Swerve/Auton", "sampleAt_velocityY"); - + // Field-centric drive request used by the pose-targeting helpers. + // BlueAlliance perspective means "forward" = toward red wall for both alliances. private final SwerveRequest.FieldCentric swreq_drive = new SwerveRequest.FieldCentric() .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + // ---- SysId ---- + // SysId (System Identification) is a WPILib tool that characterizes the drive + // motors by logging their response to known voltage inputs, then computing + // feedforward and PID gains from the data. Only translation is currently active. + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = + new SwerveRequest.SysIdSwerveTranslation(); + private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class + null, // default ramp rate (1 V/s) + Volts.of(4), // 4 V step to avoid brownout + null, // default timeout (10 s) state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) ), new SysIdRoutine.Mechanism( @@ -111,114 +154,37 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { ) ); - // /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - // private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - // new SysIdRoutine.Config( - // null, // Use default ramp rate (1 V/s) - // Volts.of(7), // Use dynamic voltage of 7 V - // null, // Use default timeout (10 s) - // // Log state with SignalLogger class - // state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - // ), - // new SysIdRoutine.Mechanism( - // volts -> setControl(m_steerCharacterization.withVolts(volts)), - // null, - // this - // ) - // ); - - // /* - // * SysId routine for characterizing rotation. - // * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - // * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - // */ - // private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - // new SysIdRoutine.Config( - // /* This is in radians per second², but SysId only supports "volts per second" */ - // Volts.of(Math.PI / 6).per(Second), - // /* This is in radians per second, but SysId only supports "volts" */ - // Volts.of(Math.PI), - // null, // Use default timeout (10 s) - // // Log state with SignalLogger class - // state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - // ), - // new SysIdRoutine.Mechanism( - // output -> { - // /* output is actually radians per second, but SysId only supports "volts" */ - // setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - // /* also log the requested output for SysId */ - // SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - // }, - // null, - // this - // ) - // ); - - /* The SysId routine to test */ private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ + + // ============================================================= + // CONSTRUCTORS + // (generated by CTRE Tuner X — the hardware config lives in TunerConstants) + // ============================================================= + + /** Standard constructor — uses default odometry update frequency. */ public Swerve( SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules ) { super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } + if (Utils.isSimulation()) startSimThread(); } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ + /** Constructor with a custom odometry update frequency. */ public Swerve( SwerveDrivetrainConstants drivetrainConstants, double odometryUpdateFrequency, SwerveModuleConstants... modules ) { super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } + if (Utils.isSimulation()) startSimThread(); } /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module + * Constructor with custom odometry frequency and separate standard deviations + * for wheel odometry vs. vision pose estimates. The Kalman filter uses these + * to decide how much to trust each source. */ public Swerve( SwerveDrivetrainConstants drivetrainConstants, @@ -228,91 +194,61 @@ public Swerve( SwerveModuleConstants... modules ) { super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); - if (Utils.isSimulation()) { - startSimThread(); - } + if (Utils.isSimulation()) startSimThread(); } - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier request) { - return run(() -> this.setControl(request.get())); - } - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ + // ============================================================= + // SYSID + // ============================================================= + + /** Runs the quasistatic (slow voltage ramp) SysId test. */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return m_sysIdRoutineToApply.quasistatic(direction); } - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ + /** Runs the dynamic (step voltage) SysId test. */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return m_sysIdRoutineToApply.dynamic(direction); } - @Override - public void periodic() { - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. - */ - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - WaltDriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); - m_hasAppliedOperatorPerspective = true; - }); - } - var state = getState(); - var speeds = m_kinematics.toChassisSpeeds(state.ModuleStates); - log_vxMPS.accept(speeds.vxMetersPerSecond); - log_vyMPS.accept(speeds.vyMetersPerSecond); - log_absoluteRobotSpeed.accept(Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)); - } + // ============================================================= + // CORE COMMANDS + // ============================================================= - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); + /** + * Applies a swerve request every loop while the command runs. + * Most driver commands use this pattern: + * applyRequest(() -> swreq_drive.withVelocityX(x).withVelocityY(y)) + */ + public Command applyRequest(Supplier request) { + return run(() -> this.setControl(request.get())); + } - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; + /** + * Locks all four wheel modules into an X-pattern (each pointing 45° outward). + * This creates a passive brake — the robot resists being pushed in any direction. + * Used at the end of auton so the robot doesn't slide after stopping. + */ + public Command xBrakeCmd() { + final SwerveRequest.SwerveDriveBrake stopReq = new SwerveDriveBrake(); + return runOnce(() -> setControl(stopReq)); + } - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); + /** Returns the robot's current velocity as vx, vy, omega (ChassisSpeeds). */ + public ChassisSpeeds getChassisSpeeds() { + return m_kinematics.toChassisSpeeds(getState().ModuleStates); } + + // ============================================================= + // VISION + // ============================================================= + /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate - * while still accounting for measurement noise. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. + * Fuses a PhotonVision pose measurement into the Kalman filter odometry. + * The FPGA timestamp conversion corrects for processing latency. */ @Override public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { @@ -320,17 +256,8 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS } /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate - * while still accounting for measurement noise. - *

- * Note that the vision measurement standard deviations passed into this method - * will continue to apply to future measurements until a subsequent call to - * {@link #setVisionMeasurementStdDevs(Matrix)} or this method. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement - * in the form [x, y, theta]ᵀ, with units in meters and radians. + * Same as above but also updates the vision standard deviations, which control + * how much the filter trusts this measurement relative to wheel odometry. */ @Override public void addVisionMeasurement( @@ -342,38 +269,38 @@ public void addVisionMeasurement( } /** - * Return the pose at a given timestamp, if the buffer is not empty. - * - * @param timestampSeconds The timestamp of the pose in seconds. - * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). + * Looks up the estimated robot pose at a past timestamp by interpolating the + * odometry history buffer. Useful for matching vision detections to where the + * robot was when the camera frame was actually captured. */ @Override public Optional samplePoseAt(double timestampSeconds) { return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds)); } - public ChassisSpeeds getChassisSpeeds() { - return m_kinematics.toChassisSpeeds(getState().ModuleStates); - } + // ============================================================= + // PATH FOLLOWING (CHOREO) + // ============================================================= + + // Called by Choreo every loop tick during auton. + // The SwerveSample has the desired pose + chassis speeds at this trajectory timestamp. + // We take Choreo's feed-forward velocities and add PID correction on top + // to stay on the planned path even if the robot drifts slightly. private void followPath(SwerveSample sample) { - m_pathThetaController.enableContinuousInput(-Math.PI, Math.PI); - var pose = getState().Pose; - // var samplePose = sample.getPose(); + m_pathThetaController.enableContinuousInput(-Math.PI, Math.PI); // wrap heading at ±π - // var speed = getState().Speeds; - var targetSpeeds = sample.getChassisSpeeds(); + var pose = getState().Pose; + var targetSpeeds = sample.getChassisSpeeds(); // feed-forward from Choreo - targetSpeeds.vxMetersPerSecond += m_pathXController.calculate( - pose.getX(), sample.x - ); - targetSpeeds.vyMetersPerSecond += m_pathYController.calculate( - pose.getY(), sample.y - ); + // Add PID correction for each axis of error. + targetSpeeds.vxMetersPerSecond += m_pathXController.calculate(pose.getX(), sample.x); + targetSpeeds.vyMetersPerSecond += m_pathYController.calculate(pose.getY(), sample.y); targetSpeeds.omegaRadiansPerSecond += m_pathThetaController.calculate( - pose.getRotation().getRadians(), sample.heading - ); + pose.getRotation().getRadians(), sample.heading); + // Apply to the swerve modules, including per-module force feedforwards from + // Choreo for better tracking on aggressive paths. setControl( m_pathApplyFieldSpeeds.withSpeeds(targetSpeeds) .withWheelForceFeedforwardsX(sample.moduleForcesX()) @@ -381,22 +308,17 @@ private void followPath(SwerveSample sample) { ); } - public Command xBrakeCmd() { - final SwerveRequest.SwerveDriveBrake stopReq = new SwerveDriveBrake(); - return runOnce(() -> setControl(stopReq)); - } - - /** - * Creates a new auto factory for this drivetrain. - * - * @return AutoFactory for this drivetrain + * Creates the Choreo AutoFactory used by WaltAdaptableAutonFactory. + * The trajectory logger fires at the start/end of each trajectory and + * logs a sample at t=4.25 s for pre-match path verification. */ public AutoFactory createAutoFactory() { return createAutoFactory((traj, isStart) -> { SwerveSample sample = traj.sampleAt(4.25, false).get(); WaltLogger.timedPrint(String.format("TrajLog - isStart: %b", isStart)); - WaltLogger.timedPrint("Sample at 4.25s (PRE BUMP); X:" + sample.getPose().getX() + " Y:" + traj.sampleAt(4.25, false).get().getPose().getY()); + WaltLogger.timedPrint("Sample at 4.25s (PRE BUMP); X:" + sample.getPose().getX() + + " Y:" + traj.sampleAt(4.25, false).get().getPose().getY()); log_sampleAt_pose.accept(sample.getPose()); log_sampleAt_velocityX.accept(sample.vx); log_sampleAt_velocityY.accept(sample.vy); @@ -404,69 +326,54 @@ public AutoFactory createAutoFactory() { }); } - /** - * Creates a new auto factory for this drivetrain with the given - * trajectory logger. - * - * @param trajLogger Logger for the trajectory - * @return AutoFactory for this drivetrain - */ + /** Creates a Choreo AutoFactory with a custom trajectory logger. */ public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) { return new AutoFactory( () -> getState().Pose, this::resetPose, this::followPath, - true, + true, // auto-flip trajectories for red alliance this, trajLogger ); } + + // ============================================================= + // POSE-TARGETING HELPERS + // ============================================================= + // Simple PID-based "go to this pose" commands used outside of Choreo auton + // (e.g. auto-alignment, short position corrections). Not as smooth as a + // full Choreo path but useful for quick corrections. + /** - * @param desPose Posd2d to move to - * @return a Command that makes the robot move to the desired Pose2d + * Drives the robot to a target Pose2d using the path PID controllers. + * Stops once within `tolerance` meters of the target translation. */ public Command roboToPose(Pose2d desPose, double tolerance) { return Commands.runOnce(() -> { Pose2d curPose = getState().Pose; double xSpeed = m_pathXController.calculate(curPose.getX(), desPose.getX()); double ySpeed = m_pathYController.calculate(curPose.getY(), desPose.getY()); - double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), desPose.getRotation().getRadians()); + double thetaSpeed = m_pathThetaController.calculate( + curPose.getRotation().getRadians(), desPose.getRotation().getRadians()); setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); }).andThen(Commands.waitUntil(() -> isNearPose(getState().Pose, desPose, tolerance))) .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); } - public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance, double rotationTolerance) { - return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance) - && isNearRotation(curPose.getRotation(), desPose.getRotation(), rotationTolerance); - } - - public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance) { - return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance); - } - - /** - * @param desRotation Rotation2d to turn to - * @return a Command that makes the robot turn to the desired Rotation2d - */ + /** Rotates to a target heading only (no translation). Stops within `tolerance` radians. */ public Command roboToRotation(Rotation2d desRotation, double tolerance) { return Commands.runOnce(() -> { Rotation2d curRotation = getState().Pose.getRotation(); - double thetaSpeed = m_pathThetaController.calculate(curRotation.getRadians(), desRotation.getRadians()); + double thetaSpeed = m_pathThetaController.calculate( + curRotation.getRadians(), desRotation.getRadians()); setControl(swreq_drive.withRotationalRate(thetaSpeed)); }).andThen(Commands.waitUntil(() -> isNearRotation(getState().Pose.getRotation(), desRotation, tolerance))) .andThen(() -> setControl(swreq_drive.withRotationalRate(0))); } - public boolean isNearRotation(Rotation2d curRotation, Rotation2d desRotation, double tolerance) { - return Math.abs(MathUtil.angleModulus(curRotation.getRadians() - desRotation.getRadians())) <= tolerance; - } - - /** - * @param desTranslation Translation2d to go to - * @return a Command that makes the robot go to the desired Translation2d - */ + /** Translates to a target (x, y) only (no rotation). Stops within `tolerance` meters. */ public Command roboToTranslation(Translation2d desTranslation, double tolerance) { return Commands.runOnce(() -> { Translation2d curTranslation = getState().Pose.getTranslation(); @@ -477,6 +384,22 @@ public Command roboToTranslation(Translation2d desTranslation, double tolerance) .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); } + // ---- proximity checks ---- + + public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance, double rotationTolerance) { + return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance) + && isNearRotation(curPose.getRotation(), desPose.getRotation(), rotationTolerance); + } + + public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance) { + return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance); + } + + public boolean isNearRotation(Rotation2d curRotation, Rotation2d desRotation, double tolerance) { + // angleModulus wraps the difference into (-π, π] so we always measure the shorter arc. + return Math.abs(MathUtil.angleModulus(curRotation.getRadians() - desRotation.getRadians())) <= tolerance; + } + public boolean isNearTranslation(Translation2d curTranslation, Translation2d desTranslation, double tolerance) { return Math.hypot( desTranslation.getX() - curTranslation.getX(), @@ -484,27 +407,75 @@ public boolean isNearTranslation(Translation2d curTranslation, Translation2d des ) <= tolerance; } - /** - * robot goes to detected target - */ - public Command swerveToObject() { + + // ============================================================= + // VISION-BASED OBJECT TRACKING + // ============================================================= + + /** Drives to the closest detected object (ball) on the field. */ + public Command swerveToObject() { PhotonTrackedTarget target = detection.getClosestObject(); Pose2d destination = detection.targetToPose(getState().Pose, target); detection.addFuel(destination); - return roboToPose(destination, 0.1); } + /** + * Computes the robot pose needed to face a fuel location with the intake. + * Moves the robot to the ball's (x, y) and rotates it so the intake side + * faces the ball — hence the +180° offset on the approach angle. + */ public static Pose2d faceFuelPose(Pose2d robotPose, Pose2d fuelLocation) { double dx = fuelLocation.getX() - robotPose.getX(); double dy = fuelLocation.getY() - robotPose.getY(); - Rotation2d desiredRotation = new Rotation2d(Math.atan2(dy, dx)).plus(Rotation2d.fromDegrees(180)); + return new Pose2d(fuelLocation.getX(), fuelLocation.getY(), desiredRotation); + } - return new Pose2d( - fuelLocation.getX(), - fuelLocation.getY(), - desiredRotation - ); + + // ============================================================= + // PERIODIC + // ============================================================= + + @Override + public void periodic() { + // Apply the alliance perspective so field-centric drive works correctly. + // We re-check every loop while disabled (in case the DS assigns an alliance + // late) but stop checking once enabled so perspective doesn't flip mid-match. + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + WaltDriverStation.getAlliance().ifPresent(allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation + ); + m_hasAppliedOperatorPerspective = true; + }); + } + + var state = getState(); + var speeds = m_kinematics.toChassisSpeeds(state.ModuleStates); + log_vxMPS.accept(speeds.vxMetersPerSecond); + log_vyMPS.accept(speeds.vyMetersPerSecond); + log_absoluteRobotSpeed.accept(Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)); + } + + + // ============================================================= + // SIMULATION + // ============================================================= + + // Runs the CTRE drivetrain simulation at 4 ms intervals on a background thread. + // The faster rate is necessary for the simulated PID loops to behave like real hardware. + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Hood.java b/src/main/java/frc/robot/subsystems/shooter/Hood.java index aca76d92..b4f9a8eb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/Hood.java @@ -27,14 +27,33 @@ import frc.util.WaltLogger.BooleanLogger; import frc.util.WaltLogger.DoubleLogger; +/* + * Hood - TalonFXS-driven hood angle control. + * Zero is set at the hardstop via current-sense homing. Position is in rotations. + * + * Vocab: + * homing - drives to the hardstop at low voltage, detects stall via stator current spike through a Debouncer + * atPos - closed-loop error is within kHoodMaxErrD of the setpoint + */ public class Hood extends SubsystemBase { + // ---- CONSTANTS ---- private static final String kLogTab = "Shooter/Hood"; + // converts encoder rotations to physical hood angle degrees for logging private static final double kAbsoluteToPhysicalAngleRatio = (kPhysicalHoodMaxPosition_double - kPhysicalHoodMinPosition_double) / (360.0 * (kHoodMaxRots_double - kHoodMinRots_double)); + + // ---- MOTOR + CONTROLS ---- private final TalonFXS m_hood = new TalonFXS(kHoodCANID, kShooterBus); private final PositionVoltage m_hoodPVRequest = new PositionVoltage(0).withEnableFOC(false); private final VoltageOut m_hoodZeroReq = new VoltageOut(0); + private final StaticBrake m_BrakeReq = new StaticBrake(); + // ---- SIGNALS ---- + private final StatusSignal sig_hoodStatorCurrent = m_hood.getStatorCurrent(); + private final StatusSignal sig_hoodPos = m_hood.getPosition(); + private final StatusSignal sig_hoodCLErr = m_hood.getClosedLoopError(); + + // ---- LOGGERS ---- private final BooleanLogger log_hoodHomed = WaltLogger.logBoolean(kLogTab, "Homed"); private final DoubleLogger log_hoodControlPos = WaltLogger.logDouble(kLogTab, "controlPos"); private final DoubleLogger log_hoodCurrentPos = WaltLogger.logDouble(kLogTab, "currentPos"); @@ -42,19 +61,16 @@ public class Hood extends SubsystemBase { private final DoubleLogger log_hoodCLErr = WaltLogger.logDouble(kLogTab, "closedLoopErr"); private final BooleanLogger log_hoodAtPos = WaltLogger.logBoolean(kLogTab, "hoodAtPos"); + // ---- STATE ---- private Debouncer m_currentDebouncer = new Debouncer(0.125, DebounceType.kRising); - - private final StatusSignal sig_hoodStatorCurrent = m_hood.getStatorCurrent(); - private final StatusSignal sig_hoodPos = m_hood.getPosition(); - private final StatusSignal sig_hoodCLErr = m_hood.getClosedLoopError(); - private BooleanSupplier m_currentSpike = () -> sig_hoodStatorCurrent.getValueAsDouble() > 5.0; - - private final StaticBrake m_BrakeReq = new StaticBrake(); - private boolean m_isHoodHomed = false; private boolean m_hoodAtPos = false; + // ============================================================= + // CONSTRUCTOR + // ============================================================= + public Hood() { m_hood.getConfigurator().apply(kHoodTalonFXSConfiguration); @@ -68,7 +84,10 @@ public Hood() { // setDefaultCommand(hoodCurrentSenseHomingCmd()); } - // ---HOOD + // ============================================================= + // CONTROL + // ============================================================= + public void setHoodPos(double rots) { m_hood.setControl(m_hoodPVRequest.withPosition(rots)); log_hoodControlPos.accept(rots); @@ -78,10 +97,19 @@ public Command setHoodPosCmd(double rots) { return runOnce(() -> setHoodPos(rots)); } + // converts encoder rotations to physical degrees - only used for logging private static double getHoodAngleDeg(double posRots) { return kPhysicalHoodMinPosition_double + (posRots * 360.0 - kHoodMinRots_double * 360.0) * kAbsoluteToPhysicalAngleRatio; } + public void setHoodNeutralMode(NeutralModeValue value) { + m_hood.setNeutralMode(value); + } + + // ============================================================= + // STATE + // ============================================================= + public boolean isHoodHomed() { return m_isHoodHomed; } @@ -96,6 +124,10 @@ public boolean atPosition() { return m_hoodAtPos; } + // ============================================================= + // HOMING + // ============================================================= + public Command hoodCurrentSenseHomingCmd(){ Runnable init = () -> { m_hood.getConfigurator().apply(ShooterK.kHoodTalonFXSConfigurationNoSoftLimit); @@ -120,15 +152,15 @@ public Command hoodCurrentSenseHomingCmd(){ log_hoodHomed.accept(m_isHoodHomed); }; - BooleanSupplier isFinished = () -> + BooleanSupplier isFinished = () -> m_currentDebouncer.calculate(m_currentSpike.getAsBoolean()); return new FunctionalCommand(init, () -> {}, end, isFinished, this).withTimeout(5); } - public void setHoodNeutralMode(NeutralModeValue value) { - m_hood.setNeutralMode(value); - } + // ============================================================= + // PERIODIC + // ============================================================= @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/shooter/README.MD b/src/main/java/frc/robot/subsystems/shooter/README.MD new file mode 100644 index 00000000..b3ef575e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/README.MD @@ -0,0 +1,160 @@ +# /shooter + +The fun part. This folder contains everything responsible for launching game pieces: the flywheel, the adjustable hood, and a full-range turret. Together they handle spinning up to speed, aiming at the correct angle, tracking a target across the field, and knowing when a ball has actually left the robot (without a sensor in the ball path, yes, really). + +All of the actual shot math lives in [`calc/`](calc/), which runs on a background thread so it doesn't steal time from the robot loop. The classes here are purely responsible for controlling real hardware. + +--- + +## Files + +- `Shooter.java`: Master subsystem. Owns the flywheel motors, holds references to Hood and Turret, handles ball detection, and applies all shot parameters from the calc thread. +- `Turret.java`: Controls the turret's rotation. Homes itself using two absolute encoders with a fancy math algorithm (more on that below). +- `Hood.java`: Controls the hood's pitch angle. Homes by driving into a hard stop and watching the current spike. +- `FuelSim.java`: *(Commented out)* Physics simulation of ball trajectories for visualization in sim mode. Still here if we want it back. +- `TurretVisualizer.java`: *(Commented out)* 3D turret visualization for AdvantageScope. + +--- + +## Shooter.java + +The main subsystem. Two Kraken X44s spin a flywheel, with Motor B following Motor A in `Opposed` mode (they spin opposite directions but push the ball the same way). `Shooter` then drives `Hood` and `Turret` with setpoints received from `ShotCalcMath`, which runs at 75 Hz on its own thread. + +### Flywheel Control + +Velocity is controlled via `VelocityTorqueCurrentFOC` (Slot 1). Torque-current feedback gives tighter velocity regulation under load vs plain voltage control. Commanding velocity = 0 switches to `CoastOut` instead, which prevents a stuck closed-loop error sitting at zero. + +```java +void setShooterVelocity(double rotPerSec) // direct +Command shootFromCalc() // uses m_calcFlywheelVelocityRotPerSec +``` + +`isShooterSpunUp()` returns true when the closed-loop error is within ±0.5 RPS of target. + +### Ball Shot Detection + +No optical sensor. The flywheel decelerates sharply when a ball engages the wheels, so we just watch the acceleration: + +``` +Threshold: acceleration <= -3.0 RPS/s +Gate: motor must actually be in velocity control mode +``` + +When a detection fires, a timer starts. The shot is "confirmed" after: +- The ball detection clears (acceleration recovers) +- 1.2 seconds have elapsed on the timer +- We're still controlling to shoot + +This debounced trigger (`getBallShotDebounceTrg()`) is what tells the rest of the robot a ball has left, so it can advance the indexer or track shot count. Pretty neat for something with no sensor. + +### Driver RPS Tweaks + +Drivers can trim the computed flywheel speed without breaking anything + +- **Dynamic**: ±5% of the calculated RPS +- **Static**: ±`kDriverRPSIncreaseD` per button press +- **Reset**: back to 0 on release + +All tweaks are clamped to `[0, kShooterMaxRPSd]`. Gated behind `kAllowDriverRPSTweak` if you want to turn it off. +It's not being used rn but if we need to cope harder this is what we do :sunglasses: + +### What periodic() Does + +Every loop: +1. Caches turret position into a volatile field (ShotCalcMath reads this from its thread). +2. Fetches the latest `ShotCalcOutputs` from the background thread. +3. Sends turret position + velocity feedforward to `Turret`. +4. Sends hood reference to `Hood` (if homed). +5. Applies driver RPS tweaks with clamping. +6. Refreshes the spun-up check. +7. Logs everything. + +A `Tracer` is wired up for epoch timing diagnostics if you're hunting loop overruns. (hopefully syscore doesn't fail us) + +### NT Tunable Overrides + +- `/Shooter/shooterRPSOverride`: force a specific flywheel RPS (must be enabled in NT) +- `/Shooter/hoodRotsOverride`: force a specific hood position (must be enabled in NT) + +Useful when building LERP tables. Leave them disabled during actual matches. + +--- + +## Turret.java + +The turret rotates on a Kraken X44 (FOC, on the Canivore bus) using `PositionVoltage` control. It also carries two absolute encoders, not for redundancy, but because one encoder alone can't tell you absolute position if the turret has more than one full rotation of range. + +### Dual-Encoder Absolute Position Recovery + +Two encoders are geared at different ratios: + +- **Encoder A** (CANcoder): 10-tooth driving a 100-tooth output, so one full encoder reading every 36° of turret travel +- **Encoder B** (DutyCycleEncoder, DIO 3): 19-tooth driving a 100-tooth output, so one full encoder reading every ~18.95° of turret travel + +Because these periods are incommensurate, at any given absolute turret angle, only one combination of (Encoder A reading, Encoder B reading) is possible. `calcTurretAngleLCM()` exploits this through Least Common Modulus: + +1. Use Encoder A's reading to generate every candidate absolute angle within the turret's range (one per integer multiple of the 36° period). +2. For each candidate, predict what Encoder B *should* read at that angle. +3. Compare predicted vs actual Encoder B, wrapping the difference to ±180° to handle rollover. +4. Return the candidate with the smallest error. + +This runs on construction (`homeTurret(true)`) and also every loop for logging. If both encoders agree, you can trust the position even after a power cycle. It also exists as an override button on the manipulator so that incase something goes wrong with that homing, we wont get screwed over :pray: + +### Snapback Logic + +The turret has soft limits but also a physical range. If the turret is commanded near one limit (e.g., it's at +0.4 rot heading toward +0.5 rot max) and the target wraps around the other side, `calcAzimuth` (in ShotCalcMath) adds ±1 full rotation to the setpoint so the turret goes the short way around instead of slamming into a hardstop. `m_isSnappingBack` flags when the error is large enough that this is actively happening. + +### Key Methods + +```java +void homeTurret(boolean useLCM) // true = dual-encoder fusion; false = kInitPosition +void setTurretPos(double rots, double velFF) // main control path, called from Shooter.periodic() +void setTurretLock(boolean locked) // lock at current position +void lockAndSetTurretLockPos(double lockPos) // lock at a specific angle +void setIntaking(boolean intaking) // freeze the turret during intake +``` + +--- + +## Hood.java + +The hood controls launch angle. A TalonFXS drives it, and position is controlled via `PositionVoltage` (FOC disabled here). + +### Homing + +The hood homes by driving slowly into its physical lower limit and waiting for the stator current to spike above 5A (sustained for 0.125s via a `Debouncer`). When it stalls: +- Position is set to `kHoodAbsoluteMinRots`. +- Soft limits are re-applied. +- `m_isHoodHomed` flips to true. + +There's a 5-second timeout in case something goes wrong. If it times out or gets interrupted, the hood brakes and logs a warning. + +> Note: In the current build, the homing command is not set as a default command. The hood is initialized with `setPosition(0)` directly in the constructor and immediately considered homed. `hoodCurrentSenseHomingCmd()` is still available if you want to actually run the homing routine, and still exists as an override button. + +### Key Methods + +```java +void setHoodPos(double rots) // set target position +Command hoodCurrentSenseHomingCmd() // full homing routine (returns a command) +boolean isHoodHomed() +boolean atPosition() // within kHoodMaxErrD of setpoint +``` + +--- + +## How It All Fits Together + +``` +75 Hz Thread (ShotCalcMath) + reads: swerve state, volatile turret position + writes: volatile ShotCalcOutputs + +50 Hz Robot Loop (Shooter.periodic) + reads: ShotCalcOutputs + drives: Turret setpoint + velocity FF + drives: Hood setpoint + drives: Flywheel velocity (from operator commands, using calc output) + reads: flywheel accel -> ball detection -> debounced shot trigger +``` + +The calc thread running at 75 Hz means shot math never blocks motor outputs, even when the iterative convergence takes a bit longer on a given cycle. diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 415c6c97..eafa672c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -18,12 +18,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; - import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Tracer; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -38,7 +34,8 @@ import java.util.function.Supplier; import frc.robot.Constants; -import frc.robot.subsystems.shooter.ShooterCalc.ShotCalcOutputs; +import frc.robot.subsystems.shooter.calc.ShotCalcMath; +import frc.robot.subsystems.shooter.calc.ShotCalcMath.ShotCalcOutputs; import frc.util.SignalManager; import frc.util.WaltMotorSim; import frc.util.WaltTunable; @@ -46,58 +43,70 @@ import frc.util.WaltLogger.BooleanLogger; import frc.util.WaltLogger.DoubleLogger; +/* + * Shooter - flywheel velocity control + Hood + Turret coordination. + * Owns ShotCalcMath (background 75 Hz thread) and pushes its outputs to each subsystem every periodic. + * Ball detection works by watching flywheel acceleration drop while in VelocityTorqueCurrentFOC mode. + * + * Vocab: + * spunUp - flywheel closed-loop error is within 0.5 RPS of setpoint + * ballDetected - acceleration drops below threshold, meaning a ball entered the flywheel + * shotDropSeen - latch set when a ball is first detected, cleared when exiting shoot control mode + * driverRPSTweak - small RPS offset the driver can add/remove at runtime for fine-tuning + */ public class Shooter extends SubsystemBase { + // ---- TUNING ---- // NT-tunable overrides for LERP table building (default off) private static final WaltTunable kShooterRPSOverride = new WaltTunable("/Shooter/shooterRPSOverride", kShooterRPSd); private static final WaltTunable kHoodRotsOverride = new WaltTunable("/Shooter/hoodRotsOverride", 0.0); + private static final WaltTunable kBallDetectionTuner = + new WaltTunable("Shooter/ballDetectionTuner", 0); private static final double kHoodLockedPosRots = Rotations.of(0.33).magnitude(); + // ---- MOTORS + CONTROLS ---- + private final TalonFX m_shooterA = new TalonFX(kShooterA_CANID, Constants.kShooterBus); // X44 + private final TalonFX m_shooterB = new TalonFX(kShooterB_CANID, Constants.kShooterBus); // X44 + // private final VelocityVoltage m_velocityRequest = new VelocityVoltage(0).withEnableFOC(true).withSlot(0); + private final VelocityTorqueCurrentFOC m_veloTQFOCReq = new VelocityTorqueCurrentFOC(0).withSlot(1); + private final CoastOut m_motorIdleReq = new CoastOut(); + + // ---- SUBSYSTEM REFS ---- + private final Supplier m_threadsafeSwerveSup; + public final Hood m_hood; + public final Turret m_turret; + private final ShotCalcMath m_ShotCalcMath; + + // ---- RUNTIME STATE ---- private final Tracer m_periodicTracer = new Tracer(); - /* VARIABLES */ // boolean m_useShotCalculator = true; - private double m_currentFlywheelVelocityRotPerSec; private double m_latestFlywheelAccelerationRotPerSec; private boolean m_isShooterSpunUp = false; private boolean m_shotDropSeen = false; - private final Timer m_shotRecoveryTimer = new Timer(); - private int m_fuelStored = 8; // private final TurretVisualizer m_turretVisualizer; // private final FuelSim m_fuelSim; - // ---MOTORS + CONTROL REQUESTS - private final TalonFX m_shooterA = new TalonFX(kShooterA_CANID, Constants.kShooterBus); // X44 - private final TalonFX m_shooterB = new TalonFX(kShooterB_CANID, Constants.kShooterBus); // X44 - // private final VelocityVoltage m_velocityRequest = new VelocityVoltage(0).withEnableFOC(true).withSlot(0); - private final VelocityTorqueCurrentFOC m_veloTQFOCReq = new VelocityTorqueCurrentFOC(0).withSlot(1); - private final CoastOut m_motorIdleReq = new CoastOut(); - - private final Supplier m_threadsafeSwerveSup; - - public final Hood m_hood; - public final Turret m_turret; - - // thread copde + // ---- CALC STATE ---- + // read by ShotCalcMath background thread - volatile so reads don't go stale + // thread co(p)de so that we don't overrun (syscore save us all please) private volatile double m_latestTurretPositionRots = 0.0; - private final ShooterCalc m_shooterCalc; - private double m_calcFlywheelVelocityRotPerSec = kShooterRPSd; private double m_calcHoodRots = kHoodRotsd; private double m_driverRPSTweak = 0.0; - private int m_ballsShot = 0; + // ---- SIGNALS ---- private final StatusSignal sig_shooterCLErr = m_shooterA.getClosedLoopError(); private final StatusSignal sig_shooterAVelo = m_shooterA.getVelocity(); private final StatusSignal sig_shooterAAccel = m_shooterA.getAcceleration(); private final StatusSignal sig_shooterACtrlMode = m_shooterA.getControlMode(); - // ---LOGIC BOOLEANS + // ---- TRIGGERS ---- private final Trigger trg_inShootCtrlMode = new Trigger(() -> {return sig_shooterACtrlMode.getValue() == ControlModeValue.VelocityVoltage; }); private final Trigger trg_ballDetected = new Trigger(() -> detectShot()).and(trg_inShootCtrlMode); private final Trigger trg_shotDropSeen = new Trigger(() -> m_shotDropSeen); @@ -108,41 +117,40 @@ public class Shooter extends SubsystemBase { .and(trg_ballDetected.negate()) .and(trg_debounceHit); - /* SIM OBJECTS */ - private final FlywheelSim m_shooterSim = new FlywheelSim(LinearSystemId.createFlywheelSystem( - DCMotor.getKrakenX44(2), kShooterMoI, kShooterGearing), DCMotor.getKrakenX60Foc(2) // returns gearbox - ); + // ---- SIM ---- + // private final FlywheelSim m_shooterSim = new FlywheelSim(LinearSystemId.createFlywheelSystem( + // DCMotor.getKrakenX44(2), kShooterMoI, kShooterGearing), DCMotor.getKrakenX60Foc(2) // returns gearbox + // ); // private final DCMotorSim m_turretSim = new DCMotorSim(LinearSystemId.createDCMotorSystem( // DCMotor.getKrakenX44Foc(1), kTurretMoI, kTurretGearing), DCMotor.getKrakenX44Foc(1) // returns gearbox // ); - /* LOGGERS */ + // ---- LOGGERS ---- private final DoubleLogger log_shooterVelocityRPS = WaltLogger.logDouble("Shooter/Flywheel", "velocityRPS"); private final DoubleLogger log_shooterAccelRPS = WaltLogger.logDouble("Shooter/Flywheel", "accelRPS"); private final DoubleLogger log_turretPositionRots = WaltLogger.logDouble("Turret", "positionRots"); private final DoubleLogger log_turretPositionRobotRelativeRots = WaltLogger.logDouble("Turret", "positionRobotRelativeRots"); - private final BooleanLogger log_spunUp = WaltLogger.logBoolean(kLogTab, "spunUp"); // private final BooleanLogger log_canTurretShoot = WaltLogger.logBoolean(kLogTab, "canTurretShoot"); - private final DoubleLogger log_shooterClosedLoopError = WaltLogger.logDouble("Shooter/Flywheel", "closedLoopError"); - private final BooleanLogger log_ballDetected = WaltLogger.logBoolean(kLogTab, "ballDetected"); private final BooleanLogger log_ballShotDebounce = WaltLogger.logBoolean(kLogTab, "ballShot"); - private final DoubleLogger log_ballsShot = new DoubleLogger("Shooter/Flywheel", "balls shot"); private final DoubleLogger log_calcFlywheelVelocity = new DoubleLogger("Shooter/Flywheel", "calcFlywheelVelocity"); private final DoubleLogger log_driverAddedRPS = WaltLogger.logDouble(kLogTab, "driverAddedRPS"); - /* CONSTRUCTOR */ + // ============================================================= + // CONSTRUCTOR + // ============================================================= + public Shooter(Supplier poseSupplier, Supplier threadsafeSwerveStateSup, Supplier fieldSpeedsSupplier) { m_hood = new Hood(); m_turret = new Turret(); m_threadsafeSwerveSup = threadsafeSwerveStateSup; - m_shooterCalc = new ShooterCalc(m_threadsafeSwerveSup, () -> m_latestTurretPositionRots); + m_ShotCalcMath = new ShotCalcMath(m_threadsafeSwerveSup, () -> m_latestTurretPositionRots); - m_shooterCalc.shouldUseStaticShot(kUseStaticShot); + m_ShotCalcMath.shouldUseStaticShot(kUseStaticShot); m_shooterA.getConfigurator().apply(kShooterATalonFXConfiguration); m_shooterB.getConfigurator().apply(kShooterBTalonFXConfiguration); @@ -171,7 +179,10 @@ public Shooter(Supplier poseSupplier, Supplier threads initSim(); } - // ---SHOOTER (Velocity Control) + // ============================================================= + // FLYWHEEL CONTROL and other misc + // ============================================================= + public Command setShooterVelocityCmd(AngularVelocity RPS) { return runOnce(() -> setShooterVelocity(RPS)); } @@ -239,11 +250,39 @@ private void refreshShooterSpunUp() { m_isShooterSpunUp = sig_shooterCLErr.isNear(0, 0.5); } + // ============================================================= + // BALL DETECTION + // ============================================================= + + // acceleration drops when a ball hits the flywheel - threshold is tunable via NT + private boolean detectShot() { + double detectionThreshold = kBallDetectionTuner.getOr(-3.0); + return m_latestFlywheelAccelerationRotPerSec <= detectionThreshold; + } + + // /** + // * Launches SIMULATION FUEL™ at the current Flywheel Velocity, current Hood + // * Angle, and the + // * current Turret Position. + // */ + // public void launchFuel() { + // if (m_fuelStored == 0) + // return; + // // m_fuelStored--; + // } + + public Trigger getBallShotDebounceTrg() { + return trg_ballShotDebounced; + } + + // ============================================================= + // GETTERS + // ============================================================= + public boolean isShooterSpunUp() { return m_isShooterSpunUp; } - /* GETTERS */ public double getShooterVelocityRotPerSec() { return m_currentFlywheelVelocityRotPerSec; } @@ -256,15 +295,6 @@ public DoubleSupplier getShooterDesiredRotPerSecSupp() { return () -> m_calcFlywheelVelocityRotPerSec; } - /* SIMULATION */ - public boolean simAbleToIntake() { - return canIntake(); - } - - public void simIntake() { - intakeFuel(); - } - /** * @return true if robot can store more fuel */ @@ -276,24 +306,16 @@ public void intakeFuel() { m_fuelStored++; } - private boolean detectShot() { - boolean accelDrop = m_latestFlywheelAccelerationRotPerSec <= -3.0; - return accelDrop; - } + // ============================================================= + // SIMULATION + // ============================================================= - // /** - // * Launches SIMULATION FUEL™ at the current Flywheel Velocity, current Hood - // * Angle, and the - // * current Turret Position. - // */ - // public void launchFuel() { - // if (m_fuelStored == 0) - // return; - // // m_fuelStored--; - // } + public boolean simAbleToIntake() { + return canIntake(); + } - public Trigger getBallShotDebounceTrg() { - return trg_ballShotDebounced; + public void simIntake() { + intakeFuel(); } // TODO: update orientation values (if needed) @@ -302,7 +324,10 @@ private void initSim() { TalonFXSimState.MotorType.KrakenX60); } - /* PERIODICS */ + // ============================================================= + // PERIODIC + // ============================================================= + @Override public void periodic() { m_periodicTracer.addEpoch("Entry (Unused Time)"); @@ -313,7 +338,7 @@ public void periodic() { m_currentFlywheelVelocityRotPerSec = sig_shooterAVelo.getValueAsDouble(); m_latestFlywheelAccelerationRotPerSec = sig_shooterAAccel.getValueAsDouble(); - ShotCalcOutputs calcData = m_shooterCalc.getLatestShotCalcOutputs(); + ShotCalcOutputs calcData = m_ShotCalcMath.getLatestShotCalcOutputs(); m_periodicTracer.addEpoch("Stashing ShotCalc data"); log_shooterClosedLoopError.accept(sig_shooterCLErr.getValueAsDouble()); @@ -324,6 +349,7 @@ public void periodic() { // set outputs var turretVelocityFF = calcData.turretCalcDetails().turretVelocityFF(); + double flywheelReference = calcData.shooterReferenceRps(); if (m_turret.getTurretLocked()) { m_turret.setTurretPos(m_turret.getTurretLockAngleRots(), 0.0); m_calcFlywheelVelocityRotPerSec = kShooterRPSd; @@ -333,9 +359,7 @@ public void periodic() { // m_turret.setTurretPos(Rotations.of(-0.250)); } else { m_turret.setTurretPos(turretReference, turretVelocityFF); - m_calcFlywheelVelocityRotPerSec = kShooterRPSOverride.enabled() - ? kShooterRPSOverride.get() - : calcData.shooterReferenceRps(); + m_calcFlywheelVelocityRotPerSec = kShooterRPSOverride.getOr(flywheelReference); if (kAllowDriverRPSTweak) { // ENABLE THIS TO ALLOW DRIVER RPS TWEAK m_calcFlywheelVelocityRotPerSec += m_driverRPSTweak; m_calcFlywheelVelocityRotPerSec = MathUtil.clamp(m_calcFlywheelVelocityRotPerSec, 0, kShooterMaxRPSd); //clamp here or clamp only when setShooterVel is called? @@ -351,9 +375,7 @@ public void periodic() { // m_hood.setHoodPos(kHoodLockedPosRots); } else { if (!m_turret.getHoldTurretAtIntake()) { - m_calcHoodRots = kHoodRotsOverride.enabled() - ? kHoodRotsOverride.get() - : hoodReference; + m_calcHoodRots = kHoodRotsOverride.getOr(hoodReference); // m_hood.setHoodPos(kHoodRotsOverride.enabled() // ? kHoodRotsOverride.get() // : hoodReference); @@ -383,8 +405,8 @@ public void periodic() { // m_periodicTracer.printEpochs(); } - @Override - public void simulationPeriodic() { - WaltMotorSim.updateSimFX(m_shooterA, m_shooterSim); - } -} \ No newline at end of file + // @Override + // public void simulationPeriodic() { + // WaltMotorSim.updateSimFX(m_shooterA, m_shooterSim); + // } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 6d21661f..b20dcee4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.DutyCycleEncoder; @@ -27,43 +26,60 @@ import frc.util.WaltLogger.IntLogger; import frc.util.WaltLogger.Pose3dLogger; +/* + * Turret - TalonFX position control with dual-encoder absolute position recovery. + * Two encoders with incommensurate gear ratios (10:1 and 19:1) let us uniquely resolve the turret + * angle without homing to a hardstop. calcTurretAngleLCM does the math. + * + * Vocab: + * LCM - the encoder fusion algorithm. searches for the turret angle that satisfies both encoder readings. + * snapback - turret wraps +-1 rotation to avoid crossing hardstops; isSnappingBack flags this in-progress + * locked - turret holds a fixed angle (used during intake to keep balls from getting stuck) + */ public class Turret extends SubsystemBase { + // ---- CONSTANTS ---- private static final double kTurretMaxRotsFromHomeDeg = kTurretMaxRotsFromHome.in(Degrees); - private boolean m_holdTurretAtIntakePos = false; - private boolean m_turretLocked = false; - private double m_turretLockAngleRots = 0.0; + // ---- MOTOR + CONTROLS ---- private final TalonFX m_turret = new TalonFX(kTurretCANID, Constants.kCanivoreBus); // X44Foc private final PositionVoltage m_PVRequest = new PositionVoltage(0).withEnableFOC(true); - // ---LOGIC BOOLEANS - private boolean m_isTurretHomed = true; - public BooleanSupplier turretHomedSupp = () -> m_isTurretHomed; - private boolean m_turretAtPos = false; - public BooleanSupplier turretAtPosSupp = () -> m_turretAtPos; - private boolean m_isSnappingBack = false; - private final BooleanSupplier supp_isSnappingBack = () -> m_isSnappingBack; - + // ---- ENCODERS ---- private final CANcoder m_lcmEncA = new CANcoder(19, Constants.kCanivoreBus); private final DutyCycleEncoder m_lcmEncB = new DutyCycleEncoder(3); + // ---- SIGNALS ---- + private final StatusSignal sig_turretCLErr = m_turret.getClosedLoopError(); + private final StatusSignal sig_turretPos = m_turret.getPosition(); + private final StatusSignal sig_lcmEncAAbsPos = m_lcmEncA.getAbsolutePosition(); + + // ---- LOGGERS ---- private final DoubleLogger log_lcmEncAPos = WaltLogger.logDouble(kLogTab, "EncA/Pos"); private final DoubleLogger log_lcmEncBPos = WaltLogger.logDouble(kLogTab, "EncB/Pos"); private final IntLogger log_lcmEncBFreq = WaltLogger.logInt(kLogTab, "EncB/Freq"); private final BooleanLogger log_lcmEncBConn = WaltLogger.logBoolean(kLogTab, "EncB/Conn"); - private final DoubleLogger log_turretControlPos = WaltLogger.logDouble(kLogTab, "turretControlPos"); private final DoubleLogger log_turretLCMPos = WaltLogger.logDouble(kLogTab, "turretLCMPos"); private final DoubleLogger log_turretClosedLoopError = WaltLogger.logDouble(kLogTab, "turretCLE"); private final BooleanLogger log_atPos = WaltLogger.logBoolean(kLogTab, "atPos"); private final BooleanLogger log_turretLocked = WaltLogger.logBoolean(kLogTab, "turretLocked"); private final Pose3dLogger log_turretTransform = WaltLogger.logPose3d(kLogTab, "turretTransform"); - private final BooleanLogger log_isTurretSnappingBack = WaltLogger.logBoolean(kLogTab, "snappingBack"); - private final StatusSignal sig_turretCLErr = m_turret.getClosedLoopError(); - private final StatusSignal sig_turretPos = m_turret.getPosition(); - private final StatusSignal sig_lcmEncAAbsPos = m_lcmEncA.getAbsolutePosition(); + // ---- STATE ---- + private boolean m_holdTurretAtIntakePos = false; + private boolean m_turretLocked = false; + private double m_turretLockAngleRots = 0.0; + private boolean m_isTurretHomed = true; + public BooleanSupplier turretHomedSupp = () -> m_isTurretHomed; + private boolean m_turretAtPos = false; + public BooleanSupplier turretAtPosSupp = () -> m_turretAtPos; + private boolean m_isSnappingBack = false; + private final BooleanSupplier supp_isSnappingBack = () -> m_isSnappingBack; + + // ============================================================= + // CONSTRUCTOR + // ============================================================= public Turret() { m_turret.getConfigurator().apply(kTurretTalonFXConfiguration); @@ -79,6 +95,11 @@ public Turret() { homeTurret(true); } + // ============================================================= + // POSITION CONTROL + // ============================================================= + + // seeds the TalonFX encoder using the LCM fusion result (or a fixed init position as fallback) public void homeTurret(boolean useLCM) { if (useLCM) { double lcmRots = calcTurretAngleLCM(sig_lcmEncAAbsPos.getValueAsDouble() * 360, -(m_lcmEncB.get() - kEncBOffset) * 360) / 360.0; @@ -88,26 +109,32 @@ public void homeTurret(boolean useLCM) { } } - private void refreshTurretCLErr() { - log_turretClosedLoopError.accept(sig_turretCLErr.getValueAsDouble()); - m_turretAtPos = sig_turretCLErr.isNear(0, kTurretMaxErrD); - m_isSnappingBack = sig_turretCLErr.getValueAsDouble() >= kTurretMaxErrDSpin; - log_atPos.accept(m_turretAtPos); - log_isTurretSnappingBack.accept(m_isSnappingBack); + public Command setTurretPosCmd(Angle rots) { + return runOnce(() -> setTurretPos(rots)); } - public BooleanSupplier isSnappingBack() { - return supp_isSnappingBack; + public void setTurretPos(Angle rots) { + setTurretPos(rots, RotationsPerSecond.zero()); } - public boolean atPosition() { - return m_turretAtPos; + public void setTurretPos(Angle rots, AngularVelocity velocityFF) { + m_turret.setControl(m_PVRequest.withPosition(rots).withVelocity(velocityFF)); + log_turretControlPos.accept(rots.in(Rotations)); } - public void setIntaking(boolean intaking) { - m_holdTurretAtIntakePos = intaking; + public void setTurretPos(double rots, double velocityFF) { + m_turret.setControl(m_PVRequest.withPosition(rots).withVelocity(velocityFF)); + log_turretControlPos.accept(rots); } + public void setTurretNeutralMode(NeutralModeValue value) { + m_turret.setNeutralMode(value); + } + + // ============================================================= + // LOCKING + INTAKE + // ============================================================= + public void setTurretLock(boolean locked) { m_turretLocked = locked; if (m_turretLocked) { @@ -117,6 +144,7 @@ public void setTurretLock(boolean locked) { log_turretLocked.accept(m_turretLocked); } + // locks at a specific angle instead of snapshotting current position public void lockAndSetTurretLockPos(double lockPos) { m_turretLocked = true; m_turretLockAngleRots = lockPos; @@ -128,26 +156,28 @@ public Command setTurretLockCmd(boolean locked) { return runOnce(() -> setTurretLock(locked)); } - public Command setTurretPosCmd(Angle rots) { - return runOnce(() -> setTurretPos(rots)); + public void setIntaking(boolean intaking) { + m_holdTurretAtIntakePos = intaking; } - public void setTurretPos(Angle rots) { - setTurretPos(rots, RotationsPerSecond.zero()); - } + // ============================================================= + // STATE + GETTERS + // ============================================================= - public void setTurretPos(Angle rots, AngularVelocity velocityFF) { - m_turret.setControl(m_PVRequest.withPosition(rots).withVelocity(velocityFF)); - log_turretControlPos.accept(rots.in(Rotations)); + private void refreshTurretCLErr() { + log_turretClosedLoopError.accept(sig_turretCLErr.getValueAsDouble()); + m_turretAtPos = sig_turretCLErr.isNear(0, kTurretMaxErrD); + m_isSnappingBack = sig_turretCLErr.getValueAsDouble() >= kTurretMaxErrDSpin; + log_atPos.accept(m_turretAtPos); + log_isTurretSnappingBack.accept(m_isSnappingBack); } - public void setTurretPos(double rots, double velocityFF) { - m_turret.setControl(m_PVRequest.withPosition(rots).withVelocity(velocityFF)); - log_turretControlPos.accept(rots); + public BooleanSupplier isSnappingBack() { + return supp_isSnappingBack; } - public void setTurretNeutralMode(NeutralModeValue value) { - m_turret.setNeutralMode(value); + public boolean atPosition() { + return m_turretAtPos; } public double getCurrTurretPos() { @@ -170,6 +200,10 @@ public boolean isTurretHomed() { return m_isTurretHomed; } + // ============================================================= + // PERIODIC + // ============================================================= + public void periodic() { double encAVal = sig_lcmEncAAbsPos.getValueAsDouble(); double encBVal = m_lcmEncB.get(); @@ -184,6 +218,13 @@ public void periodic() { log_turretLCMPos.accept(turretAngleDeg / 360.0); } + // ============================================================= + // LCM ALGORITHM + // ============================================================= + + // finds the turret angle (in degrees) that satisfies both encoder readings. + // searches over all integer multiples of encA's period within the turret's physical range, + // and picks the candidate with the smallest residual against encB. public static double calcTurretAngleLCM(double e1, double e2) { double encARatio = kGearZeroToothCount / kGearOneToothCount; // 100/10.0 double encBRatio = kGearZeroToothCount / kGearTwoToothCount; // 100/19 @@ -192,7 +233,7 @@ public static double calcTurretAngleLCM(double e1, double e2) { double bestAngle = 0; double bestError = Double.MAX_VALUE; - // Center the search on the expected LCM output at home, spanning ±turret range + // Center the search on the expected LCM output at home, spanning ± turret range double centerDeg = kLCMAtHomeRots * 360.0; double rangeDeg = kTurretMaxRotsFromHomeDeg; int kMin = (int) Math.floor((centerDeg - rangeDeg - e1 / encARatio) / encAPeriod); diff --git a/src/main/java/frc/robot/subsystems/shooter/calc/README.MD b/src/main/java/frc/robot/subsystems/shooter/calc/README.MD new file mode 100644 index 00000000..4378b500 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/calc/README.MD @@ -0,0 +1,240 @@ +# /calc + +This is where it all hit the fan :sob: :sob: :sob: + +This folder handles all of the math that turns "robot is at this position going this fast" into "set flywheel to X RPS, hood to Y rotations, turret to Z degrees." It runs on a **background thread** at 75 Hz so none of this touches the main robot loop. Two classes: `ShotCalcMath` coordinates everything and manages the thread, and `ShotCalculator` contains the actual ballistics: tables, physics, iterative moving-shot convergence, the whole deal. + +--- + +## Files + +- `ShotCalcMath.java`: The orchestrator. Runs the background Notifier, determines which target to aim at, calls into `ShotCalculator` for shot parameters, computes turret azimuth, and stores results in volatile fields for `Shooter.periodic()` to read. +- `ShotCalculator.java`: Pure ballistics utility. Contains the interpolation tables, physics equations, and the iterative moving-shot algorithm. No state of its own (mostly static methods and final records). + +--- + +## ShotCalcMath.java + +### Threading + +`ShotCalcMath` uses a WPILib `Notifier` to run `calcCallback()` at 75 Hz on a background thread: + +```java +m_notifier.startPeriodic(Hertz.of(75)); // runs calcCallback() in a loop +``` + +Results are stored in `volatile` fields so `Shooter.periodic()` (running on the main thread) can safely read them without locking: + +```java +private volatile ShotCalcOutputs m_shotCalcOutputs; +private static volatile Translation3d m_aimTarget; +private static volatile boolean m_isPassingFlag; +``` + +`Shooter.periodic()` caches the turret position into a `volatile double` at the top of every loop, and ShotCalcMath reads this from its thread. This is intentional and noted in the code: do not move that cache line. + +### What calcCallback() Does Every Cycle + +1. Grabs the current `SwerveDriveState` from a thread-safe supplier. +2. Converts module states to field-relative `ChassisSpeeds`. +3. Determines the **target** via `calculateTarget()`. +4. Calls `calcShot()` to get flywheel speed, hood angle, and azimuth. +5. Logs everything. +6. Times itself and logs the loop duration. + +### Target Selection: `calculateTarget()` + +The robot shoots at different targets depending on where it is on the field: + +``` +Robot on own side of the hub -> SHOOTING MODE + Target: enemy hub center (alliance-flipped appropriately) + +Robot past the hub into neutral zone -> PASSING MODE + Target: right by/on the bump + - Robot left of field center -> left passing point + - Robot right of field center -> right passing point +``` + +`m_isPassingFlag` is set/cleared here and read by `ShotCalculator` to select the right LERP table. + +There's also a "no-passing zone" check (`robotInNoPassingZone`) that detects when the robot is near the opponent hub, where some passing angles may not be safe. The check uses field X/Y bounds and alliance color. +>We are not using this as of right now, as it somewhat screws with targeting + +### Turret Azimuth: `calcAzimuth()` + +Given a target position, robot pose, current turret heading, and field-relative chassis speeds, this computes the turret setpoint (in rotations) and a velocity feedforward. + +1. **Compute turret pivot position**: offset the robot center by `kTurretOffsetX_m` and `kTurretOffsetY_m` using the robot's heading angle. Uses raw `cos/sin` to avoid allocations. + +2. **Compute desired yaw**: `atan2` from turret pivot to target. + +3. **Convert to turret-relative rotations**: subtract the turret's zero direction (robot heading + `kTurretAngleOffsetRad`), normalize to `[-0.5, 0.5]` rotations. + +4. **Lateral bias compensation**: a sinusoidal correction applied based on the turret's angle relative to the robot: + ``` + bias = gain * sin(turretAngleRelToRobot) + ``` + This compensates for balls curving slightly left or right depending on which direction the turret is facing. Tunable via `/ShotCalc/lateralBiasGainRots`. + +5. **Snapback**: if the turret is currently on the positive side of center (`turretHeadingRots > 0`) and `desiredAngle + 1` is still within max range, add 1 rotation to the setpoint. Same logic minus 1 on the negative side. This keeps the turret tracking through a target that would otherwise require crossing a hard stop. + +6. **Velocity feedforward**: uses tangential velocity of the target relative to the turret pivot, minus robot rotational rate: + ``` + tangentialVel = (toTargetX * vx - toTargetY * vy) / distance + turretFFRadPerSec = tangentialVel / distance - omega + ``` + +### Output Records + +**`AzimuthCalcDetails`**: turret-specific outputs +- `turretReferenceRots`: final (snapback-safe) turret setpoint +- `turretVelocityFF`: feedforward in rad/s +- `turretX`, `turretY`: turret pivot in field coordinates (for logging) +- `fieldYawRad`: desired absolute yaw angle in radians +- `currentFieldYawRad`: current turret yaw in field coordinates +- `rawDesiredRotations`: pre-snapback angle (useful for debugging) + +**`ShotCalcOutputs`**: everything `Shooter.periodic()` needs +- `turretCalcDetails`: the `AzimuthCalcDetails` record above +- `shotData`: `ShotDataLerp` with flywheel RPS, hood angle, TOF, and predicted target +- `turretReferenceRots`: pulled up from `turretCalcDetails` for convenience +- `hoodReferenceRots`: hood angle converted from radians to rotations +- `shooterReferenceRps`: flywheel speed converted from rad/s to RPS + +--- + +## ShotCalculator.java + +Pure ballistics. No threads, no state besides the static tables and tunable constants. + +### Interpolation Tables: `ShotLerpTable` + +WPILib's `InterpolatingDoubleTreeMap` allocates on every lookup (it uses `TreeMap`). `ShotLerpTable` is a zero-allocation sorted-array replacement: + +- `keys[]`: distances in meters, sorted ascending +- `exitVels[]`: exit velocity in rad/s +- `hoodAngles[]`: hood angle in radians +- `tofs[]`: time of flight in seconds +- `drags[]`: drag coefficients + +Lookup is a binary search + linear interpolation. No objects created on the hot path. + +The Builder takes inputs in human-friendly units (rot/s, rotations) and converts to SI internally: +```java +shot.add(dist_m, rps, hoodRots, tof_s, drag); +// internally: rps * 2pi = rad/s, hoodRots * 2pi = radians +``` + +### Shot Tables + +There are two active tables: + +**`kShotTable`**: normal hub shots, 14 distance points from 0.985m to 8.627m +- Flywheel: ~39.8 to 68.8 RPS (raw values have a `kScoringRPSBoost` of -0.2 applied) +- Hood: 0 to 1.65 rad +- TOF: 0.97 to 1.65s +- Drag: 0.500 at all distances (used for drag compensation math) + + +**`kPassingTable`**: passing to alliance partners, 29 distance points from 4.008m to 14.355m +- Flywheel: ~48.75 to 105.14 RPS (`kRPSBoost` = +0.75 applied; `kLongRangeRPSBoost` = +0.35 for distances >= ~10.4m) +- Hood: ~0.70 to 1.16 rad +- TOF: 1.35 to 2.08s +- Drag: 0.254 at all distances (used for drag compensation math) + +### Iterative Moving-Shot: `iterativeMovingShotFromInterpolationMap()` + +This is the core of shoot on the move. It converges on shot parameters that account for the robot moving during ball flight. Up to 8 iterations: + +``` +Start: table lookup at current distance to target + +Each iteration: + 1. Apply drag-compensated drift time to compute predicted target position: + driftT = (1 - e^(-c * tof)) / c where c = drag coefficient (default 0.5) + predX = targetX - vxLaunch * driftT + predY = targetY - vyLaunch * driftT + 2. Recalculate distance to predicted position. + 3. Look up new flywheel speed, hood angle, TOF from table. + 4. Check convergence: + |delta hood| < 0.05 rad + |delta exitVel| < 0.5 rad/s + |delta predPos| < 5mm + |delta TOF| < 5ms + 5. If converged, break early. +``` + +`vxLaunch` and `vyLaunch` are the actual velocity of the turret pivot point, accounting for both robot linear velocity and rotational velocity (the turret pivot is offset from the robot center, so rotation contributes a tangential component). + +The drag compensation is tunable via `/ShotCalc/sotmDragCoeff`. Setting it to 0 disables drag compensation entirely. + +### Physics Utilities + +**`calculateAngleFromVelocity()`**: projectile motion formula to find launch angle from exit speed and distance. Uses the full two-solution quadratic (`vel^4` under the root), picking the low-angle solution. + +**`calculateTimeOfFlightSec()`**: simple horizontal flight time: +``` +tof = distance / (velocity * cos(launchAngle)) +where launchAngle = pi/2 - hoodAngle +``` + +**`calculateShotFromFunnelClearance()`**: 2D trajectory solver that finds the velocity and angle needed for a ball to simultaneously clear a funnel above the robot *and* land in the target funnel. Solves two simultaneous linear equations derived from parabolic motion, no iteration needed, closed-form algebra. See the Desmos link in the comments if you want to visualize it. + +**`getDistanceToTargetM()`**: zero-allocation hot-path version: takes raw doubles, manually applies the turret offset transform using precomputed `cos/sin`, returns Euclidean distance. Used everywhere on the critical path. + +### Data Records + +**`ShotData`**: base shot parameters +- `exitVelocity`: rad/s +- `hoodAngle`: radians +- `target`: `Translation3d` of the aim point + +**`ShotDataLerp`**: extends `ShotData` with `tofSec`, the time of flight in seconds from the LERP table. + +Both records have `acceptLogging()` methods that push their fields to NetworkTables. + +### New Fuel Adjustment *(currently disabled)* + +`kRPSReductionNeeded = false`. When enabled, a linear regression over `kReductionDistances`/`kReductionAmount` would compute a per-distance RPS reduction applied during table building. The idea was to slightly back off flywheel speed for certain fuel states. Not active. + +--- + +## Tuning Reference + +| NT Key | Default | What It Does | +|--------|---------|-------------| +| `/ShotCalc/sotmDragCoeff` | 0.5 | Drag coefficient for SOTM lateral compensation | +| `/ShotCalc/lateralBiasGainRots` | `kTurretLateralBiasGainRots` | Sinusoidal lateral bias correction gain for the turret| +| `Shooter/Calculator/RPSBoost` | 0.75 | Boost applied to passing table RPS | + +All tunable via `WaltTunable`, enable in NT and set a value. Changes apply to the next calc cycle. + +--- + +## Quick Data Flow +```mermaid +graph TD + Hz[calcCallback @ 75 Hz] --> P1 + + subgraph Calculation Thread + P1[calculateTarget] --> O1["m_aimTarget (volatile)"] + O1 --> P2 + + subgraph P2 [iterativeMovingShotFromInt] + P2a[ShotLerpTable lookup] --> P2b[convergence loop] + P2b --> P2c[dragCompensatedTOF] + end + + P2 --> O2["ShotDataLerp + flywheel vel | hood angle + predicted target | TOF"] + + O2 --> P3[calcAzimuth] + P3 --> O3["AzimuthCalcDetails + turret setpoint | velocity FF"] + end + + O3 --> O4["ShotCalcOutputs (volatile)"] + O4 --> P4["Shooter.periodic()"] +``` diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java b/src/main/java/frc/robot/subsystems/shooter/calc/ShotCalcMath.java similarity index 67% rename from src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java rename to src/main/java/frc/robot/subsystems/shooter/calc/ShotCalcMath.java index 117de745..68d47fe9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java +++ b/src/main/java/frc/robot/subsystems/shooter/calc/ShotCalcMath.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.shooter; +package frc.robot.subsystems.shooter.calc; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -13,9 +13,6 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.networktables.BooleanPublisher; -import edu.wpi.first.networktables.BooleanTopic; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -24,7 +21,7 @@ import frc.robot.Constants.WpiK; import frc.robot.generated.TunerConstants; import frc.robot.FieldConstants; -import frc.robot.subsystems.shooter.ShotCalculator.ShotDataLerp; +import frc.robot.subsystems.shooter.calc.ShotCalculator.ShotDataLerp; import frc.util.AllianceFlipUtil; import frc.util.AllianceZoneUtil; import frc.util.WaltLogger; @@ -38,15 +35,48 @@ import frc.util.WaltTunable; -public class ShooterCalc { +/* + * ShotCalcMath + * + * All the shot math runs here, on a background thread via WPILib's Notifier, + * so it never eats into the main 50 Hz robot loop. The thread runs at 75 Hz + * and drops the result into a volatile field. Shooter.periodic() just reads + * that field every loop, no locking needed. + * + * Quick vocabulary: + * SOTM (Shoot On The Move) - the iterative algorithm that compensates for + * the robot moving while the ball is in the air. If we're driving right + * at 1 m/s and the ball takes 0.3 s to reach the hub, we aim a bit left. + * Volatile - Java keyword that tells the JVM other threads may write this. + * Without it, the main thread might see a stale cached value. + * Snapback - when the turret is near a hardstop and the target wraps to the + * other side, we add +-1 full rotation to the setpoint so the turret goes + * the short way around instead of slamming into the limit. + * + * One important rule: Shooter.periodic() caches the turret position into a + * volatile double at the top of every loop. ShotCalcMath reads it from the + * background thread. Don't move that line or the thread safety breaks. + */ +public class ShotCalcMath { private static final String kLogTab = "ShotCalc"; + + // Tunable lateral bias correction. Balls curve left/right depending on which way + // the turret faces (sinusoidal - no correction straight ahead, max at 90 degrees). + // Enable and tune via /ShotCalc/lateralBiasGainRots in NetworkTables. private static final WaltTunable kLateralBiasTuner = new WaltTunable("/ShotCalc/lateralBiasGainRots", kTurretLateralBiasGainRots); + // ---- INPUTS ---- + // Read every callback cycle. SwerveDriveState comes through a thread-safe supplier + // from CTRE's odometry thread. Turret position is written by Shooter.periodic() into + // a volatile field and read here. + private final Supplier m_threadsafeSwerveDriveStateSup; private final DoubleSupplier m_turretPosRotsSup; private final SwerveDriveKinematics m_swerveKinematics = new SwerveDriveKinematics(TunerConstants.moduleTranslations); + // ---- LOGGERS ---- + private final Pose3dLogger log_globalShotTarget = WaltLogger.logPose3d(kLogTab, "globalTarget"); // private final Pose3dLogger log_calculatedShotTarget = WaltLogger.logPose3d(kLogTab, "shotCalcTarget"); private final DoubleLogger log_rawDesiredTurretRot = WaltLogger.logDouble(kLogTab, "rawDesiredTurretRots"); @@ -60,11 +90,11 @@ public class ShooterCalc { // private static final Pose3dLogger log_turretFieldPose = WaltLogger.logPose3d(kLogTab, "turretFieldPose"); private final BooleanLogger log_robotPastOurZoneX = WaltLogger.logBoolean(kLogTab, "robotInOurZone"); private final BooleanLogger log_robotInHubPassingZone = WaltLogger.logBoolean(kLogTab, "robotInHubPassingZone"); - private final BooleanLogger log_canTurretShoot = WaltLogger.logBoolean(kLogTab, "canTurretShoot"); - private final BooleanLogger log_inTrenchZone = WaltLogger.logBoolean(kLogTab, "inTrenchZone"); - private final BooleanLogger log_underTrench = WaltLogger.logBoolean(kLogTab, "underTrench"); - // Precomputed doubles for calculateTarget zone checks + // ---- PRECOMPUTED CONSTANTS ---- + // Calculated once at startup and reused every callback to skip redundant lookups. + + // Hub X positions for the zone-boundary check in calculateTarget() private static final double kRedHubCenterX = AllianceZoneUtil.redHubCenter.getX(); private static final double kBlueHubCenterX = AllianceZoneUtil.blueHubCenter.getX(); private static final double kCenterFieldYM = AllianceZoneUtil.centerField_y_pos.baseUnitMagnitude(); @@ -74,49 +104,64 @@ public class ShooterCalc { new Translation3d(ShooterK.kPassingXAsDouble, FieldConstants.fieldWidth - 2, 0); private static final Translation3d kRightPassTarget = new Translation3d(ShooterK.kPassingXAsDouble, 2, 0); - + // private static final Translation3d kLeftPassPastHubTarget = // new Translation3d(ShooterK.kPassingXAsDouble + 0.5, FieldConstants.fieldWidth - 1.5, 0); // private static final Translation3d kRightPassPastHubTarget = // new Translation3d(ShooterK.kPassingXAsDouble + 0.5, 1.5, 0); - // Pre-allocated ballTrajectory log buffer — reused each callback to avoid array allocation + // Pre-allocated log buffer so we don't create a new Translation3d[] every callback private static final Translation3d[] m_ballTrajBuffer = new Translation3d[2]; - // Precomputed doubles for hot-path unit conversions + // Turret limits as raw doubles to skip .in(Rotations) calls on the hot path private static final double kTurretMinRotsD = kTurretMinRots.in(Rotations); private static final double kTurretMinRotsMagnitudeD = kTurretMinRots.magnitude(); private static final double kTurretMaxRotsD = kTurretMaxRots.in(Rotations); private static final double kTurretMaxRotsMagnitudeD = kTurretMaxRots.magnitude(); - private final ShotDataLerp kEmptyShotData = new ShotDataLerp(0.0, 0.0, new Translation3d(), 0.0); - private final AzimuthCalcDetails kEmptyAzimuthCalcDetails = new AzimuthCalcDetails(0, 0, 0, 0, 0, 0, 0); - private final ShotCalcOutputs kEmptyShotCalcOutputs = new ShotCalcOutputs(kEmptyAzimuthCalcDetails, kEmptyShotData, 0, 0, 0); + // ---- VOLATILE STATE ---- + // Written by the Notifier thread, read by the main thread. + // volatile means the main thread always reads the latest write, no lock needed. private volatile boolean m_useStaticShot = true; private static volatile Translation3d m_aimTarget = Translation3d.kZero; - private volatile ShotCalcOutputs m_shotCalcOutputs = kEmptyShotCalcOutputs; - private static volatile boolean m_isPassingFlag = false; + private volatile ShotCalcOutputs m_shotCalcOutputs; + private static volatile boolean m_isPassingFlag = false; // true when robot is past the hub private static final BooleanSupplier m_isPassing = () -> m_isPassingFlag; - private static volatile boolean m_canTurretShoot = false; - private static volatile boolean m_underTrench = false; + + // ---- THREAD + TIMER ---- private final Notifier m_notifier = new Notifier(this::calcCallback); - private final Timer m_calcTimer = new Timer(); + private final Timer m_calcTimer = new Timer(); // how long each callback takes + + // ---- RUNTIME STATE ---- private boolean isRed = WaltDriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red; - // private double robotX; - // private double robotY; - private boolean robotInNoPassingZone; + private boolean robotInNoPassingZone; // true when near opponent hub in the no-pass zone + - public ShooterCalc(Supplier threadsafeSwerveDriveStateSup, DoubleSupplier turretPosSup) { + // ============================================================= + // CONSTRUCTOR + // ============================================================= + + public ShotCalcMath(Supplier threadsafeSwerveDriveStateSup, DoubleSupplier turretPosSup) { m_threadsafeSwerveDriveStateSup = threadsafeSwerveDriveStateSup; m_turretPosRotsSup = turretPosSup; - m_notifier.setName("ShooterCalc"); + // Initialize with empty values so Shooter.periodic() never gets null back + ShotDataLerp emptyShotData = new ShotDataLerp(0.0, 0.0, new Translation3d(), 0.0); + AzimuthCalcDetails emptyAzimuth = new AzimuthCalcDetails(0, 0, 0, 0, 0, 0, 0); + m_shotCalcOutputs = new ShotCalcOutputs(emptyAzimuth, emptyShotData, 0, 0, 0); + + m_notifier.setName("ShotCalcMath"); m_notifier.startPeriodic(Hertz.of(75)); // 2x slower than robot loop } + + // ============================================================= + // PUBLIC API + // ============================================================= + public void shouldUseStaticShot(boolean should) { m_useStaticShot = should; } @@ -125,43 +170,26 @@ public static Translation3d getLatestAimTarget() { return m_aimTarget; } + // Called by Shooter.periodic() every loop to grab the latest shot parameters. + // Reading one volatile reference is about as cheap as it gets. public ShotCalcOutputs getLatestShotCalcOutputs() { return m_shotCalcOutputs; } + // ShotCalculator reads this to pick kShotTable vs kPassingTable without + // needing to hold a reference back to this class. public static BooleanSupplier isPassing() { return m_isPassing; } - public static boolean canTurretShoot() { - return m_canTurretShoot; - } - - public static boolean getUnderTrench() { - return m_underTrench; - } - - /** - * first checks if we're passing. If we're not passing, then we can shoot whenever - * If we are passing, it checks if we're NOT in the unable-to-pass range. If we're not in it, then the turret can shoot! - * - * lowk should be checking if we are IN THE RANGE rather than greater than the outsides, but this is cope for now cuz sadness - */ - private void refreshCanTurretShoot() { - if (isPassing().getAsBoolean()) { - if (m_turretPosRotsSup.getAsDouble() > kTurretMinNotAbleToPassRange && m_turretPosRotsSup.getAsDouble() < kTurretMaxNotAbleToPassRange/* && !robotInNoPassingZone */) { - m_canTurretShoot = true; - } else { - m_canTurretShoot = false; - } - log_canTurretShoot.accept(m_canTurretShoot); - } else { - m_canTurretShoot = true; //the turret can shoot anytime we are not passing - log_canTurretShoot.accept(m_canTurretShoot); - } - } + // ============================================================= + // BACKGROUND THREAD LOOP + // ============================================================= + // Runs at 75 Hz on the Notifier's background thread. + // calculateTarget() must go first because it sets m_isPassingFlag, + // which calcShot() reads through ShotCalculator.isPassing() to pick the right LERP table. private void calcCallback() { m_calcTimer.restart(); // getters from outside @@ -170,12 +198,9 @@ private void calcCallback() { ChassisSpeeds robotChassisSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( m_swerveKinematics.toChassisSpeeds(swerveState.ModuleStates), robotPose.getRotation()); double turretPositionRots = m_turretPosRotsSup.getAsDouble(); - Pose3d turretPose = new Pose3d(robotPose).transformBy(kTurretTransform); - m_underTrench = underTrench(turretPose.toPose2d()); m_aimTarget = calculateTarget(robotPose); m_shotCalcOutputs = calcShot(robotPose, m_useStaticShot, m_aimTarget, turretPositionRots, robotChassisSpeeds); - refreshCanTurretShoot(); // Logging log_globalShotTarget.accept(m_aimTarget); @@ -191,11 +216,25 @@ private void calcCallback() { log_loopTime.accept(m_calcTimer.get() * 1000.0); } + + // ============================================================= + // TARGET SELECTION + // ============================================================= + + // Picks which field target to aim at based on where the robot is. + // + // Robot on own side of hub -> shoot at the enemy hub center + // Robot past the hub into neutral zone -> pass to a passing point + // about where the bump is, left or right depending on which side of field center + // + // m_isPassingFlag is set here and read by ShotCalculator to pick kPassingTable. + // robotInNoPassingZone tracks when passing angles near the opponent hub are unsafe. + /** * Sets the target to a Pose on the field relative to where the robot is. * EX: Robot in alliance zone red -> Red Hub Center * Executes Passing and Shooting aiming. - * + * * @param robotPose where the robot currently is * @return target pose */ @@ -230,61 +269,58 @@ private Translation3d calculateTarget(Pose2d robotPose) { return AllianceFlipUtil.apply(theTarget); } - private boolean underTrench(Pose2d turretPose) { - isRed = WaltDriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red; + // private boolean underTrench(Pose2d turretPose) { + // isRed = WaltDriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red; - double robotX = turretPose.getX(); - double robotY = turretPose.getY(); + // double robotX = turretPose.getX(); + // double robotY = turretPose.getY(); - boolean inTrenchZone = isRed - ? (robotY < FieldConstants.LinesHorizontal.rightTrenchOpenStart || robotY > FieldConstants.LinesHorizontal.leftTrenchOpenEnd) - : (robotY > FieldConstants.LinesHorizontal.rightTrenchOpenStart || robotY < FieldConstants.LinesHorizontal.leftTrenchOpenEnd); + // boolean inTrenchZone = isRed + // ? (robotY < FieldConstants.LinesHorizontal.rightTrenchOpenStart || robotY > FieldConstants.LinesHorizontal.leftTrenchOpenEnd) + // : (robotY > FieldConstants.LinesHorizontal.rightTrenchOpenStart || robotY < FieldConstants.LinesHorizontal.leftTrenchOpenEnd); - double trenchCenterX = isRed - ? FieldConstants.LinesVertical.oppHubCenter - : FieldConstants.LinesVertical.hubCenter; - double trenchHalfDepth = FieldConstants.LeftTrench.depth / 2.0; - boolean inTrenchX = Math.abs(robotX - trenchCenterX) < trenchHalfDepth + 0.3; // 0.3m buffer + // double trenchCenterX = isRed + // ? FieldConstants.LinesVertical.oppHubCenter + // : FieldConstants.LinesVertical.hubCenter; + // double trenchHalfDepth = FieldConstants.LeftTrench.depth / 2.0; + // boolean inTrenchX = Math.abs(robotX - trenchCenterX) < trenchHalfDepth + 0.3; // 0.3m buffer - log_inTrenchZone.accept(inTrenchZone); + // log_inTrenchZone.accept(inTrenchZone); - log_underTrench.accept(inTrenchZone && inTrenchX); + // log_underTrench.accept(inTrenchZone && inTrenchX); - if (inTrenchZone && inTrenchX) { - return true; - } else { - return false; - } - } + // if (inTrenchZone && inTrenchX) { + // return true; + // } else { + // return false; + // } + // } - public record AzimuthCalcDetails( - double turretReferenceRots, double turretVelocityFF, - double turretX, double turretY, - double fieldYawRad, double currentFieldYawRad, - double rawDesiredRotations - ) { - private static final String kCalcTab = "/AzimuthCalcDetails"; - private static final DoubleLogger log_turretReferenceRots = new DoubleLogger(kLogTab + kCalcTab, "turretReferenceRots"); - private static final DoubleLogger log_turretVelocityFF = new DoubleLogger(kLogTab + kCalcTab, "turretVelocityFF"); - private static final DoubleLogger log_turretX = new DoubleLogger(kLogTab + kCalcTab, "turretX"); - private static final DoubleLogger log_turretY = new DoubleLogger(kLogTab + kCalcTab, "turretY"); - private static final DoubleLogger log_fieldYawRad = new DoubleLogger(kLogTab + kCalcTab, "fieldYawRad"); - private static final DoubleLogger log_currentFieldYawRad = new DoubleLogger(kLogTab + kCalcTab, "currentFieldYawRad"); - private static final DoubleLogger log_rawDesiredRotations = new DoubleLogger(kLogTab + kCalcTab, "rawDesiredRotations"); - - public void acceptLogging(AzimuthCalcDetails details) { - log_turretReferenceRots.accept(details.turretReferenceRots); - log_turretVelocityFF.accept(details.turretVelocityFF); - log_turretX.accept(details.turretX); - log_turretY.accept(details.turretY); - log_fieldYawRad.accept(details.fieldYawRad); - log_currentFieldYawRad.accept(details.currentFieldYawRad); - log_rawDesiredRotations.accept(details.rawDesiredRotations); - } - } + // ============================================================= + // SHOT + AZIMUTH CALCULATION + // ============================================================= + // Precomputed for calcAzimuth — skip calling kTurretTransform.getTranslation().getZ() every cycle + private static final double kTurretOffsetZ_m = kTurretTransform.getTranslation().getZ(); + // private static final Rotation3d kTurretRealPoseRotation = + // new Rotation3d(0, 0, -(kTurretAngleOffset.plus(Rotation2d.kPi)).getRadians()); + + /** + * + * Computes the turret setpoint in rotations and velocity feedforward. + * Uses raw doubles to avoid Pose3d/Rotation2d allocations on the hot path. + * + * What it does: + * - Computes the turret pivot position in field coords from robot heading + physical offset + * - atan2 from pivot to target gives the desired field yaw + * - Subtracts turret zero direction, normalizes to [-0.5, 0.5] rotations + * - Applies sinusoidal lateral bias correction if enabled + * - Snapback: if near a soft limit, shifts the setpoint +-1 rotation so the + * turret wraps the short way around instead of crossing the hardstop + * - Velocity FF from tangential velocity of the target relative to the pivot + * * Calculates the turret's *TARGET* angle while ensuring it stays within * physical limits. * IF the turret is near a limit, snaps 360 degrees in the opposite direction to @@ -292,17 +328,12 @@ public void acceptLogging(AzimuthCalcDetails details) { * without hitting the hardstop. * Note that if you have less than 360 degrees on the turret, you will simply * snap back to the other hard limit. - * + * * @param target target position * @return safe rotation setpoint that is accurate to the target within bounds * of kTurretMaxAngle * and kTurretMinAngle */ - // Precomputed for calcAzimuth logging - private static final double kTurretOffsetZ_m = kTurretTransform.getTranslation().getZ(); - // private static final Rotation3d kTurretRealPoseRotation = - // new Rotation3d(0, 0, -(kTurretAngleOffset.plus(Rotation2d.kPi)).getRadians()); - public static AzimuthCalcDetails calcAzimuth(Translation3d target, Pose2d robotPose, double turretHeading, ChassisSpeeds fieldSpeeds) { // Compute turret pivot position and zero direction with raw doubles // (eliminates Pose3d(robotPose).transformBy() + Rotation2d allocations) @@ -379,30 +410,10 @@ public static AzimuthCalcDetails calcAzimuth(Translation3d target, Pose2d robotP return calcDetails; } - public final record ShotCalcOutputs( - AzimuthCalcDetails turretCalcDetails, - ShotDataLerp shotData, - double turretReferenceRots, - double hoodReferenceRots, - double shooterReferenceRps - ) { - private static final String kCalcTab = "/ShotCalcOutputs"; - - private static final DoubleLogger log_turretReferenceRots = new DoubleLogger(kLogTab + kCalcTab, "turretReferenceRots"); - private static final DoubleLogger log_hoodReferenceRots = new DoubleLogger(kLogTab + kCalcTab, "hoodReferenceRots"); - private static final DoubleLogger log_shooterReferenceRPS = new DoubleLogger(kLogTab + kCalcTab, "shooterReferenceRPS"); - - public void acceptLogging(ShotCalcOutputs outputs) { - log_turretReferenceRots.accept(outputs.turretReferenceRots); - log_hoodReferenceRots.accept(outputs.hoodReferenceRots); - log_shooterReferenceRPS.accept(outputs.shooterReferenceRps); - } - } - /** * Calculates the ideal shot to put the FUEL™ into the HUB™ * Accounts for moving speeds - * + * * @param robotPose current Robot position. */ public static ShotCalcOutputs calcShot( @@ -430,4 +441,81 @@ public static ShotCalcOutputs calcShot( outputs.acceptLogging(outputs); return outputs; } + + + // ============================================================= + // OUTPUT RECORDS + // ============================================================= + + /* + * AzimuthCalcDetails + * + * Everything calcAzimuth() knows about where the turret should point. + * This gets passed up through ShotCalcOutputs to Shooter.periodic(). + * + * turretReferenceRots - final snapback-safe setpoint in rotations + * turretVelocityFF - angular velocity feedforward in rad/s + * turretX / turretY - turret pivot in field coords (for logging) + * fieldYawRad - desired absolute yaw in radians + * currentFieldYawRad - current turret yaw in field coords (shows tracking lag) + * rawDesiredRotations - pre-snapback angle, useful for debugging wrap-around + */ + public record AzimuthCalcDetails( + double turretReferenceRots, double turretVelocityFF, + double turretX, double turretY, + double fieldYawRad, double currentFieldYawRad, + double rawDesiredRotations + ) { + private static final String kCalcTab = "/AzimuthCalcDetails"; + + private static final DoubleLogger log_turretReferenceRots = new DoubleLogger(kLogTab + kCalcTab, "turretReferenceRots"); + private static final DoubleLogger log_turretVelocityFF = new DoubleLogger(kLogTab + kCalcTab, "turretVelocityFF"); + private static final DoubleLogger log_turretX = new DoubleLogger(kLogTab + kCalcTab, "turretX"); + private static final DoubleLogger log_turretY = new DoubleLogger(kLogTab + kCalcTab, "turretY"); + private static final DoubleLogger log_fieldYawRad = new DoubleLogger(kLogTab + kCalcTab, "fieldYawRad"); + private static final DoubleLogger log_currentFieldYawRad = new DoubleLogger(kLogTab + kCalcTab, "currentFieldYawRad"); + private static final DoubleLogger log_rawDesiredRotations = new DoubleLogger(kLogTab + kCalcTab, "rawDesiredRotations"); + + public void acceptLogging(AzimuthCalcDetails details) { + log_turretReferenceRots.accept(details.turretReferenceRots); + log_turretVelocityFF.accept(details.turretVelocityFF); + log_turretX.accept(details.turretX); + log_turretY.accept(details.turretY); + log_fieldYawRad.accept(details.fieldYawRad); + log_currentFieldYawRad.accept(details.currentFieldYawRad); + log_rawDesiredRotations.accept(details.rawDesiredRotations); + } + } + + /* + * ShotCalcOutputs + * + * The full package of parameters Shooter.periodic() needs to drive the mechanism. + * Produced once per 75 Hz callback and stored in a volatile field. + * + * turretCalcDetails - turret angle + feedforward (from calcAzimuth) + * shotData - flywheel speed, hood angle, TOF, predicted target (from ShotCalculator) + * turretReferenceRots - pulled from turretCalcDetails for convenience + * hoodReferenceRots - hood angle converted from radians to rotations + * shooterReferenceRps - flywheel speed converted from rad/s to RPS + */ + public final record ShotCalcOutputs( + AzimuthCalcDetails turretCalcDetails, + ShotDataLerp shotData, + double turretReferenceRots, + double hoodReferenceRots, + double shooterReferenceRps + ) { + private static final String kCalcTab = "/ShotCalcOutputs"; + + private static final DoubleLogger log_turretReferenceRots = new DoubleLogger(kLogTab + kCalcTab, "turretReferenceRots"); + private static final DoubleLogger log_hoodReferenceRots = new DoubleLogger(kLogTab + kCalcTab, "hoodReferenceRots"); + private static final DoubleLogger log_shooterReferenceRPS = new DoubleLogger(kLogTab + kCalcTab, "shooterReferenceRPS"); + + public void acceptLogging(ShotCalcOutputs outputs) { + log_turretReferenceRots.accept(outputs.turretReferenceRots); + log_hoodReferenceRots.accept(outputs.hoodReferenceRots); + log_shooterReferenceRPS.accept(outputs.shooterReferenceRps); + } + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/calc/ShotCalculator.java similarity index 84% rename from src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java rename to src/main/java/frc/robot/subsystems/shooter/calc/ShotCalculator.java index c93cb777..4161941c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/calc/ShotCalculator.java @@ -1,5 +1,5 @@ -package frc.robot.subsystems.shooter; +package frc.robot.subsystems.shooter.calc; import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.Meters; @@ -28,23 +28,43 @@ import java.util.DoubleSummaryStatistics; import java.util.TreeMap; +/* + * ShotCalculator - pure ballistics, no threads, no state. + * + * Everything here is static methods or records. The two main entry points: + * - iterativeMovingShotFromInterpolationMap: the SOTM workhorse - converges on params that account for robot velocity during ball flight + * - calculateShotFromFunnelClearance: 2D closed-form solver for clearing a funnel above the robot + * + * ShotLerpTable is the zero-allocation sorted-array replacement for InterpolatingDoubleTreeMap. + * Tables store SI units internally (rad/s, radians, seconds). You put in rot/s and rotations via the Builder. + * + * Vocab: + * SOTM - Shoot On The Move. iterative convergence that adjusts for robot velocity during ball flight. + * TOF - Time of Flight. how long the ball is in the air. + * drift - lateral ball displacement during flight. drag-compensated via (1 - e^(-c*t)) / c. + * passing - shooting at the passing target instead of the hub. selects kPassingTable. + */ public class ShotCalculator { + // ---- LOGGERS ---- private static final DoubleLogger log_distToTargetMeters = new DoubleLogger("Shooter/Calculator", "distTargetToMeters"); private static final BooleanLogger log_isPassingLerp = new BooleanLogger("Shooter/Calculator", "isPassingLERP"); - private static final DoubleLogger log_dragCoefficient = new DoubleLogger("Shooter/Calculator", "dragCoefficient"); private static final IntLogger log_lerpIterationCount = new IntLogger("Shooter/Calculator", "lerpIterationCount"); private static final BooleanLogger log_calcConvergedBreakout = new BooleanLogger("Shooter/Calculator", "calcConvergedBreakout"); + // ---- TUNING ---- private static final double kMetersToInches = 1.0 / 0.0254; - // Horizontal drag damping: actual drift = v * (1 - e^(-c*t)) / c < v*t - // c = 0 disables drag compensation. Enable via /ShotCalc/sotmDragCoeff/enabled. - //YAYAYAYYAYAYAY 2974 IS OUR LUCKY NUMBER HUZZAH YAY YIPPEE private static final WaltTunable kDragCoeffTuner = new WaltTunable("/ShotCalc/sotmDragCoeff", 0.5000, false); // private static final Tracer m_iterativeTracer = new Tracer(); - // private static final double kRedHubCenterX = AllianceZoneUtil.redHubCenter.getX(); - // private static final double kBlueHubCenterX = AllianceZoneUtil.blueHubCenter.getX(); + private static final double kRPSBoost = 0.75; + private static final double kLongRangeRPSBoost = 0.35; + private static final double kScoringRPSBoost = -0.2; + //NOTE THAT THIS IS NOT TUNING THE VALUES IN THE TABLE -- IT IS ON TOP OF THE REST OF THE ADDITIONS + private static final WaltTunable kRPSOverallBoostTuner = new WaltTunable("Shooter/Calculator/RPSBoostDelta", 0.0); + + // ---- TABLE CONSTANTS ---- + // kReductionDistances/kReductionAmount feed the linear regression in calcRPSReduction - currently unused (kRPSReductionNeeded = false) private static final double[] kReductionDistances = {1.48, 2.31, 4.12}; private static final double[] kReductionAmount = {0, 2, 4}; @@ -56,12 +76,7 @@ public class ShotCalculator { private static final boolean kRPSReductionNeeded = false; - private static double kRPSBoost = 0.75; - private static double kLongRangeRPSBoost = 0.35; - - private static double kScoringRPSBoost = -0.2; - private static final WaltTunable kRPSBoostTuner = new WaltTunable("Shooter/Calculator/RPSBoost", kRPSBoost); - + // ---- LERP TABLES ---- /** * Zero-allocation sorted-array interpolation tables replacing InterpolatingTreeMap. * All values stored in SI units (rad/s, radians, seconds). @@ -76,33 +91,17 @@ private static void addNewFuelAdjPoint(double distance) { kNewFuelAdjTable.put(distance, calcRPSReduction(distance)); } + // ============================================================= + // TABLE INITIALIZATION + // ============================================================= + static { //TODO: find the actual minDistance and maxDistance for shooting - minDistance = 1.168; - maxDistance = 5.672; - - kRPSBoost = kRPSBoostTuner.enabled() ? kRPSBoostTuner.get() : kRPSBoost; - + minDistance = 0.985; + maxDistance = 8.627; ShotLerpTable.Builder shot = new ShotLerpTable.Builder(); - // normal table - // shot.add(8.627, 69.000, 1.160, 1.65, 0.500); - // shot.add(7.801, 64.700, 1.134, 1.37, 0.500); - // shot.add(6.973, 62.300, 1.104, 1.33, 0.500); - // shot.add(6.126, 58.000, 1.071, 1.33, 0.500); - // shot.add(5.577, 57.600, 1.046, 1.19, 0.500); - // shot.add(4.555, 54.200, 0.994, 1.12, 0.500); - // shot.add(4.231, 54.400, 0.974, 1.09, 0.500); - // shot.add(3.798, 51.900, 0.852, 1.08, 0.500); - // shot.add(3.267, 47.600, 0.907, 0.95, 0.500); - // shot.add(2.826, 46.400, 0.869, 0.93, 0.500); - // shot.add(2.212, 42.500, 0.608, 0.98, 0.500); - // shot.add(1.929, 41.700, 0.500, 0.87, 0.500); - // shot.add(1.093, 43.450, 0.000, 1.02, 0.500); - // shot.add(0.985, 40.000, 0.000, 0.97, 0.500); - - //spoof table shot.add(8.627, 69.000 + kScoringRPSBoost, 1.160, 1.65, 0.500); shot.add(7.801, 65.865 + kScoringRPSBoost, 1.106, 1.37, 0.500); //1.524 shot.add(6.973, 62.723 + kScoringRPSBoost, 1.046, 1.33, 0.500); //1.411 @@ -167,47 +166,10 @@ private static void addNewFuelAdjPoint(double distance) { passing.add(14.355, 104.39 + kRPSBoost + kLongRangeRPSBoost, 1.16, 2.08, 0.254); kPassingTable = passing.build(); } - static { - //THIS IS ONLY USED IF THE TURRET IS IN A POSITION THAT IS UNABLE TO SHOOT WITH HOOD UP DURING PASSING - //NO ANGRY PASSING IN OPPOSING ALLIANCE ZONE - // ShotLerpTable.Builder angry = new ShotLerpTable.Builder(); - // angry.add(7.574, 92.5, 0.08, 2.08); - // angry.add(7.135, 86, 0.08, 2.06); - // angry.add(6.653, 81, 0.08, 2.04); - // angry.add(6.254, 78, 0.08, 2.02); - // //note that the points above have spoofed tofs - // angry.add(5.672, 82.50 - kRPSReduction, 0.08, 2.11); - // angry.add(5.321, 81.00 - kRPSReduction, 0.08, 1.94); - // angry.add(5.223, 75.75 - kRPSReduction, 0.08, 1.79); - // angry.add(5.167, 79.50 - kRPSReduction, 0.08, 2.03); - // angry.add(4.996, 78.00 - kRPSReduction, 0.08, 1.97); - // angry.add(4.869, 76.50 - kRPSReduction, 0.08, 1.80); - // angry.add(4.696, 75.00 - kRPSReduction, 0.08, 1.33); - // angry.add(4.546, 73.50 - kRPSReduction, 0.08, 1.83); - // angry.add(4.402, 72.50 - kRPSReduction, 0.08, 1.85); - // angry.add(4.258, 71.00 - kRPSReduction, 0.08, 1.64); - // angry.add(4.098, 69.00 - kRPSReduction, 0.08, 1.58); - // angry.add(3.932, 68.00 - kRPSReduction, 0.08, 1.71); - // angry.add(3.785, 66.90 - kRPSReduction, 0.08, 1.56); - // angry.add(3.611, 65.40 - kRPSReduction, 0.08, 1.56); - // angry.add(3.464, 63.90 - kRPSReduction, 0.08, 1.50); - // angry.add(3.293, 62.40 - kRPSReduction, 0.08, 1.50); - // angry.add(3.134, 60.90 - kRPSReduction, 0.08, 1.51); - // angry.add(2.995, 59.40 - kRPSReduction, 0.08, 1.37); - // angry.add(2.855, 57.90 - kRPSReduction, 0.08, 1.35); - // angry.add(2.704, 56.40 - kRPSReduction, 0.08, 1.38); - // angry.add(2.586, 54.90 - kRPSReduction, 0.08, 1.28); - // angry.add(2.395, 53.50 - kRPSReduction, 0.08, 1.29); - // angry.add(2.221, 52.00 - kRPSReduction, 0.08, 1.25); - // angry.add(2.083, 50.50 - kRPSReduction, 0.08, 1.14); - // angry.add(1.912, 49.00 - kRPSReduction, 0.08, 1.19); - // angry.add(1.691, 47.50 - kRPSReduction, 0.08, 0.98); - // angry.add(1.575, 47.50 - kRPSReduction, 0.08, 1.18); - // angry.add(1.528, 46.00 - kRPSReduction, 0.08, 1.20); - // angry.add(1.307, 44.50 - kRPSReduction, 0.08, 1.13); - // angry.add(1.168, 44.50 - kRPSReduction, 0.08, 1.16); - // kAngryTurretTable = angry.build(); - } + + // ============================================================= + // DISTANCE + VELOCITY UTILITIES + // ============================================================= /** * @param distance @@ -231,7 +193,7 @@ public static double calcRPSReduction(double distance) { } r /= (kReductionAmount.length - 1); - + double slope = r * (reductionSTDev/distanceSTDev); double intercept = reductionSummaryStats.getAverage() - slope * distanceSummaryStats.getAverage(); @@ -256,7 +218,7 @@ public static double getDistanceToTargetM(double robotX, double robotY, double r /** * Gets the Distance from current robot position to desired target. - * Allocates Pose3d/Distance — use {@link #getDistanceToTargetM} on hot paths. + * Allocates Pose3d/Distance - use {@link #getDistanceToTargetM} on hot paths. */ public static Distance getDistanceToTarget(Pose2d robot, Translation3d target) { Pose3d turretPose = new Pose3d(robot).transformBy(kTurretTransform); @@ -266,6 +228,28 @@ public static Distance getDistanceToTarget(Pose2d robot, Translation3d target) { return Meters.of(dist); } + public static AngularVelocity linearToAngularVelocity(LinearVelocity vel, Distance radius) { + return RadiansPerSecond.of(vel.in(MetersPerSecond) / radius.in(Meters)); + } + + public static LinearVelocity angularToLinearVelocity(AngularVelocity vel, Distance radius) { + return MetersPerSecond.of(vel.in(RadiansPerSecond) * radius.in(Meters)); + } + + /** Raw double: rad/s from m/s and radius in meters. */ + public static double linearToAngularVelocityRadPerSec(double mps, double radiusM) { + return mps / radiusM; + } + + /** Raw double: m/s from rad/s and radius in meters. */ + public static double angularToLinearVelocityMps(double radPerSec, double radiusM) { + return radPerSec * radiusM; + } + + // ============================================================= + // TRAJECTORY PHYSICS + // ============================================================= + // see https://www.desmos.com/geometry/l4edywkmha public static Angle calculateAngleFromVelocity(Pose2d robot, LinearVelocity velocity, Translation3d target) { @@ -298,28 +282,9 @@ public static double calculateTimeOfFlightSec(double velMps, double hoodAngleRad return tofSec; } - public static AngularVelocity linearToAngularVelocity(LinearVelocity vel, Distance radius) { - return RadiansPerSecond.of(vel.in(MetersPerSecond) / radius.in(Meters)); - } - - public static LinearVelocity angularToLinearVelocity(AngularVelocity vel, Distance radius) { - return MetersPerSecond.of(vel.in(RadiansPerSecond) * radius.in(Meters)); - } - - /** Raw double: rad/s from m/s and radius in meters. */ - public static double linearToAngularVelocityRadPerSec(double mps, double radiusM) { - return mps / radiusM; - } - - /** Raw double: m/s from rad/s and radius in meters. */ - public static double angularToLinearVelocityMps(double radPerSec, double radiusM) { - return radPerSec * radiusM; - } - /** Returns drag-compensated drift time: (1 - e^(-c*t)) / c, or t if drag is disabled. */ private static double dragCompensatedTOF(double tof, double dragCoeff) { - // if (!kDragCoeffTuner.enabled()) return tof; - double c = kDragCoeffTuner.enabled() ? kDragCoeffTuner.get() : dragCoeff; + double c = dragCoeff; if (c < 1e-6) return tof; return (1.0 - Math.exp(-c * tof)) / c; } @@ -332,6 +297,10 @@ public static double getMaxTimeOfFlight() { return kShotTable.tof(maxDistance); } + // ============================================================= + // SHOOT ON THE MOVE + // ============================================================= + /** * Move a target a set time in the future along a velocity defined by * fieldSpeeds @@ -362,7 +331,7 @@ public static ShotData calculateShotFromFunnelClearance(Pose2d robot, predictedTarget.getX(), predictedTarget.getY(), predictedTarget.getZ()); } - /** Raw-double overload — zero allocation in the hot loop. */ + /** Raw-double overload - zero allocation in the hot loop. */ static ShotData calculateShotFromFunnelClearance(Pose2d robot, double actualTargetX, double actualTargetY, double predX, double predY, double predZ) { @@ -397,7 +366,7 @@ static ShotData calculateShotFromFunnelClearance(Pose2d robot, theta = 0; } - // v0 is in inches/sec — convert to rad/s via flywheel radius in inches + // v0 is in inches/sec - convert to rad/s via flywheel radius in inches double exitVelRadPerSec = v0 / kFlywheelRadiusIn; return new ShotData(exitVelRadPerSec, Math.PI / 2 - theta, new Translation3d(predX, predY, predZ)); } @@ -472,8 +441,8 @@ public static ShotDataLerp iterativeMovingShotFromInterpolationMap(Pose2d robot, double vyLaunch = vy + (turretX - robotX) * omega; double distance = getDistanceToTargetM(robotX, robotY, headingRad, targetX, targetY); - boolean passing = ShooterCalc.isPassing().getAsBoolean(); - // boolean canTurretShoot = ShooterCalc.canTurretShoot(); + boolean passing = ShotCalcMath.isPassing().getAsBoolean(); + // boolean canTurretShoot = ShotCalcMath.canTurretShoot(); // ShotLerpTable shotTable = passing ? (canTurretShoot ? kPassingTable : kAngryTurretTable) : kShotTable; ShotLerpTable shotTable = passing ? kPassingTable : kShotTable; @@ -496,10 +465,10 @@ public static ShotDataLerp iterativeMovingShotFromInterpolationMap(Pose2d robot, double prevPredX = predX; double prevPredY = predY; - // Inline predictTargetPos — no Translation3d/Time allocation + // Inline predictTargetPos - no Translation3d/Time allocation double coeffDrag = 0.5000; //used for SOTM movement SIDE TO SIDE // double coeffDrag = shotTable.drag(distance); - //2974 RAHHHHHHHHHHHHHHH – correction: more like 254 RAHHHHHHHHHHHHHHH + //2974 RAHHHHHHHHHHHHHHH - correction: more like 254 RAHHHHHHHHHHHHHHH // if ( (Math.abs(vx) <= 0.05) || (Math.abs(vy) <= 0.05) ) { //NOTE: not sure if these numbers are right // coeffDrag = 0.2974; //used during static shot (or when the robot is low speed and should be static shooting) // } @@ -507,19 +476,23 @@ public static ShotDataLerp iterativeMovingShotFromInterpolationMap(Pose2d robot, // coeffDrag = 0.53; //0.7 // } - coeffDrag = kDragCoeffTuner.enabled() ? kDragCoeffTuner.get(): coeffDrag; + coeffDrag = kDragCoeffTuner.getOr(coeffDrag); double driftT = dragCompensatedTOF(tofSec, coeffDrag); predX = targetX - vxLaunch * driftT; predY = targetY - vyLaunch * driftT; distance = getDistanceToTargetM(robotX, robotY, headingRad, predX, predY); - passing = ShooterCalc.isPassing().getAsBoolean(); + passing = ShotCalcMath.isPassing().getAsBoolean(); // shotTable = passing ? (canTurretShoot ? kPassingTable : kAngryTurretTable) : kShotTable; shotTable = passing ? kPassingTable : kShotTable; exitVel = shotTable.exitVelocity(distance); hoodAngle = shotTable.hoodAngle(distance); tofSec = passing ? kPassingTable.tof(distance) : kShotTable.tof(distance); + if (kRPSOverallBoostTuner.enabled()) { + exitVel += (kRPSOverallBoostTuner.get() * (2.0 * Math.PI)); + } + double dExitVel = prevExitVel - exitVel; double dHood = prevHoodAngle - hoodAngle; double dTOF = prevTOF - tofSec; @@ -542,6 +515,10 @@ public static ShotDataLerp iterativeMovingShotFromInterpolationMap(Pose2d robot, return data; } + // ============================================================= + // DATA RECORDS + // ============================================================= + public record ShotData (double exitVelocity, double hoodAngle, Translation3d target) { public ShotData(AngularVelocity exitVelocity, Angle hoodAngle, Translation3d target) { this(exitVelocity.in(RadiansPerSecond), hoodAngle.in(Radians), target); @@ -612,6 +589,10 @@ public void acceptLogging(ShotDataLerp data) { public double getTofSec() { return tofSec; } } + // ============================================================= + // LERP TABLE + // ============================================================= + /** * Zero-allocation sorted-array interpolation table. * Replaces InterpolatingTreeMap / InterpolatingDoubleTreeMap on hot paths. @@ -661,8 +642,8 @@ public static final class Builder { /** dist: meters, rps: rot/s (shooter), hoodRots: rotations, tof: seconds */ public void add(double dist, double rps, double hoodRots, double tof, double drag) { entries.put(dist, new double[]{ - rps * (2.0 * Math.PI), // rot/s → rad/s - hoodRots * (2.0 * Math.PI), // rotations → radians + rps * (2.0 * Math.PI), // rot/s -> rad/s + hoodRots * (2.0 * Math.PI), // rotations -> radians tof, drag }); diff --git a/src/main/java/frc/util/HubShiftUtil.java b/src/main/java/frc/util/HubShiftUtil.java index ceca48af..a8d8cda2 100644 --- a/src/main/java/frc/util/HubShiftUtil.java +++ b/src/main/java/frc/util/HubShiftUtil.java @@ -13,7 +13,7 @@ import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.Supplier; -import frc.robot.subsystems.shooter.ShotCalculator; +import frc.robot.subsystems.shooter.calc.ShotCalculator; public class HubShiftUtil { public enum ShiftEnum { diff --git a/src/test/java/frc/robot/subsystems/shooter/ShotCalcPerfTest.java b/src/test/java/frc/robot/subsystems/shooter/ShotCalcPerfTest.java index b12082df..45cab6a0 100644 --- a/src/test/java/frc/robot/subsystems/shooter/ShotCalcPerfTest.java +++ b/src/test/java/frc/robot/subsystems/shooter/ShotCalcPerfTest.java @@ -14,8 +14,9 @@ import frc.robot.Constants.ShooterK; import frc.robot.FieldConstants; -import frc.robot.subsystems.shooter.ShotCalculator.ShotData; -import frc.robot.subsystems.shooter.ShotCalculator.ShotDataLerp; +import frc.robot.subsystems.shooter.calc.ShotCalculator; +import frc.robot.subsystems.shooter.calc.ShotCalculator.ShotData; +import frc.robot.subsystems.shooter.calc.ShotCalculator.ShotDataLerp; import org.junit.jupiter.api.*; @@ -379,8 +380,8 @@ void benchmarkComparison() { System.out.println(" Raw Doubles: 1 object (the double[] result array)"); System.out.println(); - // Print at 25Hz (the ShooterCalc Notifier rate) - System.out.println("At 25Hz ShooterCalc rate:"); + // Print at 25Hz (the ShotCalcMath Notifier rate) + System.out.println("At 25Hz ShotCalcMath rate:"); System.out.printf(" Immutable Units: %.1f us/cycle (%.1f%% of 40ms budget)%n", immutableUs, immutableUs / 40000.0 * 100); System.out.printf(" Mutable Units: %.1f us/cycle (%.1f%% of 40ms budget)%n", diff --git a/src/test/java/frc/robot/subsystems/shooter/ShotCalcTest.java b/src/test/java/frc/robot/subsystems/shooter/ShotCalcTest.java index c882536d..59140725 100644 --- a/src/test/java/frc/robot/subsystems/shooter/ShotCalcTest.java +++ b/src/test/java/frc/robot/subsystems/shooter/ShotCalcTest.java @@ -10,10 +10,12 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.*; -import frc.robot.subsystems.shooter.ShotCalculator.ShotData; -import frc.robot.subsystems.shooter.ShotCalculator.ShotDataLerp; -import frc.robot.subsystems.shooter.ShooterCalc.AzimuthCalcDetails; -import frc.robot.subsystems.shooter.ShooterCalc.ShotCalcOutputs; +import frc.robot.subsystems.shooter.calc.ShotCalculator.ShotData; +import frc.robot.subsystems.shooter.calc.ShotCalculator.ShotDataLerp; +import frc.robot.subsystems.shooter.calc.ShotCalcMath; +import frc.robot.subsystems.shooter.calc.ShotCalcMath.AzimuthCalcDetails; +import frc.robot.subsystems.shooter.calc.ShotCalcMath.ShotCalcOutputs; +import frc.robot.subsystems.shooter.calc.ShotCalculator; import org.junit.jupiter.api.*; @@ -302,7 +304,7 @@ void movingShot_targetDiffersFromStatic() { void moreIterations_convergesToSameResult() { // The iterative solver exhibits damped oscillation (alternating over/undershoot) // with a ~1/3 contraction ratio per step. At vx=2.0, vy=1.0 m/s it needs ~7 - // iterations to fully converge. Production uses 5 (ShooterCalc.calcShot), + // iterations to fully converge. Production uses 5 (ShotCalcMath.calcShot), // so we verify that 5 iterations lands close to the fully-converged answer. ShotDataLerp shot5 = ShotCalculator.iterativeMovingShotFromInterpolationMap( MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 5); @@ -519,44 +521,44 @@ void movingRobot_returnsValidShot() { } // ================================================================ - // ShooterCalc.calcAzimuth (static method) + // ShotCalcMath.calcAzimuth (static method) // ================================================================ @Nested class CalcAzimuth { @Test void turretAtZero_closeTarget_returnsValidAzimuth() { - AzimuthCalcDetails details = ShooterCalc.calcAzimuth( + AzimuthCalcDetails details = ShotCalcMath.calcAzimuth( HUB_TARGET, CLOSE_POSE, 0.0, ZERO_SPEEDS); assertAzimuthValid(details); } @Test void turretAtZero_midTarget_returnsValidAzimuth() { - AzimuthCalcDetails details = ShooterCalc.calcAzimuth( + AzimuthCalcDetails details = ShotCalcMath.calcAzimuth( HUB_TARGET, MID_POSE, 0.0, ZERO_SPEEDS); assertAzimuthValid(details); } @Test void turretAtPositiveQuarter_returnsValidAzimuth() { - AzimuthCalcDetails details = ShooterCalc.calcAzimuth( + AzimuthCalcDetails details = ShotCalcMath.calcAzimuth( HUB_TARGET, MID_POSE, 0.25, ZERO_SPEEDS); assertAzimuthValid(details); } @Test void turretAtNegativeQuarter_returnsValidAzimuth() { - AzimuthCalcDetails details = ShooterCalc.calcAzimuth( + AzimuthCalcDetails details = ShotCalcMath.calcAzimuth( HUB_TARGET, MID_POSE, -0.25, ZERO_SPEEDS); assertAzimuthValid(details); } @Test void movingRobot_producesVelocityFF() { - AzimuthCalcDetails stationary = ShooterCalc.calcAzimuth( + AzimuthCalcDetails stationary = ShotCalcMath.calcAzimuth( HUB_TARGET, MID_POSE, 0.0, ZERO_SPEEDS); - AzimuthCalcDetails moving = ShooterCalc.calcAzimuth( + AzimuthCalcDetails moving = ShotCalcMath.calcAzimuth( HUB_TARGET, MID_POSE, 0.0, TRANSLATING_SPEEDS); // Moving robot should produce nonzero velocity feedforward assertNotEquals(0.0, moving.turretVelocityFF(), 1e-6, @@ -565,7 +567,7 @@ void movingRobot_producesVelocityFF() { @Test void stationaryRobot_zeroVelocityFF() { - AzimuthCalcDetails details = ShooterCalc.calcAzimuth( + AzimuthCalcDetails details = ShotCalcMath.calcAzimuth( HUB_TARGET, MID_POSE, 0.0, ZERO_SPEEDS); assertEquals(0.0, details.turretVelocityFF(), 1e-6, "Stationary robot should have zero velocity FF (omega=0, v=0)"); @@ -578,7 +580,7 @@ void turretReference_withinPhysicalLimits() { double[] turretRots = {-0.5, -0.25, 0, 0.25, 0.5}; for (Pose2d pose : poses) { for (double rot : turretRots) { - AzimuthCalcDetails d = ShooterCalc.calcAzimuth( + AzimuthCalcDetails d = ShotCalcMath.calcAzimuth( HUB_TARGET, pose, rot, ZERO_SPEEDS); double refRots = d.turretReferenceRots(); assertTrue(refRots >= -0.76 && refRots <= 0.76, @@ -590,58 +592,58 @@ void turretReference_withinPhysicalLimits() { } // ================================================================ - // ShooterCalc.calcShot (static method, full pipeline) + // ShotCalcMath.calcShot (static method, full pipeline) // ================================================================ @Nested class CalcShot { @Test void staticShot_close_returnsValidOutputs() { - ShotCalcOutputs out = ShooterCalc.calcShot( + ShotCalcOutputs out = ShotCalcMath.calcShot( CLOSE_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); assertShotCalcOutputsValid(out); } @Test void staticShot_mid_returnsValidOutputs() { - ShotCalcOutputs out = ShooterCalc.calcShot( + ShotCalcOutputs out = ShotCalcMath.calcShot( MID_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); assertShotCalcOutputsValid(out); } @Test void staticShot_far_returnsValidOutputs() { - ShotCalcOutputs out = ShooterCalc.calcShot( + ShotCalcOutputs out = ShotCalcMath.calcShot( FAR_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); assertShotCalcOutputsValid(out); } @Test void dynamicShot_translating_returnsValidOutputs() { - ShotCalcOutputs out = ShooterCalc.calcShot( + ShotCalcOutputs out = ShotCalcMath.calcShot( MID_POSE, false, HUB_TARGET, 0.0, TRANSLATING_SPEEDS); assertShotCalcOutputsValid(out); } @Test void dynamicShot_rotating_returnsValidOutputs() { - ShotCalcOutputs out = ShooterCalc.calcShot( + ShotCalcOutputs out = ShotCalcMath.calcShot( MID_POSE, false, HUB_TARGET, 0.0, ROTATING_SPEEDS); assertShotCalcOutputsValid(out); } @Test void dynamicShot_combined_returnsValidOutputs() { - ShotCalcOutputs out = ShooterCalc.calcShot( + ShotCalcOutputs out = ShotCalcMath.calcShot( MID_POSE, false, HUB_TARGET, 0.1, COMBINED_SPEEDS); assertShotCalcOutputsValid(out); } @Test void staticVsDynamic_sameWhenStationary() { - ShotCalcOutputs staticOut = ShooterCalc.calcShot( + ShotCalcOutputs staticOut = ShotCalcMath.calcShot( MID_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); - ShotCalcOutputs dynamicOut = ShooterCalc.calcShot( + ShotCalcOutputs dynamicOut = ShotCalcMath.calcShot( MID_POSE, false, HUB_TARGET, 0.0, ZERO_SPEEDS); // With zero chassis speeds, static and dynamic should produce identical results assertEquals( @@ -660,9 +662,9 @@ void staticVsDynamic_sameWhenStationary() { @Test void farShot_higherVelocityThanClose() { - ShotCalcOutputs closeOut = ShooterCalc.calcShot( + ShotCalcOutputs closeOut = ShotCalcMath.calcShot( CLOSE_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); - ShotCalcOutputs farOut = ShooterCalc.calcShot( + ShotCalcOutputs farOut = ShotCalcMath.calcShot( FAR_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); assertTrue( farOut.shooterReferenceRps() >