diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 013bedb..a0bd46b 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -31,7 +31,6 @@ import frc.robot.generated.ChoreoVars; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.Utilities; @Logged @@ -152,8 +151,6 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. @NotLogged private final Drive _driveSubsystem; @NotLogged - private final Shooter _shooterSubsystem; - @NotLogged private final Intake _intakeSubsystem; @NotLogged private final AutoFactory _autoFactory; @@ -182,10 +179,9 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. @Logged private Pose2d _currentDesiredPathPose = new Pose2d(); - public Autos(Drive driveSubsystem, Shooter shooterSubsystem, Intake intakeSubsystem) + public Autos(Drive driveSubsystem, Intake intakeSubsystem) { _driveSubsystem = driveSubsystem; - _shooterSubsystem = shooterSubsystem; _intakeSubsystem = intakeSubsystem; _autoFactory = new AutoFactory(() -> driveSubsystem.getState().Pose, driveSubsystem::resetPose, this::followTrajectory, true, driveSubsystem); _trajectoryDrawRoutine = _autoFactory.newRoutine("Trajectory Drawing"); @@ -223,7 +219,6 @@ public Command buildAuto() var driveOption = _driveChooser.getSelected(); var startPose = getStartPose(mode, startPosition, driveOption); var resetPose = Commands.runOnce(() -> _driveSubsystem.resetPose(startPose)); - var shoot = _shooterSubsystem.shoot(); var delay = Commands.waitSeconds(delaySeconds); var extend = _intakeSubsystem.getExtendCmd(); var pickup = _intakeSubsystem.runRollersForward(); @@ -236,13 +231,13 @@ public Command buildAuto() { case DoNothing -> resetPose; case DriveOnly -> Commands.sequence(resetPose, drive); - case ShootOnly -> Commands.sequence(resetPose, shoot); - case ShootWithDelay -> Commands.sequence(resetPose, delay, shoot); - case DriveThenShoot -> Commands.sequence(resetPose, drive, shoot); - case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, shoot, drive); - case HubOutpostDelayTowerShoot -> Commands.sequence(resetPose, hubToOutpost, Commands.waitSeconds(HUB_OUTPOST_TOWER_DELAY), outpostToTower, shoot); - case DrivePickupThenShoot -> Commands.sequence(resetPose, extend, Commands.deadline(drive, pickup), shoot.alongWith(jiggle)); - case DriveWithDelayPickupThenShoot -> Commands.sequence(resetPose, delay, extend, Commands.deadline(drive, pickup), shoot.alongWith(jiggle)); + case ShootOnly -> Commands.sequence(resetPose); + case ShootWithDelay -> Commands.sequence(resetPose, delay); + case DriveThenShoot -> Commands.sequence(resetPose, drive); + case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, drive); + case HubOutpostDelayTowerShoot -> Commands.sequence(resetPose, hubToOutpost, Commands.waitSeconds(HUB_OUTPOST_TOWER_DELAY), outpostToTower); + case DrivePickupThenShoot -> Commands.sequence(resetPose, extend, Commands.deadline(drive, pickup), jiggle); + case DriveWithDelayPickupThenShoot -> Commands.sequence(resetPose, delay, extend, Commands.deadline(drive, pickup), jiggle); }; } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 92d2126..8ccfaec 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,18 +8,14 @@ import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Milliseconds; import static edu.wpi.first.units.Units.Percent; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; import java.util.List; -import java.util.Map; -import java.util.stream.Stream; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; @@ -28,15 +24,11 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.AngleUnit; -import edu.wpi.first.units.AngularAccelerationUnit; -import edu.wpi.first.units.AngularVelocityUnit; import edu.wpi.first.units.DistanceUnit; -import edu.wpi.first.units.VoltageUnit; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -159,146 +151,6 @@ public static class GeneralConstants public static final Translation2d FIELD_CENTER = new Translation2d(FIELD_SIZE_X.div(2), FIELD_SIZE_Y.div(2)); } - public static class ShooterConstants - { - // Flywheel - public static final double FLYWHEEL_KP = 0.00015; // TODO: Tune - public static final double FLYWHEEL_KI = 0.0; // TODO: Tune - public static final double FLYWHEEL_KD = 0.0005; - public static final Voltage FLYWHEEL_KS = Volts.of(0.0); // TODO: Tune - static friction voltage - public static final Per FLYWHEEL_KV = Volts.of(12.0).div(RPM.of(6784.0)); - public static final Per FLYWHEEL_KA = Volts.of(0).per(RotationsPerSecondPerSecond); // TODO: Tune - acceleration voltage - public static final Dimensionless FLYWHEEL_TOLERANCE = Percent.of(15); - public static final Current FLYWHEEL_CURRENT_LIMIT = Amps.of(60); - public static final AngularVelocity MANUAL_SHOOT_RPM = RPM.of(3500.0); - public static final AngularVelocity PASS_FLYWHEEL_VELOCITY = RPM.of(3000.0); // TODO: Tune - - // Turret - public static final Current TURRET_CURRENT_LIMIT = Amps.of(60.0); - public static final double TURRET_KP = 0.19; - public static final double TURRET_KI = 0.01; - public static final double TURRET_KD = 0.0115; - public static final Voltage TURRET_STATIC_FF = Volts.of(0.22); - public static final Angle TURRET_STATIC_FF_ERROR_DEADBAND = Degrees.of(0.75); - public static final Voltage TURRET_MAX_OUTPUT_STEP_PER_LOOP = Volts.of(3.0); - public static final Angle TURRET_HARD_MIN_ANGLE = Degrees.of(-180.0); - public static final Angle TURRET_HARD_MAX_ANGLE = Degrees.of(180.0); - public static final boolean TURRET_SENSOR_INVERTED = true; - public static final Angle TURRET_POT_OFFSET = Degrees.of(-10.0); - public static final Angle TURRET_SOFT_MIN_ANGLE = Degrees.of(-160.0); - public static final Angle TURRET_SOFT_MAX_ANGLE = Degrees.of(160.0); - public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); - public static final Angle TURRET_TOLERANCE = Degrees.of(1.25); - public static final Angle TURRET_LINED_UP_HOLD_TOLERANCE = Degrees.of(2.0); - public static final Angle TURRET_TARGET_SETPOINT_DEADBAND = Degrees.of(0.1); - public static final Angle TURRET_MAX_SETPOINT_STEP_PER_LOOP = Degrees.of(4.0); - public static final Angle TURRET_TRACK_COMMAND_DEADBAND = Degrees.of(0.5); - public static final Angle TURRET_TRACK_COMMAND_MAX_STEP_PER_LOOP = Degrees.of(4.0); - public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(1.0); - public static final boolean TURRET_USE_VISION_TX_TRIM = false; - public static final Time TURRET_VISION_TX_FILTER_WINDOW = Milliseconds.of(100.0); - public static final Angle TURRET_VISION_TX_MAX_STEP_PER_LOOP = Degrees.of(0.4); - public static final Angle TURRET_TRACK_BIAS = Degrees.zero(); // TODO: Tune static shot bias from log data - public static final boolean TURRET_USE_LINEAR_DRIFT_COMPENSATION = true; - public static final Distance TURRET_DRIFT_LATERAL_BIAS = Inches.of(11.060660171779821); - public static final Distance TURRET_DRIFT_LATERAL_SINE_AMPLITUDE = Inches.of(7.1890180494515015); - public static final Angle TURRET_DRIFT_LATERAL_PHASE_OFFSET = Degrees.of(-20.866427056070258); - public static final Angle TURRET_LINEAR_DRIFT_MAX_CORRECTION = Degrees.of(15.0); - public static final double TURRET_LINEAR_DRIFT_CORRECTION_SIGN = -1.0; - public static final String LIMELIGHT_NAME = "limelight-shooter"; - public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice - public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); - public static final List BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26); - public static final List RED_HUB_TAG_IDS = List.of(2, 4, 5, 10); - public static final List ALL_HUB_TAG_IDS = Stream.concat(BLUE_HUB_TAG_IDS.stream(), RED_HUB_TAG_IDS.stream()).toList(); - public static final Translation3d CENTER_TAG_TO_HUB_CENTER_OFFSET = new Translation3d(Inches.of(-23.5), Inches.zero(), Inches.zero()); - public static final Angle TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD = Degrees.of(90); // Hub "zero" is 90 degrees - // left of robot forward - public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); - public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); - public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); - public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.of(1.0), Inches.zero()); // Measured - // release - // point - // is - // ~1 - // in - // forward - // of - // the - // pivot - // (turret - // -90), - // not - // lateral - public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); - public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); - public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" - public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); - public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); - public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); - public static final Time SWM_RELEASE_PHASE_DELAY = Seconds.of(0.03); - public static final int SWM_LOOKAHEAD_ITERATIONS = 8; - public static final LinearVelocity SWM_ENABLE_TRANSLATIONAL_SPEED = MetersPerSecond.of(0.15); - public static final AngularVelocity SWM_ENABLE_ANGULAR_SPEED = DegreesPerSecond.of(10.0); - public static final LinearVelocity SWM_FEED_MAX_TRANSLATIONAL_SPEED = MetersPerSecond.of(1.5); - public static final AngularVelocity SWM_FEED_MAX_ANGULAR_SPEED = DegreesPerSecond.of(45.0); - public static final Angle SWM_FEED_TURRET_TOLERANCE = Degrees.of(5.0); - public static final Time SWM_FEED_STABILITY_WINDOW = Milliseconds.of(60.0); - private static final double SWM_MIN_TOF_DISTANCE_IN = 64.0; - private static final double SWM_MAX_TOF_DISTANCE_IN = 159.0; - - // @formatter:off - private static final InterpolatingDoubleTreeMap FLYWHEEL_SPEED_TABLE = InterpolatingDoubleTreeMap.ofEntries - ( - Map.entry(50.0, 2500.0), - Map.entry(75.5, 2950.0), - Map.entry(101.0, 3400.0), - Map.entry(113.0, 3850.0), - Map.entry(130.0, 3900.0), - Map.entry(151.0, 4600.0), - Map.entry(183.0, 5100.0), - Map.entry(204.0, 5850.0) - ); - - // Keep TOF as its own lookup so the moving-shot solve matches the empirical - // map-based pattern used by teams like 5000, 6328, and Eeshwar's writeup. - private static final InterpolatingDoubleTreeMap SHOT_TOF_TABLE = InterpolatingDoubleTreeMap.ofEntries - ( - Map.entry(64.0, 0.542), - Map.entry(85.0, 1.06), - Map.entry(87.0, 1.02), - Map.entry(134.0, 1.22), - Map.entry(159.0, 1.74) - ); - // @formatter:on - - // Feeder - public static final Current FEEDER_CURRENT_LIMIT = Amps.of(60); - public static final Voltage FEEDER_VOLTAGE = Volts.of(6); - public static final Current ROTOR_CURRENT_LIMIT = Amps.of(80); - public static final Voltage ROTOR_FAST_VOLTAGE = Volts.of(8.0); - public static final Voltage ROTOR_MID_VOLTAGE = Volts.of(8.0); - public static final Voltage ROTOR_RETRACTED_VOLTAGE = Volts.of(8.0); - - // TODO: Tune these values with testing! - public static AngularVelocity getFlywheelSpeedForDistance(Distance distance) - { - return RPM.of(FLYWHEEL_SPEED_TABLE.get(distance.in(Inches))); - } - - public static Time getShotTimeOfFlight(Distance distance) - { - return Seconds.of(SHOT_TOF_TABLE.get(distance.in(Inches))); - } - - public static boolean isMovingShotDistanceValid(Distance distance) - { - var distanceInches = distance.in(Inches); - return distanceInches >= SWM_MIN_TOF_DISTANCE_IN && distanceInches <= SWM_MAX_TOF_DISTANCE_IN; - } - } - public static class VisionConstants { public static final String LEFT_CAMERA_NAME = "limelight-left"; @@ -330,6 +182,12 @@ public static class VisionConstants // Reject vision updates when no AprilTag is detected within this range public static final Distance MAX_TARGET_DISTANCE = Feet.of(10); + + // AprilTag constants + public static final List BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26); + public static final List RED_HUB_TAG_IDS = List.of(2, 4, 5, 10); + public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); + public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); } public static class ClimberConstants diff --git a/src/main/java/frc/robot/NetParams.java b/src/main/java/frc/robot/NetParams.java deleted file mode 100644 index 5e3f39f..0000000 --- a/src/main/java/frc/robot/NetParams.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot; - -public class NetParams -{ - public NetParams() - { - turretAngle = 0; - flywheelSpeed = 70; - } - - public double turretAngle; - public double flywheelSpeed; -} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2097a02..b63e203 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,7 +3,6 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Value; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; @@ -13,38 +12,31 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.DriveConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.TunerConstants; import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.MeasureUtil; @Logged public class RobotContainer { - private static final double MANUAL_FLYWHEEL_START_RPM = 3500.0; - private static final double MANUAL_FLYWHEEL_STEP_RPM = 50.0; - private final SwerveRequest.FieldCentric _fieldCentric = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - private final SwerveRequest.RobotCentric _robotCentric = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - private final CommandJoystick _driver = new CommandJoystick(0); - private final CommandXboxController _operator = new CommandXboxController(1); - private final Drive _drive = TunerConstants.createDrivetrain(); - private final Intake _intake = new Intake(); - private final Shooter _shooter = new Shooter(_drive::getState, _intake::isExtended, _intake::isRetracted); - private final Autos _autos = new Autos(_drive, _shooter, _intake); - private boolean _driverPOVReleased = true; + private final SwerveRequest.FieldCentric _fieldCentric = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + private final SwerveRequest.RobotCentric _robotCentric = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + private final CommandJoystick _driver = new CommandJoystick(0); + private final CommandXboxController _operator = new CommandXboxController(1); + private final Drive _drive = TunerConstants.createDrivetrain(); + private final Intake _intake = new Intake(); + private final Autos _autos = new Autos(_drive, _intake); // private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; - private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM; public RobotContainer() { - configureBindings(); + configureDefaultCommands(); + configureButtonBindings(); } private LinearVelocity getDrive() @@ -67,70 +59,25 @@ private SwerveRequest.FieldCentric getFieldCentricRequest() return _fieldCentric.withVelocityX(getDrive()).withVelocityY(getStrafe()).withRotationalRate(getRotate()); } - private void setManualFlywheelRPM(double rpm) - { - _manualFlywheelRPM = Math.max(0.0, rpm); - _shooter.setManualFlywheel(_manualFlywheelRPM); - } - - private void configureBindings() + private void configureDefaultCommands() { _drive.setDefaultCommand(_drive.applyRequest(this::getFieldCentricRequest)); final var idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled().whileTrue(_drive.applyRequest(() -> idle).ignoringDisable(true)); + } - // _driver.button(1).whileTrue(Commands.parallel(_shooter.shoot(), - // Commands.startEnd(() -> _drive.disableVisionPoseCorrection(true), () -> - // _drive.disableVisionPoseCorrection(false)))); - _driver.button(1).whileTrue(_shooter.manualShootCmd(() -> _manualFlywheelRPM)); - // _driver.button(2).whileTrue(Commands.startEnd(() -> _driveMultiplier = - // DriveConstants.SLOW_MODE_SCALE, () -> _driveMultiplier = - // DriveConstants.FULL_SPEED_SCALE)); + private void configureButtonBindings() + { _driver.button(2).whileTrue(_intake.runRollersForward()); _driver.button(3).onTrue(_intake.getExtendCmd()); _driver.button(4).onTrue(_intake.getRetractCmd()); - _driver.button(5).whileTrue(_shooter.pass()); - _driver.button(6).whileTrue(_shooter.trackOnly()); _driver.button(7).onTrue(_drive.runOnce(_drive::seedFieldCentric)); - // _driver.button(8).onTrue(_shooter.setManualTurretAngle(Degrees.zero())); - // _driver.button(9).onTrue(_shooter.setManualTurretAngle(Degrees.of(45.0))); - // _driver.button(10).onTrue(_shooter.setManualTurretAngle(Degrees.of(-45.0))); - // _driver.button(11).onTrue(_shooter.setManualTurretAngle(Degrees.of(90.0))); - // _driver.button(12).onTrue(_shooter.setManualTurretAngle(Degrees.of(-90.0))); - // _driver.button(13).onTrue(_shooter.setManualTurretAngle(Degrees.of(135.0))); - // _driver.button(14).onTrue(_shooter.setManualTurretAngle(Degrees.of(-135.0))); _driver.button(16).whileTrue(_drive.applyRequest(() -> _robotCentric.withVelocityX(getDrive()).withVelocityY(getStrafe()).withRotationalRate(getRotate()))); - // Debounce the transition out of -1 by 50ms - Trigger povJustPressed = _driver.pov(-1).negate().debounce(0.05).and(() -> _driverPOVReleased); - - povJustPressed.onTrue(Commands.runOnce(() -> - { - int angle = _driver.getHID().getPOV(); - _driverPOVReleased = false; - - _shooter.setManualTurretAngleCommand(Degrees.of(switch (angle) - { - case 0 -> -90; - case 180 -> 90; - case 225 -> 45; - case 315 -> -45; - default -> 0; - })); - - })); - - _driver.pov(-1).onTrue(Commands.runOnce(() -> _driverPOVReleased = true)); - _operator.leftTrigger().whileTrue(_intake.runRollersForward()); _operator.leftBumper().whileTrue(_intake.runRollersReverse()); _operator.rightTrigger().whileTrue(_intake.jiggle()); - _operator.y().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(MANUAL_FLYWHEEL_START_RPM))); - _operator.x().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(_manualFlywheelRPM - MANUAL_FLYWHEEL_STEP_RPM))); - _operator.b().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(_manualFlywheelRPM + MANUAL_FLYWHEEL_STEP_RPM))); - _operator.a().onTrue(Commands.runOnce(_shooter::stopManualFlywheel)); - _operator.rightBumper().whileTrue(_shooter.runManualFeeder()); _operator.povDown().onTrue(_intake.getRetractCmd()); _operator.povUp().onTrue(_intake.getExtendCmd()); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Feeder.java b/src/main/java/frc/robot/subsystems/shooter/Feeder.java deleted file mode 100644 index fcf5453..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/Feeder.java +++ /dev/null @@ -1,48 +0,0 @@ -package frc.robot.subsystems.shooter; - -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Volts; - -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.CANConstants; -import frc.robot.Constants.GeneralConstants; -import frc.robot.Constants.ShooterConstants; - -@Logged -public class Feeder -{ - private final SparkFlex _feederMotor; - @Logged - private Voltage _feederMotorVoltage = Volts.of(0.0); - @Logged - private boolean _enabled = false; - - public Feeder() - { - _feederMotor = new SparkFlex(CANConstants.FEEDER_MOTOR, MotorType.kBrushless); - - SparkFlexConfig config = new SparkFlexConfig(); - config.inverted(false).idleMode(IdleMode.kBrake).smartCurrentLimit((int)ShooterConstants.FEEDER_CURRENT_LIMIT.in(Amps)).voltageCompensation(GeneralConstants.MOTOR_VOLTAGE.in(Volts)); - - _feederMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - } - - public void periodic() - { - _feederMotorVoltage = Volts.of(_feederMotor.getAppliedOutput() * _feederMotor.getBusVoltage()); - } - - public void set(boolean on) - { - _enabled = on; - Voltage targetVoltage = on ? ShooterConstants.FEEDER_VOLTAGE : Volts.zero(); - _feederMotor.setVoltage(targetVoltage.in(Volts)); - } -} diff --git a/src/main/java/frc/robot/subsystems/shooter/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/Flywheel.java deleted file mode 100644 index 0e0e18c..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/Flywheel.java +++ /dev/null @@ -1,136 +0,0 @@ -package frc.robot.subsystems.shooter; - -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.RPM; -import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Value; -import static edu.wpi.first.units.Units.Volts; - -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.sim.SparkFlexSim; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; - -import frc.robot.Constants.CANConstants; -import frc.robot.Constants.GeneralConstants; -import frc.robot.Constants.ShooterConstants; - -@Logged -public class Flywheel -{ - private final SparkFlex _leadMotor; - private final SparkFlex _followMotor; - private final SparkClosedLoopController _closedLoopController; - private final FlywheelSim _flywheelSim; - private final RelativeEncoder _flywheelEncoder; - private final SparkFlexSim _leadMotorSim; - @Logged - private AngularVelocity _velocity = RPM.zero(); - @Logged - private AngularVelocity _targetVelocity = RPM.zero(); - @Logged - private Voltage _flywheelVoltage = Volts.zero(); - - public Flywheel() - { - _leadMotor = new SparkFlex(CANConstants.FLYWHEEL_LEAD, MotorType.kBrushless); - _followMotor = new SparkFlex(CANConstants.FLYWHEEL_FOLLOW, MotorType.kBrushless); - - if (RobotBase.isReal()) - { - _leadMotor.setCANTimeout(250); - _followMotor.setCANTimeout(250); - } - - var config = new SparkFlexConfig(); - config.inverted(false).idleMode(IdleMode.kCoast).smartCurrentLimit((int)ShooterConstants.FLYWHEEL_CURRENT_LIMIT.in(Amps)).voltageCompensation(GeneralConstants.MOTOR_VOLTAGE.in(Volts)); - config.closedLoop.p(ShooterConstants.FLYWHEEL_KP).d(ShooterConstants.FLYWHEEL_KD); - config.closedLoop.feedForward.kS(ShooterConstants.FLYWHEEL_KS.in(Volts)).kV(ShooterConstants.FLYWHEEL_KV.in(Volts.per(RPM))).kA(ShooterConstants.FLYWHEEL_KA.in(Volts.per(RotationsPerSecondPerSecond))); - _leadMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - - config.follow(_leadMotor, true); - _followMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - - _closedLoopController = _leadMotor.getClosedLoopController(); - _flywheelEncoder = _leadMotor.getEncoder(); - - if (RobotBase.isReal()) - { - _leadMotor.setCANTimeout(0); - _followMotor.setCANTimeout(0); - _leadMotorSim = null; - _flywheelSim = null; - } - else - { - var gearbox = DCMotor.getNeoVortex(2); - _leadMotorSim = new SparkFlexSim(_leadMotor, DCMotor.getNeoVortex(2)); - _flywheelSim = new FlywheelSim(LinearSystemId.createFlywheelSystem(gearbox, 0.001, 1), gearbox); - } - } - - public void periodic() - { - _velocity = RPM.of(_flywheelEncoder.getVelocity()); - _flywheelVoltage = Volts.of(_leadMotor.getAppliedOutput() * _leadMotor.getBusVoltage()); - } - - public void simulationPeriodic() - { - _flywheelSim.setInputVoltage(_flywheelVoltage.in(Volts)); - _flywheelSim.update(GeneralConstants.LOOP_PERIOD.in(Seconds)); - var velocity = _flywheelSim.getAngularVelocity().in(RPM); - _leadMotorSim.getRelativeEncoderSim().setVelocity(velocity); - _leadMotorSim.iterate(velocity, RoboRioSim.getVInVoltage(), GeneralConstants.LOOP_PERIOD.in(Seconds)); - } - - public void setVelocity(AngularVelocity targetVelocity) - { - if (targetVelocity == null) - { - stop(); - return; - } - _targetVelocity = targetVelocity; - _closedLoopController.setSetpoint(_targetVelocity.in(RPM), ControlType.kVelocity, ClosedLoopSlot.kSlot0); - } - - public void stop() - { - _targetVelocity = RPM.zero(); - _leadMotor.setVoltage(0.0); - } - - public boolean atSpeed() - { - if (_targetVelocity == RPM.zero()) return true; - return getVelocity().isNear(_targetVelocity, ShooterConstants.FLYWHEEL_TOLERANCE.in(Value)); - } - - public AngularVelocity getVelocity() - { - return _velocity; - } - - public AngularVelocity getTargetVelocity() - { - return _targetVelocity; - } -} diff --git a/src/main/java/frc/robot/subsystems/shooter/Rotor.java b/src/main/java/frc/robot/subsystems/shooter/Rotor.java deleted file mode 100644 index 585012c..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/Rotor.java +++ /dev/null @@ -1,59 +0,0 @@ -package frc.robot.subsystems.shooter; - -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Volts; - -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.units.measure.Voltage; - -import frc.robot.Constants.CANConstants; -import frc.robot.Constants.ShooterConstants; - -@Logged -public class Rotor -{ - private final TalonFX _rotorMotor; - @Logged - private Voltage _rotorMotorVoltage = Volts.zero(); - - public Rotor() - { - _rotorMotor = new TalonFX(CANConstants.ROTOR_MOTOR); - - var currentConfig = new CurrentLimitsConfigs(); - currentConfig.StatorCurrentLimit = ShooterConstants.ROTOR_CURRENT_LIMIT.in(Amps); - currentConfig.StatorCurrentLimitEnable = true; - - var outputConfig = new MotorOutputConfigs(); - outputConfig.NeutralMode = NeutralModeValue.Brake; - outputConfig.Inverted = InvertedValue.Clockwise_Positive; - _rotorMotor.getConfigurator().apply(new TalonFXConfiguration().withCurrentLimits(currentConfig).withMotorOutput(outputConfig)); - } - - public void periodic() - { - _rotorMotorVoltage = _rotorMotor.getMotorVoltage().getValue(); - } - - public void set(boolean on) - { - setVoltage(on ? ShooterConstants.ROTOR_FAST_VOLTAGE : Volts.zero()); - } - - public void setVoltage(Voltage voltage) - { - _rotorMotor.setVoltage(voltage.in(Volts)); - } - - public void stop() - { - setVoltage(Volts.zero()); - } -} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java deleted file mode 100644 index 7c9ae58..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ /dev/null @@ -1,505 +0,0 @@ -package frc.robot.subsystems.shooter; - -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RPM; -import static edu.wpi.first.units.Units.Seconds; - -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.units.measure.Angle; -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.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ShooterConstants; -import frc.robot.subsystems.shooter.TurretDirector.ShotMode; -import frc.robot.subsystems.shooter.TurretDirector.ShotSolution; - -@Logged -public class Shooter extends SubsystemBase -{ - public enum ShooterState - { - Idle, Preparing, Ready, Firing, Manual, TrackingOnly; - } - - public final Flywheel _flywheel; - public final Feeder _feeder; - public final Rotor _rotor; - public final Turret _turret; - public final TurretDirector _turretDirector; - private final Supplier _swerveStateSupplier; - private final Supplier _intakeExtendedSupplier; - private final Supplier _intakeRetractedSupplier; - private final Debouncer _movingFeedDebouncer; - @Logged - private ShooterState _state; - @Logged - private boolean _autoShootEnabled; - @Logged - private ShotMode _requestedShotMode; - private ShotSolution _currentShotSolution; - @Logged - private boolean _solutionReady; - @Logged - private boolean _turretReady; - @Logged - private boolean _flywheelReady; - @Logged - private boolean _hubReady; - @Logged - private boolean _inRangeReady; - @Logged - private boolean _rolloutReady; - @Logged - private boolean _stabilityReady; - @Logged - private boolean _usingMovingShotMath; - @Logged - private double _robotTranslationSpeedMetersPerSecond; - @Logged - private double _robotAngularSpeedRadiansPerSecond; - @Logged - private boolean _feedReady; - - public Shooter(Supplier swerveStateSupplier, Supplier intakeExtendedSupplier, Supplier intakeRetractedSupplier) - { - _swerveStateSupplier = swerveStateSupplier; - _intakeExtendedSupplier = intakeExtendedSupplier; - _intakeRetractedSupplier = intakeRetractedSupplier; - _flywheel = new Flywheel(); - _feeder = new Feeder(); - _rotor = new Rotor(); - _turret = new Turret(); - _turretDirector = new TurretDirector(swerveStateSupplier); - _movingFeedDebouncer = new Debouncer(ShooterConstants.SWM_FEED_STABILITY_WINDOW.in(Seconds)); - _state = ShooterState.Idle; - _autoShootEnabled = false; - _requestedShotMode = ShotMode.Idle; - _currentShotSolution = createIdleSolution(); - _solutionReady = false; - _turretReady = false; - _flywheelReady = false; - _hubReady = false; - _inRangeReady = false; - _rolloutReady = false; - _stabilityReady = false; - _usingMovingShotMath = false; - _robotTranslationSpeedMetersPerSecond = 0.0; - _robotAngularSpeedRadiansPerSecond = 0.0; - _feedReady = false; - - _feeder.set(false); - _rotor.stop(); - _flywheel.stop(); - _turret.clearTargetAngle(); - } - - public Command startShooter() - { - return runOnce(() -> startPreparedShoot(false)); - } - - public Command fire() - { - return runOnce(this::commenceFiring); - } - - public Command shoot() - { - return startEnd(() -> startPreparedShoot(true), this::stopShooter); - } - - public Command startPass() - { - return runOnce(() -> startPass(false)); - } - - public Command pass() - { - return startEnd(() -> startPass(true), this::stopShooter); - } - - public void setManualFlywheel(double rpm) - { - beginManualControl(false); - _flywheel.setVelocity(RPM.of(Math.max(0.0, rpm))); - } - - public void stopManualFlywheel() - { - _flywheel.stop(); - - if (_state == ShooterState.Manual) - { - _state = ShooterState.Idle; - } - } - - public Command setFlywheelVelocity(AngularVelocity velocity) - { - return runOnce(() -> - { - beginManualControl(false); - _flywheel.setVelocity(velocity); - }); - } - - public Command runManualFeeder() - { - return startEnd(() -> - { - beginManualControl(false); - _feeder.set(true); - setRotorEnabled(true); - }, () -> - { - _feeder.set(false); - setRotorEnabled(false); - }); - } - - public Command runFeeder() - { - return startEnd(() -> - { - _state = ShooterState.Manual; - _feeder.set(true); - setRotorEnabled(true); - }, () -> - { - _feeder.set(false); - setRotorEnabled(false); - _flywheel.stop(); - _state = ShooterState.Idle; - }); - } - - public Command stop() - { - return runOnce(this::stopShooter); - } - - public Command homeTurret() - { - return setManualTurretAngle(ShooterConstants.TURRET_HOME_ANGLE); - } - - public Command setManualTurretAngle(Angle angle) - { - return runOnce(() -> setManualTurretAngleCommand(angle)); - } - - public Command setTurretAngleCmd(DoubleSupplier angle) - { - return runOnce(() -> setManualTurretAngleCommand(Degrees.of(angle.getAsDouble()))); - } - - public Command manualShootCmd(DoubleSupplier speed) - { - // @formatter:off - return Commands.parallel( - Commands.startEnd(() -> setManualFlywheel(speed.getAsDouble()), () -> stopManualFlywheel()), - runManualFeeder() - ); - // @formatter:on - } - - public Command trackOnly() - { - return startEnd(() -> - { - _state = ShooterState.TrackingOnly; - _requestedShotMode = ShotMode.Track; - }, this::stopShooter); - } - - public void bumpManualTurretAngle(double deltaDeg) - { - setManualTurretAngle(_turret.getManualAngle().plus(Degrees.of(deltaDeg))); - } - - @Override - public void periodic() - { - switch (_state) - { - case Preparing: - case Ready: - _feeder.set(false); - setRotorEnabled(false); - runRequestedShot(true); - - if (isReadyToFeed()) - { - if (_autoShootEnabled) - { - _state = ShooterState.Firing; - } - else - { - _state = ShooterState.Ready; - } - } - else - { - _state = ShooterState.Preparing; - } - break; - - case Firing: - runRequestedShot(true); - if (isReadyToFeed()) - { - _feeder.set(true); - setRotorEnabled(true); - } - else - { - _feeder.set(false); - setRotorEnabled(false); - _state = ShooterState.Preparing; - } - break; - - case Manual: - if (_requestedShotMode != ShotMode.Idle) - { - runRequestedShot(false); - } - break; - - case TrackingOnly: - _feeder.set(false); - setRotorEnabled(false); - _flywheel.stop(); - _requestedShotMode = ShotMode.Track; - runRequestedShot(false); - break; - - case Idle: - default: - _flywheel.stop(); - _feeder.set(false); - setRotorEnabled(false); - clearShotRequest(); - break; - } - - _turret.periodic(); - _flywheel.periodic(); - _feeder.periodic(); - _rotor.periodic(); - } - - @Override - public void simulationPeriodic() - { - _flywheel.simulationPeriodic(); - } - - public void startPreparedShoot(boolean autoShoot) - { - beginShootingSequence(ShotMode.Track, autoShoot); - } - - public void startPass(boolean autoShoot) - { - beginShootingSequence(ShotMode.Pass, autoShoot); - } - - private void beginShootingSequence(ShotMode shotMode, boolean autoShoot) - { - _feeder.set(false); - _requestedShotMode = shotMode; - _autoShootEnabled = autoShoot; - _state = ShooterState.Preparing; - } - - private void beginManualControl(boolean clearTurretTarget) - { - _state = ShooterState.Manual; - _autoShootEnabled = false; - _requestedShotMode = ShotMode.Idle; - _currentShotSolution = createIdleSolution(); - clearReadinessGate(); - _feeder.set(false); - setRotorEnabled(false); - - if (clearTurretTarget) - { - _turret.clearTargetAngle(); - } - } - - public void setManualTurretAngleCommand(Angle angle) - { - beginManualControl(true); - _turret.setManualAngle(angle); - } - - public void stopShooter() - { - _state = ShooterState.Idle; - _autoShootEnabled = false; - clearShotRequest(); - } - - public void commenceFiring() - { - if (_state == ShooterState.Ready) - { - _state = ShooterState.Firing; - } - } - - private void runRequestedShot(boolean spinFlywheel) - { - if (_requestedShotMode == ShotMode.Idle) - { - _turret.clearTargetAngle(); - _currentShotSolution = createIdleSolution(); - clearReadinessGate(); - return; - } - - updateRobotMotionState(); - _usingMovingShotMath = shouldUseMovingShotMath(); - _currentShotSolution = _turretDirector.calculate(_requestedShotMode, _usingMovingShotMath); - _turret.setTargetAngle(_currentShotSolution.turretAngle()); - - if (spinFlywheel) - { - _flywheel.setVelocity(ShooterConstants.getFlywheelSpeedForDistance(_currentShotSolution.distance())); - } - } - - private void clearShotRequest() - { - _requestedShotMode = ShotMode.Idle; - _currentShotSolution = createIdleSolution(); - clearReadinessGate(); - setRotorEnabled(false); - } - - private ShotSolution createIdleSolution() - { - return new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false, false, false); - } - - private boolean isReadyToFeed() - { - _solutionReady = _currentShotSolution.valid(); - _turretReady = isTurretReadyForFeed(); - _flywheelReady = _flywheel.atSpeed(); - _hubReady = hasActiveHubForCurrentShot(); - _inRangeReady = hasValidDistanceForCurrentShot(); - _rolloutReady = isWithinMovingFeedRolloutEnvelope(); - - var rawFeedReady = _solutionReady && _turretReady && _flywheelReady && _inRangeReady && _rolloutReady; - - _stabilityReady = !_usingMovingShotMath || _movingFeedDebouncer.calculate(rawFeedReady); - _feedReady = rawFeedReady && _stabilityReady; - return _feedReady; - } - - private boolean isTurretReadyForFeed() - { - if (!_usingMovingShotMath || _currentShotSolution.mode() != ShotMode.Track) - { - return _turret.isLinedUp(); - } - - var movingToleranceDeg = ShooterConstants.SWM_FEED_TURRET_TOLERANCE.in(Degrees); - var turretErrorDeg = Math.abs(_turret.getTargetAngleError().in(Degrees)); - return turretErrorDeg <= movingToleranceDeg; - } - - private boolean hasActiveHubForCurrentShot() - { - return switch (_currentShotSolution.mode()) - { - case Track -> _currentShotSolution.hubActive(); - case Pass -> true; - case Idle -> false; - }; - } - - private boolean hasValidDistanceForCurrentShot() - { - return switch (_currentShotSolution.mode()) - { - case Track -> !_usingMovingShotMath || _currentShotSolution.inRange(); - case Pass -> true; - case Idle -> false; - }; - } - - private boolean shouldUseMovingShotMath() - { - return _requestedShotMode == ShotMode.Track; - } - - private boolean isWithinMovingFeedRolloutEnvelope() - { - if (!_usingMovingShotMath || _currentShotSolution.mode() != ShotMode.Track) - { - return true; - } - - return _robotTranslationSpeedMetersPerSecond <= ShooterConstants.SWM_FEED_MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond) - && Math.abs(_robotAngularSpeedRadiansPerSecond) <= ShooterConstants.SWM_FEED_MAX_ANGULAR_SPEED.in(RadiansPerSecond); - } - - private void updateRobotMotionState() - { - var speeds = _swerveStateSupplier.get().Speeds; - - _robotTranslationSpeedMetersPerSecond = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - _robotAngularSpeedRadiansPerSecond = speeds.omegaRadiansPerSecond; - } - - private void clearReadinessGate() - { - _solutionReady = false; - _turretReady = false; - _flywheelReady = false; - _hubReady = false; - _inRangeReady = false; - _rolloutReady = false; - _stabilityReady = false; - _usingMovingShotMath = false; - _robotTranslationSpeedMetersPerSecond = 0.0; - _robotAngularSpeedRadiansPerSecond = 0.0; - _movingFeedDebouncer.calculate(false); - _feedReady = false; - } - - private void setRotorEnabled(boolean enabled) - { - if (!enabled) - { - _rotor.stop(); - return; - } - - if (_intakeExtendedSupplier.get()) - { - _rotor.setVoltage(ShooterConstants.ROTOR_FAST_VOLTAGE); - return; - } - - var rotorVoltage = _intakeRetractedSupplier.get() ? ShooterConstants.ROTOR_RETRACTED_VOLTAGE : ShooterConstants.ROTOR_MID_VOLTAGE; - _rotor.setVoltage(rotorVoltage); - } -} diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java deleted file mode 100644 index 55cf20a..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ /dev/null @@ -1,307 +0,0 @@ -package frc.robot.subsystems.shooter; - -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Volts; - -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.AnalogPotentiometer; -import frc.robot.Constants.AIOConstants; -import frc.robot.Constants.CANConstants; -import frc.robot.Constants.ShooterConstants; - -@Logged -public class Turret -{ - private static final int kCandidateWrapCount = 2; - private static final double kComparisonToleranceDeg = 1e-9; - - enum ControlMode - { - Idle, TargetAngle, ManualAngle - } - - private final TalonFX _turretMotor; - private final AnalogPotentiometer _turretPotentiometer; - private final PIDController _pidController; - private ControlMode _controlMode; - @Logged - private Angle _manualAngleSetpoint; - @Logged - private Angle _targetAngleSetpoint; - @Logged - private Angle _turretAngle; - @Logged - private Angle _rawTurretAngle; - @Logged - private Angle _turretSetpoint; - @Logged - private Angle _commandedTargetAngle; - @Logged - private Angle _targetAngleError; - @Logged - private boolean _hasSetpoint; - @Logged - private Voltage _motorVoltage; - private Voltage _lastCommandedMotorVoltage; - @Logged - private boolean _linedUp; - @Logged - private double _motorPositionRotations; - - public Turret() - { - var sensorRange = ShooterConstants.TURRET_HARD_MAX_ANGLE.minus(ShooterConstants.TURRET_HARD_MIN_ANGLE); - var sensorOffset = ShooterConstants.TURRET_HARD_MIN_ANGLE; - - if (ShooterConstants.TURRET_SENSOR_INVERTED) - { - sensorRange = sensorRange.unaryMinus(); - sensorOffset = sensorOffset.unaryMinus(); - } - - _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); - _turretPotentiometer = new AnalogPotentiometer(AIOConstants.TURRET_POTENTIOMETER, sensorRange.in(Degrees), sensorOffset.in(Degrees)); - _pidController = new PIDController(ShooterConstants.TURRET_KP, ShooterConstants.TURRET_KI, ShooterConstants.TURRET_KD); - _controlMode = ControlMode.Idle; - _manualAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _targetAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _turretAngle = Degrees.zero(); - _rawTurretAngle = Degrees.zero(); - _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _commandedTargetAngle = ShooterConstants.TURRET_HOME_ANGLE; - _targetAngleError = Degrees.zero(); - _hasSetpoint = false; - _motorVoltage = Volts.zero(); - _lastCommandedMotorVoltage = Volts.zero(); - _linedUp = false; - _motorPositionRotations = 0.0; - - var currentConfig = new CurrentLimitsConfigs(); - currentConfig.StatorCurrentLimit = ShooterConstants.TURRET_CURRENT_LIMIT.in(Amps); - currentConfig.StatorCurrentLimitEnable = true; - - var outputConfig = new MotorOutputConfigs(); - outputConfig.NeutralMode = NeutralModeValue.Brake; - outputConfig.Inverted = InvertedValue.Clockwise_Positive; - - _turretMotor.getConfigurator().apply(new TalonFXConfiguration().withCurrentLimits(currentConfig).withMotorOutput(outputConfig)); - _pidController.setTolerance(ShooterConstants.TURRET_TOLERANCE.in(Degrees)); - } - - public void periodic() - { - _rawTurretAngle = Degrees.of(_turretPotentiometer.get()); - _turretAngle = _rawTurretAngle.plus(ShooterConstants.TURRET_POT_OFFSET); - _motorPositionRotations = _turretMotor.getPosition().getValue().baseUnitMagnitude(); - _motorVoltage = _turretMotor.getMotorVoltage().getValue(); - - var motorOutput = Volts.zero(); - - switch (_controlMode) - { - case TargetAngle: - _hasSetpoint = true; - _turretSetpoint = selectLegalSetpoint(_targetAngleSetpoint); - break; - - case ManualAngle: - _hasSetpoint = true; - _turretSetpoint = selectLegalSetpoint(_manualAngleSetpoint); - break; - - case Idle: - default: - _hasSetpoint = false; - _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - break; - } - - if (_hasSetpoint) - { - _turretSetpoint = limitSetpointStep(_commandedTargetAngle, _turretSetpoint); - } - - updateLinedUpState(); - - _commandedTargetAngle = _turretSetpoint; - _targetAngleError = _commandedTargetAngle.minus(_turretAngle); - - if (_hasSetpoint) - { - var errorDegrees = _turretSetpoint.minus(_turretAngle).in(Degrees); - var outputVolts = _pidController.calculate(_turretAngle.in(Degrees), _turretSetpoint.in(Degrees)); - var staticFFVolts = ShooterConstants.TURRET_STATIC_FF.in(Volts); - - if (!MathUtil.isNear(0.0, errorDegrees, ShooterConstants.TURRET_STATIC_FF_ERROR_DEADBAND.in(Degrees))) - { - outputVolts += Math.copySign(staticFFVolts, errorDegrees); - } - - motorOutput = Volts.of(outputVolts); - } - - motorOutput = Volts.of(limitOutputStep(motorOutput.in(Volts))); - motorOutput = applySoftLimit(motorOutput); - _turretMotor.setVoltage(motorOutput.in(Volts)); - _lastCommandedMotorVoltage = motorOutput; - } - - public void simulationPeriodic() - { - } - - public void setTargetAngle(Angle angle) - { - var targetDeltaDegrees = Math.abs(MathUtil.inputModulus(angle.minus(_targetAngleSetpoint).in(Degrees), -180.0, 180.0)); - - if (_controlMode != ControlMode.TargetAngle || targetDeltaDegrees >= ShooterConstants.TURRET_TARGET_SETPOINT_DEADBAND.in(Degrees)) - { - _targetAngleSetpoint = angle; - } - - _controlMode = ControlMode.TargetAngle; - } - - public void clearTargetAngle() - { - _controlMode = ControlMode.Idle; - } - - public void setManualAngle(Angle angle) - { - _manualAngleSetpoint = angle; - _controlMode = ControlMode.ManualAngle; - } - - public void bumpManualAngle(Angle delta) - { - _manualAngleSetpoint = _manualAngleSetpoint.plus(delta); - _controlMode = ControlMode.ManualAngle; - } - - public Angle getManualAngle() - { - return _manualAngleSetpoint; - } - - public boolean isLinedUp() - { - return _linedUp; - } - - public Angle getTargetAngleError() - { - return _targetAngleError; - } - - private Angle selectLegalSetpoint(Angle requestedAngle) - { - return chooseNearestLegalAngle(_turretAngle, requestedAngle); - } - - static Angle chooseNearestLegalAngle(Angle currentAngle, Angle requestedAngle) - { - var currentDegrees = currentAngle.in(Degrees); - var requestedDegrees = requestedAngle.in(Degrees); - var minDegrees = ShooterConstants.TURRET_SOFT_MIN_ANGLE.in(Degrees); - var maxDegrees = ShooterConstants.TURRET_SOFT_MAX_ANGLE.in(Degrees); - var wrapCenter = (int)Math.round((currentDegrees - requestedDegrees) / 360.0); - var bestTravel = Double.POSITIVE_INFINITY; - var bestWrapDistance = Integer.MAX_VALUE; - Double bestLegalAngle = null; - - for (int wrapOffset = -kCandidateWrapCount; wrapOffset <= kCandidateWrapCount; wrapOffset++) - { - var wrapIndex = wrapCenter + wrapOffset; - var wrappedDegrees = requestedDegrees + 360.0 * wrapIndex; - if (wrappedDegrees < minDegrees || wrappedDegrees > maxDegrees) - { - continue; - } - - var travelDegrees = Math.abs(wrappedDegrees - currentDegrees); - var wrapDistance = Math.abs(wrapIndex); - - var isBetterCandidate = travelDegrees < bestTravel - || MathUtil.isNear(travelDegrees, bestTravel, kComparisonToleranceDeg) && (wrapDistance < bestWrapDistance || wrapDistance == bestWrapDistance && (bestLegalAngle == null || wrappedDegrees > bestLegalAngle)); - - if (isBetterCandidate) - { - bestLegalAngle = wrappedDegrees; - bestTravel = travelDegrees; - bestWrapDistance = wrapDistance; - } - } - - if (bestLegalAngle != null) - { - return Degrees.of(bestLegalAngle); - } - - return Degrees.of(MathUtil.clamp(requestedDegrees, minDegrees, maxDegrees)); - } - - private Angle limitSetpointStep(Angle previousSetpoint, Angle requestedSetpoint) - { - var maxStepDeg = ShooterConstants.TURRET_MAX_SETPOINT_STEP_PER_LOOP.in(Degrees); - var deltaDeg = requestedSetpoint.minus(previousSetpoint).in(Degrees); - var limitedDeltaDeg = MathUtil.clamp(deltaDeg, -maxStepDeg, maxStepDeg); - var limitedSetpoint = previousSetpoint.plus(Degrees.of(limitedDeltaDeg)); - return selectLegalSetpoint(limitedSetpoint); - } - - private double limitOutputStep(double requestedVolts) - { - var previousVolts = _lastCommandedMotorVoltage.in(Volts); - var maxStepVolts = ShooterConstants.TURRET_MAX_OUTPUT_STEP_PER_LOOP.in(Volts); - var deltaVolts = requestedVolts - previousVolts; - var limitedDeltaVolts = MathUtil.clamp(deltaVolts, -maxStepVolts, maxStepVolts); - return previousVolts + limitedDeltaVolts; - } - - private Voltage applySoftLimit(Voltage requestedVoltage) - { - var requestedVolts = requestedVoltage.in(Volts); - var turretDegrees = _turretAngle.in(Degrees); - - if (turretDegrees <= ShooterConstants.TURRET_SOFT_MIN_ANGLE.in(Degrees) && requestedVolts < 0.0) - { - return Volts.zero(); - } - - if (turretDegrees >= ShooterConstants.TURRET_SOFT_MAX_ANGLE.in(Degrees) && requestedVolts > 0.0) - { - return Volts.zero(); - } - - return requestedVoltage; - } - - private void updateLinedUpState() - { - if (!_hasSetpoint) - { - _linedUp = false; - return; - } - - var angleErrorDegrees = Math.abs(_turretSetpoint.minus(_turretAngle).in(Degrees)); - var acquireTolerance = ShooterConstants.TURRET_TOLERANCE.in(Degrees); - var holdTolerance = ShooterConstants.TURRET_LINED_UP_HOLD_TOLERANCE.in(Degrees); - var allowedErrorDegrees = _linedUp ? holdTolerance : acquireTolerance; - - _linedUp = angleErrorDegrees <= allowedErrorDegrees; - } -} diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java deleted file mode 100644 index ca90a66..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java +++ /dev/null @@ -1,438 +0,0 @@ -package frc.robot.subsystems.shooter; - -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Seconds; - -import java.util.List; -import java.util.function.Supplier; - -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.Time; -import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.Constants.GeneralConstants; -import frc.robot.Constants.ShooterConstants; -import frc.robot.util.Utilities; -import limelight.Limelight; -import limelight.networktables.target.AprilTagFiducial; - -@Logged -public class TurretDirector -{ - public enum ShotMode - { - Idle, Track, Pass - } - - public record ShotSolution(boolean valid, Angle turretAngle, Distance distance, ShotMode mode, Pose2d targetPose, boolean hasVisionTarget, boolean inRange, boolean hubActive) - { - } - - private record MovingTrackState(Pose2d releasePose, Translation2d lookaheadTurretOrigin, Distance lookaheadDistance, Time timeOfFlight) - { - } - - private final Supplier _swerveStateSupplier; - private final Limelight _limelight; - private final LinearFilter _visionTxFilter; - private List _targetTagFilter; - private ShotSolution _currentSolution; - @Logged - private Distance _targetDistance; - @Logged - private Pose2d _targetPose; - @Logged - private boolean _limelightHasTarget; - @Logged - private boolean _hubActive; - @Logged - private boolean _targetValid; - @Logged - private Angle _desiredTurretAngle; - @Logged - private Pose2d _predictedReleasePose; - @Logged - private Pose2d _lookaheadTurretOriginPose; - @Logged - private Time _shotTimeOfFlight; - @Logged - private Angle _visionTxRaw; - @Logged - private Angle _visionTxFiltered; - @Logged - private Angle _visionTxApplied; - @Logged - private int _selectedVisionTagId; - @Logged - private Angle _poseOnlyTrackAngle; - @Logged - private Angle _poseVisionTrackDelta; - @Logged - private Angle _empiricalDriftCorrection; - @Logged - private double _empiricalDriftLateralErrorInches; - @Logged - private double _empiricalDriftSign; - @Logged - private Angle _trackCommandDelta; - - public TurretDirector(Supplier swerveStateSupplier) - { - var txFilterWindowSamples = Math.max(1, (int)Math.round(ShooterConstants.TURRET_VISION_TX_FILTER_WINDOW.in(Seconds) / GeneralConstants.LOOP_PERIOD.in(Seconds))); - _swerveStateSupplier = swerveStateSupplier; - _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); - _visionTxFilter = LinearFilter.movingAverage(txFilterWindowSamples); - _targetTagFilter = List.of(); - _currentSolution = new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false, false, false); - _targetDistance = Meters.zero(); - _targetPose = new Pose2d(); - _limelightHasTarget = false; - _hubActive = false; - _targetValid = false; - _desiredTurretAngle = ShooterConstants.TURRET_HOME_ANGLE; - _predictedReleasePose = new Pose2d(); - _lookaheadTurretOriginPose = new Pose2d(); - _shotTimeOfFlight = Seconds.zero(); - _visionTxRaw = Degrees.zero(); - _visionTxFiltered = Degrees.zero(); - _visionTxApplied = Degrees.zero(); - _selectedVisionTagId = -1; - _poseOnlyTrackAngle = ShooterConstants.TURRET_HOME_ANGLE; - _poseVisionTrackDelta = Degrees.zero(); - _empiricalDriftCorrection = Degrees.zero(); - _empiricalDriftLateralErrorInches = 0.0; - _empiricalDriftSign = ShooterConstants.TURRET_LINEAR_DRIFT_CORRECTION_SIGN; - _trackCommandDelta = Degrees.zero(); - - _limelight.getSettings().withAprilTagOffset(ShooterConstants.CENTER_TAG_TO_HUB_CENTER_OFFSET).save(); - updateFilter(Utilities.getOurHubTagIds(), true); - } - - public ShotSolution calculate(ShotMode shotMode) - { - return calculate(shotMode, false); - } - - public ShotSolution calculate(ShotMode shotMode, boolean useMovingShotMath) - { - var swerveState = _swerveStateSupplier.get(); - var targetTagIds = Utilities.getOurHubTagIds(); - var isBlueAlliance = Utilities.isBlueAlliance(); - - updateFilter(targetTagIds, false); - - _hubActive = Utilities.isHubActive(); - - var fiducials = new AprilTagFiducial[0]; - var results = _limelight.getLatestResults(); - - if (results.isPresent()) - { - fiducials = results.get().targets_Fiducials; - } - - var solution = calculate(shotMode, useMovingShotMath, swerveState, Utilities.getHubCoordinates(), targetTagIds, isBlueAlliance, _hubActive, fiducials); - solution = stabilizeTrackCommand(solution); - _currentSolution = solution; - _targetDistance = solution.distance(); - _targetPose = solution.targetPose(); - _limelightHasTarget = solution.hasVisionTarget(); - _targetValid = solution.valid(); - _desiredTurretAngle = solution.turretAngle(); - return solution; - } - - ShotSolution calculate(ShotMode shotMode, SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean isBlueAlliance, boolean hubActive, AprilTagFiducial... fiducials) - { - return calculate(shotMode, false, swerveState, hubCoordinates, targetTagIds, isBlueAlliance, hubActive, fiducials); - } - - ShotSolution calculate(ShotMode shotMode, boolean useMovingShotMath, SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean isBlueAlliance, boolean hubActive, AprilTagFiducial... fiducials) - { - return switch (shotMode) - { - case Track -> useMovingShotMath ? calculateMovingTrack(swerveState, hubCoordinates, targetTagIds, hubActive, fiducials) : calculateTrack(swerveState, hubCoordinates, targetTagIds, hubActive, fiducials); - case Pass -> calculatePass(swerveState, isBlueAlliance); - case Idle -> createIdleSolution(swerveState.Pose); - }; - } - - private ShotSolution stabilizeTrackCommand(ShotSolution solution) - { - if (solution.mode() != ShotMode.Track || _currentSolution.mode() != ShotMode.Track || !_currentSolution.valid()) - { - _trackCommandDelta = Degrees.zero(); - return solution; - } - - var requestedDeg = solution.turretAngle().in(Degrees); - var previousDeg = _currentSolution.turretAngle().in(Degrees); - var deltaDeg = MathUtil.inputModulus(requestedDeg - previousDeg, -180.0, 180.0); - _trackCommandDelta = Degrees.of(deltaDeg); - - var deadbandDeg = ShooterConstants.TURRET_TRACK_COMMAND_DEADBAND.in(Degrees); - - if (Math.abs(deltaDeg) <= deadbandDeg) - { - return new ShotSolution(solution.valid(), _currentSolution.turretAngle(), solution.distance(), solution.mode(), solution.targetPose(), solution.hasVisionTarget(), solution.inRange(), solution.hubActive()); - } - - var maxStepDeg = ShooterConstants.TURRET_TRACK_COMMAND_MAX_STEP_PER_LOOP.in(Degrees); - var stepDeg = MathUtil.clamp(deltaDeg, -maxStepDeg, maxStepDeg); - var stableDeg = previousDeg + stepDeg; - - return new ShotSolution(solution.valid(), Degrees.of(stableDeg), solution.distance(), solution.mode(), solution.targetPose(), solution.hasVisionTarget(), solution.inRange(), solution.hubActive()); - } - - private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean hubActive, AprilTagFiducial... fiducials) - { - var turretReleaseField = getPointInField(swerveState.Pose, ShooterConstants.ROBOT_TO_TURRET_RELEASE); - var releaseToHubField = hubCoordinates.minus(turretReleaseField); - var distance = Meters.of(releaseToHubField.getNorm()); - var rawSetpoint = getTurretSetpointForFieldTarget(swerveState.Pose, releaseToHubField.getAngle().getMeasure()); - _poseOnlyTrackAngle = rawSetpoint; - var bestTag = selectBestTag(targetTagIds, fiducials); - rawSetpoint = applyVisionTrim(rawSetpoint, bestTag); - rawSetpoint = rawSetpoint.plus(ShooterConstants.TURRET_TRACK_BIAS); - rawSetpoint = rawSetpoint.plus(getEmpiricalLinearDriftCorrection(rawSetpoint, distance)); - _poseVisionTrackDelta = rawSetpoint.minus(_poseOnlyTrackAngle); - - logShotKinematics(swerveState.Pose, turretReleaseField, distance, ShooterConstants.getShotTimeOfFlight(distance)); - - return new ShotSolution(true, rawSetpoint, distance, ShotMode.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), bestTag != null, true, hubActive); - } - - private Angle applyVisionTrim(Angle rawSetpoint, AprilTagFiducial bestTag) - { - if (bestTag == null) - { - _visionTxRaw = Degrees.zero(); - _visionTxFiltered = Degrees.zero(); - _visionTxApplied = Degrees.zero(); - _visionTxFilter.reset(); - return rawSetpoint; - } - - var rawTxDegrees = bestTag.tx; - _visionTxRaw = Degrees.of(rawTxDegrees); - _visionTxFiltered = Degrees.of(_visionTxFilter.calculate(rawTxDegrees)); - - if (!ShooterConstants.TURRET_USE_VISION_TX_TRIM) - { - _visionTxApplied = Degrees.zero(); - return rawSetpoint; - } - - var desiredTxDegrees = _visionTxFiltered.in(Degrees); - - if (MathUtil.isNear(0.0, desiredTxDegrees, ShooterConstants.TURRET_TRACK_TX_DEADBAND.in(Degrees))) - { - desiredTxDegrees = 0.0; - } - - var previousTxDegrees = _visionTxApplied.in(Degrees); - var deltaTxDegrees = desiredTxDegrees - previousTxDegrees; - var maxStepDegrees = ShooterConstants.TURRET_VISION_TX_MAX_STEP_PER_LOOP.in(Degrees); - var stepTxDegrees = MathUtil.clamp(deltaTxDegrees, -maxStepDegrees, maxStepDegrees); - _visionTxApplied = Degrees.of(previousTxDegrees + stepTxDegrees); - - return rawSetpoint.plus(_visionTxApplied); - } - - private Angle getEmpiricalLinearDriftCorrection(Angle commandedAngle, Distance distance) - { - if (!ShooterConstants.TURRET_USE_LINEAR_DRIFT_COMPENSATION) - { - _empiricalDriftCorrection = Degrees.zero(); - _empiricalDriftLateralErrorInches = 0.0; - return Degrees.zero(); - } - - _empiricalDriftSign = ShooterConstants.TURRET_LINEAR_DRIFT_CORRECTION_SIGN; - - var commandedDegrees = commandedAngle.in(Degrees); - var rawLateralOffsetInches = ShooterConstants.TURRET_DRIFT_LATERAL_BIAS.in(edu.wpi.first.units.Units.Inches) - + ShooterConstants.TURRET_DRIFT_LATERAL_SINE_AMPLITUDE.in(edu.wpi.first.units.Units.Inches) * Math.sin(Math.toRadians(commandedDegrees + ShooterConstants.TURRET_DRIFT_LATERAL_PHASE_OFFSET.in(Degrees))); - var signedLateralOffsetInches = _empiricalDriftSign * rawLateralOffsetInches; - var distanceMeters = Math.max(distance.in(Meters), 0.25); - var signedCorrectionDegrees = Math.toDegrees(Math.atan2(Units.inchesToMeters(signedLateralOffsetInches), distanceMeters)); - var maxCorrectionDeg = ShooterConstants.TURRET_LINEAR_DRIFT_MAX_CORRECTION.in(Degrees); - signedCorrectionDegrees = MathUtil.clamp(signedCorrectionDegrees, -maxCorrectionDeg, maxCorrectionDeg); - - _empiricalDriftLateralErrorInches = signedLateralOffsetInches; - _empiricalDriftCorrection = Degrees.of(signedCorrectionDegrees); - return _empiricalDriftCorrection; - } - - private ShotSolution calculatePass(SwerveDriveState swerveState, boolean isBlueAlliance) - { - Angle fieldTargetAngle; - Distance distance; - - if (isBlueAlliance) - { - distance = swerveState.Pose.getMeasureX(); - fieldTargetAngle = Degrees.of(180); - } - else - { - distance = GeneralConstants.FIELD_SIZE_X.minus(swerveState.Pose.getMeasureX()); - fieldTargetAngle = Degrees.zero(); - } - - var rawSetpoint = fieldTargetAngle.minus(swerveState.Pose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); - - logShotKinematics(swerveState.Pose, getPointInField(swerveState.Pose, ShooterConstants.ROBOT_TO_TURRET_PIVOT), distance, ShooterConstants.getShotTimeOfFlight(distance)); - - return new ShotSolution(true, rawSetpoint, distance, ShotMode.Pass, buildTargetPose(swerveState.Pose, distance, rawSetpoint), false, true, true); - } - - private ShotSolution calculateMovingTrack(SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean hubActive, AprilTagFiducial... fiducials) - { - var movingTrackState = calculateMovingTrackState(swerveState, hubCoordinates); - var lookaheadToHub = hubCoordinates.minus(movingTrackState.lookaheadTurretOrigin()); - var rawSetpoint = getTurretSetpointForFieldTarget(movingTrackState.releasePose(), lookaheadToHub.getAngle().getMeasure()); - var bestTag = selectBestTag(targetTagIds, fiducials); - rawSetpoint = rawSetpoint.plus(ShooterConstants.TURRET_TRACK_BIAS); - _poseVisionTrackDelta = Degrees.zero(); - _empiricalDriftCorrection = Degrees.zero(); - _empiricalDriftLateralErrorInches = 0.0; - - logShotKinematics(movingTrackState.releasePose(), movingTrackState.lookaheadTurretOrigin(), movingTrackState.lookaheadDistance(), movingTrackState.timeOfFlight()); - - var inRange = ShooterConstants.isMovingShotDistanceValid(movingTrackState.lookaheadDistance()); - - return new ShotSolution( - inRange, rawSetpoint, movingTrackState.lookaheadDistance(), ShotMode.Track, buildTargetPose(movingTrackState.releasePose(), movingTrackState.lookaheadDistance(), rawSetpoint), bestTag != null, inRange, hubActive - ); - } - - private Pose2d buildTargetPose(Pose2d robotPose, Distance distance, Angle rawSetpoint) - { - var forwardTranslation = new Translation2d(distance, Meters.zero()); - var rotatedByTurretOffset = forwardTranslation.rotateBy(new Rotation2d(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD)); - var rotatedBySetpoint = rotatedByTurretOffset.rotateBy(new Rotation2d(rawSetpoint)); - - return robotPose.plus(new Transform2d(rotatedBySetpoint, new Rotation2d())); - } - - private Angle getTurretSetpointForFieldTarget(Pose2d robotPose, Angle fieldTargetAngle) - { - return fieldTargetAngle.minus(robotPose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); - } - - private MovingTrackState calculateMovingTrackState(SwerveDriveState swerveState, Translation2d hubCoordinates) - { - var releasePose = predictReleasePose(swerveState.Pose, swerveState.Speeds); - var turretOriginField = getPointInField(releasePose, ShooterConstants.ROBOT_TO_TURRET_RELEASE); - var turretPointVelocity = getTurretPointVelocityField(releasePose, swerveState.Speeds, ShooterConstants.ROBOT_TO_TURRET_RELEASE); - var lookaheadOrigin = turretOriginField; - var lookaheadDistance = Meters.of(hubCoordinates.getDistance(lookaheadOrigin)); - var timeOfFlight = ShooterConstants.getShotTimeOfFlight(lookaheadDistance); - - for (int i = 0; i < ShooterConstants.SWM_LOOKAHEAD_ITERATIONS; i++) - { - lookaheadOrigin = turretOriginField.plus(turretPointVelocity.times(timeOfFlight.in(Seconds))); - lookaheadDistance = Meters.of(hubCoordinates.getDistance(lookaheadOrigin)); - timeOfFlight = ShooterConstants.getShotTimeOfFlight(lookaheadDistance); - } - - return new MovingTrackState(releasePose, lookaheadOrigin, lookaheadDistance, timeOfFlight); - } - - private Pose2d predictReleasePose(Pose2d robotPose, ChassisSpeeds robotRelativeSpeeds) - { - var phaseDelaySeconds = ShooterConstants.SWM_RELEASE_PHASE_DELAY.in(Seconds); - - return robotPose.exp(new Twist2d(robotRelativeSpeeds.vxMetersPerSecond * phaseDelaySeconds, robotRelativeSpeeds.vyMetersPerSecond * phaseDelaySeconds, robotRelativeSpeeds.omegaRadiansPerSecond * phaseDelaySeconds)); - } - - private Translation2d getPointInField(Pose2d robotPose, Translation2d robotRelativePoint) - { - return robotPose.getTranslation().plus(robotRelativePoint.rotateBy(robotPose.getRotation())); - } - - private Translation2d getTurretPointVelocityField(Pose2d robotPose, ChassisSpeeds robotRelativeSpeeds, Translation2d robotRelativeOffset) - { - var fieldRelativeRobotVelocity = new Translation2d(robotRelativeSpeeds.vxMetersPerSecond, robotRelativeSpeeds.vyMetersPerSecond).rotateBy(robotPose.getRotation()); - var fieldRelativeOffset = robotRelativeOffset.rotateBy(robotPose.getRotation()); - var omegaCrossR = new Translation2d(-robotRelativeSpeeds.omegaRadiansPerSecond * fieldRelativeOffset.getY(), robotRelativeSpeeds.omegaRadiansPerSecond * fieldRelativeOffset.getX()); - - return fieldRelativeRobotVelocity.plus(omegaCrossR); - } - - private ShotSolution createIdleSolution(Pose2d robotPose) - { - logShotKinematics(robotPose, getPointInField(robotPose, ShooterConstants.ROBOT_TO_TURRET_PIVOT), Meters.zero(), Seconds.zero()); - return new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, robotPose, false, false, false); - } - - private void logShotKinematics(Pose2d releasePose, Translation2d lookaheadTurretOrigin, Distance distance, Time timeOfFlight) - { - _predictedReleasePose = releasePose; - _lookaheadTurretOriginPose = new Pose2d(lookaheadTurretOrigin, new Rotation2d()); - _targetDistance = distance; - _shotTimeOfFlight = timeOfFlight; - } - - private AprilTagFiducial selectBestTag(List targetTagIds, AprilTagFiducial... fiducials) - { - AprilTagFiducial bestTag = null; - AprilTagFiducial lockedTag = null; - - for (AprilTagFiducial tag : fiducials) - { - var tagId = (int)Math.round(tag.fiducialID); - - if (!targetTagIds.contains(tagId)) - { - continue; - } - - if (tagId == _selectedVisionTagId) - { - lockedTag = tag; - } - - if (bestTag == null || tag.ta > bestTag.ta || tag.ta == bestTag.ta && Math.abs(tag.tx) < Math.abs(bestTag.tx)) - { - bestTag = tag; - } - } - - var selectedTag = lockedTag != null ? lockedTag : bestTag; - - if (selectedTag == null) - { - _selectedVisionTagId = -1; - return null; - } - - _selectedVisionTagId = (int)Math.round(selectedTag.fiducialID); - return selectedTag; - } - - private void updateFilter(List filters, boolean force) - { - if (!force && (!DriverStation.isDisabled() || filters.equals(_targetTagFilter))) - { - return; - } - - _limelight.getSettings().withAprilTagIdFilter(filters).save(); - _targetTagFilter = List.copyOf(filters); - } -} diff --git a/src/main/java/frc/robot/util/Utilities.java b/src/main/java/frc/robot/util/Utilities.java index fda4d6f..0eb5f4a 100644 --- a/src/main/java/frc/robot/util/Utilities.java +++ b/src/main/java/frc/robot/util/Utilities.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.VisionConstants; /** * Utility class for reading REBUILT 2026 game data from the Driver Station / @@ -27,7 +27,7 @@ public final class Utilities private static final Time kShift2EndTimeSecs = Seconds.of(80.0); // 1:20 private static final Time kShift3EndTimeSecs = Seconds.of(55.0); // 0:55 private static final Time kShift4EndTimeSecs = Seconds.of(30.0); // 0:30 - private static final List kAllHubTagIds = Stream.concat(ShooterConstants.RED_HUB_TAG_IDS.stream(), ShooterConstants.BLUE_HUB_TAG_IDS.stream()).toList(); + private static final List kAllHubTagIds = Stream.concat(VisionConstants.RED_HUB_TAG_IDS.stream(), VisionConstants.BLUE_HUB_TAG_IDS.stream()).toList(); private Utilities() { @@ -94,9 +94,9 @@ public static List getOurHubTagIds() if (alliance.isEmpty()) return kAllHubTagIds; if (isRedAlliance()) { - return ShooterConstants.RED_HUB_TAG_IDS; + return VisionConstants.RED_HUB_TAG_IDS; } - return ShooterConstants.BLUE_HUB_TAG_IDS; + return VisionConstants.BLUE_HUB_TAG_IDS; } private static int getAllianceShift(Time timeRemaining) @@ -121,7 +121,7 @@ private static Alliance getInactiveFirstAlliance(String gameData) public static Translation2d getHubCoordinates() { - return isBlueAlliance() ? ShooterConstants.BLUE_HUB : ShooterConstants.RED_HUB; + return isBlueAlliance() ? VisionConstants.BLUE_HUB : VisionConstants.RED_HUB; } /**