diff --git a/documentation/Rebuilt-TurretTargetTriangulation.png b/documentation/Rebuilt-TurretTargetTriangulation.png new file mode 100644 index 0000000..5a93e1d Binary files /dev/null and b/documentation/Rebuilt-TurretTargetTriangulation.png differ diff --git a/documentation/Rebuilt.drawio b/documentation/Rebuilt.drawio index 0bcb485..57746bc 100644 --- a/documentation/Rebuilt.drawio +++ b/documentation/Rebuilt.drawio @@ -1,6 +1,6 @@ - - - + + + @@ -388,4 +388,143 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f338453..cb304af 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,33 +1,19 @@ package frc.robot; -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.Inches; -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.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.Value; -import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.*; import java.util.List; import java.util.Map; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; - import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; 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.math.system.plant.DCMotor; import edu.wpi.first.units.AngleUnit; import edu.wpi.first.units.AngularAccelerationUnit; import edu.wpi.first.units.AngularVelocityUnit; @@ -42,7 +28,6 @@ import edu.wpi.first.units.measure.Per; import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.math.system.plant.DCMotor; import frc.robot.generated.TunerConstants; public final class Constants @@ -157,6 +142,33 @@ public static class ShooterConstants Map.entry(7.0, 5000.0) ); // @formatter:on + // Pot calibration model: + // - MIN/MAX volts are measured at conceptual turret min/max angle. + // - ZERO_OFFSET shifts the conceptual 0-deg heading without changing hard + // limits. + public static final double TURRET_POTENTIOMETER_MIN_VOLTS = 0.35; // TODO: Calibrate pot voltage at min turret angle + public static final double TURRET_POTENTIOMETER_MAX_VOLTS = 4.65; // TODO: Calibrate pot voltage at max turret angle + public static final double TURRET_POTENTIOMETER_ZERO_OFFSET_DEG = 0.0; // TODO: Calibrate conceptual turret zero offset (degrees) + // Dead-zone model (robot-relative): + // - Center/width define forbidden cable-wrap sector. + // - Width <= 0 disables dead-zone routing logic. + public static final double TURRET_DEAD_ZONE_CENTER = 180.0; // degrees (robot-relative) + public static final double TURRET_DEAD_ZONE_WIDTH = 0.0; // degrees; set > 0 once cable-wrap dead-zone is measured + public static final List> BLUE_HUB_CENTER_OFFSET_TAG_PAIRS = List.of(List.of(18, 27), List.of(20, 19), List.of(21, 24), List.of(26, 25)); + public static final List> RED_HUB_CENTER_OFFSET_TAG_PAIRS = List.of(List.of(2, 11), List.of(4, 3), List.of(5, 8), List.of(10, 9)); + public static final double TURRET_CL_METERS = 0.3556; // 14.00 in + public static final double TURRET_CH_METERS = 0.5969; // 23.50 ins + // @formatter:off + private static final InterpolatingDoubleTreeMap HOOD_ANGLE_TABLE = InterpolatingDoubleTreeMap.ofEntries + ( + Map.entry(0.0, 20.0), + Map.entry(2.0, 15.0), + Map.entry(3.5, 22.0), + Map.entry(5.0, 30.0), + Map.entry(6.5, 38.0), + Map.entry(7.0, 45.0) + ); + // @formatter:on // Feeder public static final Current FEEDER_CURRENT_LIMIT = Amps.of(60); @@ -167,6 +179,13 @@ public static AngularVelocity getFlywheelSpeedForDistance(Distance distance) { return RPM.of(FLYWHEEL_SPEED_TABLE.get(distance.in(Meters))); } + + // TODO: Tune these values with testing! + + public static double getHoodAngleForDistance(double meters) + { + return HOOD_ANGLE_TABLE.get(meters); + } } public static class VisionConstants diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0a5e38e..fa2879c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,6 @@ import static edu.wpi.first.units.Units.Value; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; - import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.epilogue.Logged; @@ -24,8 +23,9 @@ import frc.robot.subsystems.Drive; import frc.robot.subsystems.TestOperation; import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.Hood.HoodPosition; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.Turret; import frc.robot.util.MeasureUtil; @Logged @@ -43,8 +43,10 @@ public class RobotContainer private final Intake _intake = new Intake(); private final Shooter _shooter = new Shooter(_drivetrain::getState); private final TestOperation _testop = new TestOperation(); + private final CommandXboxController _joystick = new CommandXboxController(0); // TODO: Check if this can be removed // private final Turret _turret = new Turret(_drivetrain::getState); - private final Autos _autos = new Autos(_drivetrain); + private final Autos _autos = new Autos(_drivetrain); + private final Turret _turret = new Turret(_drivetrain::getState); public RobotContainer() { @@ -94,6 +96,12 @@ private void configureBindings() _operator.rightTrigger().onTrue(_shooter.fireCmd()); // Reset the field-centric heading on left bumper press. _driver.povDown().onTrue(_drivetrain.runOnce(_drivetrain::seedFieldCentric)); + _joystick.leftBumper().onTrue(_drivetrain.runOnce(_drivetrain::seedFieldCentric)); + + // Turret simulation bindings. + _joystick.leftTrigger().whileTrue(_turret.getTrackCmd()); + _joystick.y().whileTrue(_turret.getPassCmd()); + _joystick.leftStick().onTrue(_turret.getIdleCmd()); _drivetrain.registerTelemetry(_logger::telemeterize); diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index bc0e0c3..140f222 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -8,7 +8,9 @@ import static edu.wpi.first.units.Units.Value; import static edu.wpi.first.units.Units.Volts; +import java.util.HashMap; import java.util.List; +import java.util.Map; import java.util.function.Supplier; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -17,33 +19,34 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.AnalogPotentiometer; -import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.AnalogInputSim; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.MotorHook; -import frc.robot.TestHook; import frc.robot.Constants.AIOConstants; import frc.robot.Constants.CANConstants; import frc.robot.Constants.GeneralConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.MotorHook; +import frc.robot.TestHook; import frc.robot.util.MeasureUtil; import frc.robot.util.Utilities; import limelight.Limelight; +import limelight.results.RawFiducial; @Logged public class Turret extends SubsystemBase @@ -53,37 +56,84 @@ public enum TurretState Idle, Track, Pass } - private final TalonFX _turretMotor; - private final TalonFXSimState _turretMotorSim; - private final DCMotorSim _motorSimModel; - private final AnalogPotentiometer _turretSensor; - private final AnalogInputSim _turretSensorSim; - private final Limelight _limelight; - private Supplier _swerveStateSupplier; - private SwerveDriveState _swerveDriveState; - private PositionVoltage _positionRequest = new PositionVoltage(0).withSlot(0); - private List _cachedTagFilter; + public Command getTrackCmd() + { + return startEnd(() -> setTurretState(TurretState.Track), () -> setTurretState(TurretState.Idle)); + } + + public Command getPassCmd() + { + return startEnd(() -> setTurretState(TurretState.Pass), () -> setTurretState(TurretState.Idle)); + } + + public Command getIdleCmd() + { + return runOnce(() -> setTurretState(TurretState.Idle)); + } + + private final DCMotor gearbox = DCMotor.getKrakenX44(1); // FIXME: This is wrong. Find right value to make it right. + private final TalonFX _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); + private final TalonFXSimState _turretMotorSim = _turretMotor.getSimState(); + private final DCMotorSim _motorSimModel = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.001, ShooterConstants.TURRET_GEAR_RATIO.in(Value)), gearbox);; + private final AnalogInput _turretSensorInput = new AnalogInput(AIOConstants.TURRET_POTENTIOMETER);; + private final AnalogPotentiometer _turretSensor = new AnalogPotentiometer( + _turretSensorInput, ShooterConstants.TURRET_MAX_ANGLE.minus(ShooterConstants.TURRET_MIN_ANGLE).in(Degrees), ShooterConstants.TURRET_MIN_ANGLE.in(Degrees) + ); // TODO: check against _turretSensor = new + // AnalogPotentiometer(_turretSensorInput, getTurretPotFullRange(), + // getTurretPotOffset()); + private final AnalogInputSim _turretSensorSim = new AnalogInputSim(_turretSensorInput); + private final Limelight _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); + private final Supplier _swerveStateSupplier; + private final PositionVoltage _positionRequest = new PositionVoltage(0).withSlot(0); + private List _cachedTagFilter; + private final Angle _defaultSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + private SwerveDriveState _swerveDriveState; + @Logged + private Angle _fieldTurretAngle; + @Logged + private Angle _robotTurretAngle; + @Logged + private double _continuousRobotAngleDeg; @Logged - private Angle _fieldTurretAngle; + private double _continuousTurretSetpointDeg; @Logged - private Angle _robotTurretAngle; + private Angle _turretSetpoint; @Logged - private Angle _turretSetpoint; + private boolean _hasSetpoint; @Logged - private boolean _hasSetpoint; + private Voltage _turretMotorVoltage; @Logged - private Voltage _turretMotorVoltage; + private TurretState _turretState; @Logged - private TurretState _turretState; + private boolean _hasTarget; @Logged - private boolean _hasTarget; + private Angle _targetHorizontalOffset; @Logged - private Angle _targetHorizontalOffset; + private double _distanceToHubMeters; + private TurretDirector.TagObservation _centerTagObservation; + private TurretDirector.TagObservation _offsetTagObservation; + private double _lastWrappedRobotAngleDeg; public Turret(Supplier swerveStateSupplier) { - // Setting up motor - _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); + _swerveStateSupplier = swerveStateSupplier; + _swerveDriveState = new SwerveDriveState(); + _fieldTurretAngle = Degrees.zero(); + _robotTurretAngle = Degrees.zero(); + _turretSetpoint = _defaultSetpoint; + _hasSetpoint = false; + _turretMotorVoltage = Volts.zero(); + _turretState = TurretState.Idle; + _hasTarget = false; + _targetHorizontalOffset = Degrees.zero(); + _distanceToHubMeters = 0.0; + _cachedTagFilter = List.of(); + _centerTagObservation = null; + _offsetTagObservation = null; + _continuousRobotAngleDeg = 0.0; + _continuousTurretSetpointDeg = 0.0; + _lastWrappedRobotAngleDeg = 0.0; + var currentConfig = new CurrentLimitsConfigs(); currentConfig.StatorCurrentLimit = ShooterConstants.TURRET_CURRENT_LIMIT.in(Amps); currentConfig.StatorCurrentLimitEnable = true; @@ -99,89 +149,74 @@ public Turret(Supplier swerveStateSupplier) _turretMotor.getConfigurator().apply(new TalonFXConfiguration().withCurrentLimits(currentConfig).withMotorOutput(outputConfig).withSlot0(slot0Configs)); AnalogInput turretSensorInput = new AnalogInput(AIOConstants.TURRET_POTENTIOMETER); - _turretSensor = new AnalogPotentiometer(turretSensorInput, ShooterConstants.TURRET_MAX_ANGLE.minus(ShooterConstants.TURRET_MIN_ANGLE).in(Degrees), ShooterConstants.TURRET_MIN_ANGLE.in(Degrees)); - _positionRequest = new PositionVoltage(0).withSlot(0); - _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); - - _robotTurretAngle = Degrees.of(0.0); - _fieldTurretAngle = Degrees.of(0.0); - _turretMotorVoltage = Volts.of(0.0); - _turretState = TurretState.Idle; - _hasTarget = false; - _targetHorizontalOffset = Degrees.of(0.0); - _turretSetpoint = Degrees.zero(); - _hasSetpoint = false; - _swerveStateSupplier = (swerveStateSupplier == null) ? () -> new SwerveDriveState() : swerveStateSupplier; - _swerveDriveState = new SwerveDriveState(); - _cachedTagFilter = List.of(); + syncMotorEncoderToPotentiometer(); - if (RobotBase.isReal()) - { - _turretMotorSim = null; - _motorSimModel = null; - _turretSensorSim = null; - } - else - { - _turretMotorSim = _turretMotor.getSimState(); - _turretSensorSim = new AnalogInputSim(turretSensorInput); - var gearbox = DCMotor.getKrakenX44(1); - _motorSimModel = new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.001, ShooterConstants.TURRET_GEAR_RATIO.in(Value)), gearbox); - } } + /** + * This function executes automatically every 20 milliseconds. For the turret + * specifically, we need to ensure the turret is behaving according to the rules + * defined by the state the turret is in. If in Idle, the turret + * shouldn't rotate at all, even if the robot rotates. If in Track + * or Pass, the turret should rotate to whatever angle is provided + * by the + * TurretDirector class. + */ @Override public void periodic() { - _robotTurretAngle = Degrees.of(_turretSensor.get()); - - SwerveDriveState state = _swerveStateSupplier.get(); + SwerveDriveState state = _swerveStateSupplier == null ? null : _swerveStateSupplier.get(); if (state != null) { _swerveDriveState = state; } - _fieldTurretAngle = _robotTurretAngle.plus(_swerveDriveState.Pose.getRotation().getMeasure()); - _turretMotorVoltage = _turretMotor.getMotorVoltage().getValue(); - if (RobotBase.isReal()) - { - List desiredTags = Utilities.getOurHubTagIds(); - if (!_cachedTagFilter.equals(desiredTags)) - { - _limelight.getSettings().withAprilTagIdFilter(desiredTags).save(); - _cachedTagFilter = List.copyOf(desiredTags); - } + Angle robotHeading = _swerveDriveState.Pose.getRotation().getMeasure(); + _robotTurretAngle = getCalibratedRobotAngle(); + updateContinuousRobotAngle(_robotTurretAngle.in(Degrees)); + _fieldTurretAngle = _robotTurretAngle.plus(robotHeading); + _turretMotorVoltage = _turretMotor.getMotorVoltage().getValue(); - var targetData = _limelight.getData().targetData; - _hasTarget = targetData.getTargetStatus(); - _targetHorizontalOffset = Degrees.of(targetData.getHorizontalOffset()); - } + updateVisionData(); - Angle requestedSetpoint = switch (_turretState) - { - case Idle -> null; - case Track -> _hasTarget ? _robotTurretAngle.plus(_targetHorizontalOffset) : ShooterConstants.TURRET_HOME_ANGLE; - case Pass -> ShooterConstants.TURRET_PASS_TARGET.minus(_swerveDriveState.Pose.getRotation().getMeasure()); - }; + TurretDirector.DirectorContext directorContext = new TurretDirector.DirectorContext(_turretState, _fieldTurretAngle, robotHeading, _targetHorizontalOffset, _hasTarget, _centerTagObservation, _offsetTagObservation); + TurretDirector.TurretAimSolution aimSolution = TurretDirector.getAimSolution(directorContext); - if (requestedSetpoint == null) + if (!aimSolution.hasSetpoint()) { - _turretSetpoint = null; - _turretMotor.setVoltage(0.0); + // This occured while debugging merges. //TODO: Remove this if it safely can be. + // case Idle -> null; + // case Track -> _hasTarget ? _robotTurretAngle.plus(_targetHorizontalOffset) : + // ShooterConstants.TURRET_HOME_ANGLE; + // case Pass -> + // ShooterConstants.TURRET_PASS_TARGET.minus(_swerveDriveState.Pose.getRotation().getMeasure()); + stopTurret(); return; - } - - _turretSetpoint = MeasureUtil.clamp(requestedSetpoint, ShooterConstants.TURRET_MIN_ANGLE, ShooterConstants.TURRET_MAX_ANGLE); + } ; + + // if (requestedSetpoint == null) + // { + // _turretSetpoint = null; + // _turretMotor.setVoltage(0.0); + // return; + // } + + // _turretSetpoint = MeasureUtil.clamp(requestedSetpoint, + // ShooterConstants.TURRET_MIN_ANGLE, ShooterConstants.TURRET_MAX_ANGLE); + // //TODO: Check this var target = _turretSetpoint.times(ShooterConstants.TURRET_GEAR_RATIO); _turretMotor.setControl(_positionRequest.withPosition(target.in(Rotations))); } + /** + * Any logic needed to specifically manage any hardware interactions when the + * robot code is executing in the simulation (for example, update the 10-turn + * potentiometer's angle based on how the motor driving the turret is moving) + * should go here. + */ @Override public void simulationPeriodic() { - _hasTarget = false; - _targetHorizontalOffset = Degrees.of(0.0); - _turretMotorSim.setSupplyVoltage(RoboRioSim.getVInVoltage()); Voltage motorVoltage = _turretMotorSim.getMotorVoltageMeasure(); @@ -191,44 +226,88 @@ public void simulationPeriodic() _turretMotorSim.setRawRotorPosition(_motorSimModel.getAngularPosition().times(ShooterConstants.TURRET_GEAR_RATIO)); _turretMotorSim.setRotorVelocity(_motorSimModel.getAngularVelocity().times(ShooterConstants.TURRET_GEAR_RATIO)); - var mechanismAngle = _motorSimModel.getAngularPosition().div(ShooterConstants.TURRET_GEAR_RATIO); - Angle clamped = MeasureUtil.clamp(mechanismAngle, ShooterConstants.TURRET_MIN_ANGLE, ShooterConstants.TURRET_MAX_ANGLE); - var normalized = clamped.minus(ShooterConstants.TURRET_MIN_ANGLE).div(ShooterConstants.TURRET_MAX_ANGLE.minus(ShooterConstants.TURRET_MIN_ANGLE)); - _turretSensorSim.setVoltage(RoboRioSim.getUserVoltage5V() * normalized.in(Value)); + double mechanismAngle = _motorSimModel.getAngularPosition().div(ShooterConstants.TURRET_GEAR_RATIO).in(Degrees); // TODO: Quickly glance over this line to ensure it's correct. + Angle clamped = MeasureUtil.clamp(Degrees.of(mechanismAngle), ShooterConstants.TURRET_MIN_ANGLE, ShooterConstants.TURRET_MAX_ANGLE); + double normalized = clamped.in(Degrees) - (ShooterConstants.TURRET_MIN_ANGLE.in(Degrees)) / (ShooterConstants.TURRET_MAX_ANGLE.in(Degrees) - (ShooterConstants.TURRET_MIN_ANGLE.in(Degrees))); // TODO: Do this in the actually + // better way. + _turretSensorSim.setVoltage(RoboRioSim.getUserVoltage5V() * normalized); } + /** + * Sets the desired state of the turret. + * + * @param state The desired state of the turret + */ public void setTurretState(TurretState state) { - _turretState = state; + _turretState = state == null ? TurretState.Idle : state; } + /** + * Gets the current state of the turret. + * + * @return The current state of the turret + */ public TurretState getTurretState() { return _turretState; } - public boolean hasTarget() + /** + * Gets the current angle of the turret. Angle is given in field-relative space. + * + * @return The current angle of the turret + */ + public Angle getAngle() { - return _hasTarget; + return _fieldTurretAngle; } + /** + * Gets whether the turret is currently pointing at its desired target. The + * target to point at is determined by the state of the turret. + * + * @return true if the turret is facing the desired target location + * (or if there is no desired location), otherwise false. + */ public boolean isLinedUp() { - if (_turretSetpoint == null) return _turretState == TurretState.Idle; - if (_turretState == TurretState.Track && !_hasTarget) return false; + if (!_hasSetpoint || _turretSetpoint == null) + { + return _turretState == TurretState.Idle; // This originally returned false, new version should work better + } + + if (_turretState == TurretState.Track && !_hasTarget) + { + return false; + } + + return Math.abs(_continuousRobotAngleDeg - _continuousTurretSetpointDeg) <= ShooterConstants.TURRET_TOLERANCE.in(Degrees); + // This could also be the below line if that works better + // TODO: check which one is better. + // return _robotTurretAngle.isNear(_turretSetpoint, + // ShooterConstants.TURRET_TOLERANCE); + } - return _robotTurretAngle.isNear(_turretSetpoint, ShooterConstants.TURRET_TOLERANCE); + public boolean hasTarget() + { + return _hasTarget; } - public Distance getDistanceToTarget() + public double getDistanceToTarget() { - if (RobotBase.isSimulation() || !_hasTarget) + if (_distanceToHubMeters > 0.0) { - return Meters.zero(); + return _distanceToHubMeters; + } + + if (_limelight == null || !_hasTarget) + { + return Meters.zero().in(Meters); // FIXME: Reread this, make it right. } var targetPose = _limelight.getData().targetData.getCameraToTarget(); - return Meters.of(targetPose.getTranslation().toTranslation2d().getNorm()); + return Meters.of(targetPose.getTranslation().toTranslation2d().getNorm()).in(Meters); // FIXME: Ditto } private class TurretHook extends MotorHook @@ -250,4 +329,219 @@ public TestHook getHook() { return new TurretHook(); } + + private void updateVisionData() + { + if (_limelight == null) + { + _hasTarget = false; + _targetHorizontalOffset = Degrees.zero(); + _centerTagObservation = null; + _offsetTagObservation = null; + return; + } + + List desiredTags = Utilities.getOurHubTagIds(); + if (!_cachedTagFilter.equals(desiredTags)) + { + _limelight.getSettings().withAprilTagIdFilter(desiredTags).save(); + _cachedTagFilter = List.copyOf(desiredTags); + } + + var limelightData = _limelight.getData(); + var targetData = limelightData.targetData; + _targetHorizontalOffset = Degrees.of(targetData.getHorizontalOffset()); + + boolean allianceIsRed = Utilities.isRedAlliance(); + List> hubPairs = TurretDirector.getHubCenterOffsetPairs(allianceIsRed); + Map observationsById = new HashMap<>(); + for (RawFiducial fiducial : limelightData.getRawFiducials()) + { + observationsById.put(fiducial.id, new TurretDirector.TagObservation(fiducial.id, Degrees.of(fiducial.txnc), fiducial.distToRobot)); + } + + _centerTagObservation = null; + _offsetTagObservation = null; + + double bestPairScore = Double.POSITIVE_INFINITY; + for (List hubPair : hubPairs) + { + TurretDirector.TagObservation centerCandidate = observationsById.get(hubPair.get(0)); + TurretDirector.TagObservation offsetCandidate = observationsById.get(hubPair.get(1)); + if (centerCandidate == null || offsetCandidate == null) + { + continue; + } + + double pairScore = (centerCandidate.distanceMeters() + offsetCandidate.distanceMeters()) / 2.0; + if (pairScore < bestPairScore) + { + bestPairScore = pairScore; + _centerTagObservation = centerCandidate; + _offsetTagObservation = offsetCandidate; + } + } + + if (_centerTagObservation == null) + { + double closestCenterDistance = Double.POSITIVE_INFINITY; + for (List hubPair : hubPairs) + { + TurretDirector.TagObservation centerCandidate = observationsById.get(hubPair.get(0)); + if (centerCandidate == null || centerCandidate.distanceMeters() >= closestCenterDistance) + { + continue; + } + + closestCenterDistance = centerCandidate.distanceMeters(); + _centerTagObservation = centerCandidate; + } + } + + _hasTarget = targetData.getTargetStatus() || _centerTagObservation != null; + } + + private void stopTurret() + { + _hasSetpoint = false; + _turretSetpoint = _defaultSetpoint; + _continuousTurretSetpointDeg = _continuousRobotAngleDeg; + _distanceToHubMeters = 0.0; + _turretMotor.setVoltage(0.0); + } + + private static Angle clampToTurretLimits(Angle angle) + { + return Degrees.of(MathUtil.clamp(angle.in(Degrees), ShooterConstants.TURRET_MIN_ANGLE.in(Degrees), ShooterConstants.TURRET_MAX_ANGLE.in(Degrees))); + } + + private void syncMotorEncoderToPotentiometer() + { + Angle sensorAngle = getCalibratedRobotAngle(); + _robotTurretAngle = sensorAngle; + _lastWrappedRobotAngleDeg = sensorAngle.in(Degrees); + _continuousRobotAngleDeg = _lastWrappedRobotAngleDeg; + _continuousTurretSetpointDeg = _continuousRobotAngleDeg; + + double motorRotations = _continuousRobotAngleDeg * ShooterConstants.TURRET_GEAR_RATIO.baseUnitMagnitude(); // TODO: Check if there's a better way to do this line. + _turretMotor.setPosition(motorRotations); + } + + private Angle getCalibratedRobotAngle() + { + return clampToTurretLimits(Degrees.of(_turretSensor.get())); + } + + private void updateContinuousRobotAngle(double wrappedAngleDeg) + { + double delta = MathUtil.inputModulus(wrappedAngleDeg - _lastWrappedRobotAngleDeg, -180.0, 180.0); + _continuousRobotAngleDeg += delta; + _lastWrappedRobotAngleDeg = wrappedAngleDeg; + } + + private double chooseContinuousSetpoint(double requestedWrappedSetpointDeg) + { + requestedWrappedSetpointDeg = moveSetpointOutOfDeadZone(requestedWrappedSetpointDeg); + double currentWrappedDeg = wrapDegrees(_continuousRobotAngleDeg); + double shortestDelta = MathUtil.inputModulus(requestedWrappedSetpointDeg - currentWrappedDeg, -180.0, 180.0); + double alternateDelta = shortestDelta > 0.0 ? shortestDelta - 360.0 : shortestDelta + 360.0; + + if (!pathCrossesDeadZone(currentWrappedDeg, shortestDelta)) + { + return _continuousRobotAngleDeg + shortestDelta; + } + + if (!pathCrossesDeadZone(currentWrappedDeg, alternateDelta)) + { + return _continuousRobotAngleDeg + alternateDelta; + } + + return _continuousRobotAngleDeg; + } + + private double moveSetpointOutOfDeadZone(double requestedWrappedSetpointDeg) + { + if (ShooterConstants.TURRET_DEAD_ZONE_WIDTH <= 0.0 || !isInDeadZone(requestedWrappedSetpointDeg)) + { + return requestedWrappedSetpointDeg; + } + + double deadZoneCenter = wrapDegrees(ShooterConstants.TURRET_DEAD_ZONE_CENTER); + double halfWidth = ShooterConstants.TURRET_DEAD_ZONE_WIDTH / 2.0; + double leftEdge = wrapDegrees(deadZoneCenter - halfWidth); + double rightEdge = wrapDegrees(deadZoneCenter + halfWidth); + double leftDelta = Math.abs(MathUtil.inputModulus(requestedWrappedSetpointDeg - leftEdge, -180.0, 180.0)); + double rightDelta = Math.abs(MathUtil.inputModulus(requestedWrappedSetpointDeg - rightEdge, -180.0, 180.0)); + + return leftDelta <= rightDelta ? leftEdge : rightEdge; + } + + private boolean pathCrossesDeadZone(double startWrappedDeg, double deltaDeg) + { + if (ShooterConstants.TURRET_DEAD_ZONE_WIDTH <= 0.0) + { + return false; + } + + if (Math.abs(deltaDeg) <= 1e-9) + { + return isInDeadZone(startWrappedDeg); + } + + double zoneCenter = wrapDegrees(ShooterConstants.TURRET_DEAD_ZONE_CENTER); + double halfWidth = ShooterConstants.TURRET_DEAD_ZONE_WIDTH / 2.0; + double endUnwrapped = startWrappedDeg + deltaDeg; + + double startExclusive = deltaDeg > 0.0 ? Math.nextUp(startWrappedDeg) : Math.nextDown(startWrappedDeg); + double low = Math.min(startExclusive, endUnwrapped); + double high = Math.max(startExclusive, endUnwrapped); + + // Exact interval intersection against periodic dead-zone copies (center+360k), + // which is robust across +/-180 wrap boundaries. + int kMin = (int)Math.floor((low - zoneCenter) / 360.0) - 1; + int kMax = (int)Math.ceil((high - zoneCenter) / 360.0) + 1; + for (int k = kMin; k <= kMax; k++) + { + double centerK = zoneCenter + (360.0 * k); + double intervalLow = centerK - halfWidth; + double intervalHigh = centerK + halfWidth; + if (intervalHigh >= low && intervalLow <= high) + { + return true; + } + } + + return false; + } + + private boolean isInDeadZone(double wrappedAngleDeg) + { + double centerDelta = Math.abs(MathUtil.inputModulus(wrappedAngleDeg - ShooterConstants.TURRET_DEAD_ZONE_CENTER, -180.0, 180.0)); + return centerDelta <= ShooterConstants.TURRET_DEAD_ZONE_WIDTH / 2.0; + } + + private static double wrapDegrees(double angleDeg) + { + return MathUtil.inputModulus(angleDeg, -180.0, 180.0); + } + + private static double getTurretPotFullRange() + { + return ((ShooterConstants.TURRET_MAX_ANGLE.minus(ShooterConstants.TURRET_MIN_ANGLE)).in(Degrees) * GeneralConstants.SENSOR_VOLTAGE.in(Volts)) + / (ShooterConstants.TURRET_POTENTIOMETER_MAX_VOLTS - ShooterConstants.TURRET_POTENTIOMETER_MIN_VOLTS); // TODO: Check if all this conversion is needed or this can be done better. + } + + private static double getTurretPotOffset() + { + double fullRange = getTurretPotFullRange(); + return ShooterConstants.TURRET_MIN_ANGLE.in(Degrees) - ((ShooterConstants.TURRET_POTENTIOMETER_MIN_VOLTS / GeneralConstants.SENSOR_VOLTAGE.in(Volts)) * fullRange) + ShooterConstants.TURRET_POTENTIOMETER_ZERO_OFFSET_DEG; // TODO: + // Check if + // all this + // conversion + // is needed + // or this + // can be + // done + // better. + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java new file mode 100644 index 0000000..95ef60c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java @@ -0,0 +1,136 @@ +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.Degrees; + +import java.util.List; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.measure.Angle; +import frc.robot.Constants.ShooterConstants; +import frc.robot.subsystems.shooter.Turret.TurretState; + +public final class TurretDirector +{ + private TurretDirector() + { + } + + public static List> getHubCenterOffsetPairs(boolean isRedAlliance) + { + if (isRedAlliance) + { + return ShooterConstants.RED_HUB_CENTER_OFFSET_TAG_PAIRS; + } + return ShooterConstants.BLUE_HUB_CENTER_OFFSET_TAG_PAIRS; + } + + public static TurretAimSolution getAimSolution(DirectorContext context) + { + return switch (context.turretState()) + { + case Idle -> TurretAimSolution.none(); + case Track -> getTrackSolution(context); + case Pass -> TurretAimSolution.of(ShooterConstants.TURRET_PASS_TARGET, 0.0); + }; + } + + private static TurretAimSolution getTrackSolution(DirectorContext context) + { + if (context.centerTagObservation() != null && context.offsetTagObservation() != null) + { + TriangulationResult triangulation = triangulate(context.centerTagObservation(), context.offsetTagObservation()); + if (triangulation.valid()) + { + return TurretAimSolution.of(context.currentFieldAngle().plus(Degrees.of(triangulation.relativeAngleDeg())), triangulation.distanceMeters()); + } + } + + if (context.hasTarget()) + { + double fallbackDistance = context.centerTagObservation() == null ? 0.0 : context.centerTagObservation().distanceMeters(); + return TurretAimSolution.of(context.currentFieldAngle().plus(context.horizontalOffset()), fallbackDistance); + } + + return TurretAimSolution.of(context.robotHeading().plus((ShooterConstants.TURRET_HOME_ANGLE)), 0.0); + } + + /** + * Solves for robot-to-hub angle and distance using two known triangles. + *

+ * Triangle R-C-O: Robot (R), center tag (C), offset tag (O). We know RC and RO + * from vision and CL from field drawings (TURRET_CL_METERS). Law of sines gives + * angle at C. + *

+ * Triangle R-C-H: Hub center (H) is TURRET_CH_METERS from C (field drawing). + * Law of cosines gives RH (distance to hub). Law of sines gives angle at R from + * R→C to R→H. We add the camera-to-center-tag angle (tx) to get full + * robot-frame angle to hub. + */ + private static TriangulationResult triangulate(TagObservation centerTagObservation, TagObservation offsetTagObservation) + { + double rcDistanceMeters = centerTagObservation.distanceMeters(); + double roDistanceMeters = offsetTagObservation.distanceMeters(); + if (rcDistanceMeters <= 0.0 || roDistanceMeters <= 0.0) + { + return TriangulationResult.invalid(); + } + + double angleLrcRad = Math.toRadians(offsetTagObservation.horizontalOffset().in(Degrees) - centerTagObservation.horizontalOffset().in(Degrees)); + double sinLcr = (roDistanceMeters * Math.sin(angleLrcRad)) / ShooterConstants.TURRET_CL_METERS; + double angleLcrRad = Math.asin(MathUtil.clamp(sinLcr, -1.0, 1.0)); + + double angleLcrPlusNinetyRad = angleLcrRad + Math.PI / 2.0; + double rhSquared = (rcDistanceMeters * rcDistanceMeters) + (ShooterConstants.TURRET_CH_METERS * ShooterConstants.TURRET_CH_METERS) + - (2.0 * rcDistanceMeters * ShooterConstants.TURRET_CH_METERS * Math.cos(angleLcrPlusNinetyRad)); + if (rhSquared <= 0.0 || !Double.isFinite(rhSquared)) + { + return TriangulationResult.invalid(); + } + + double rhDistanceMeters = Math.sqrt(rhSquared); + double sinHrc = (ShooterConstants.TURRET_CH_METERS * Math.sin(angleLcrPlusNinetyRad)) / rhDistanceMeters; + double angleHrcDeg = Math.toDegrees(Math.asin(MathUtil.clamp(sinHrc, -1.0, 1.0))); + + double angleRhDeg = angleHrcDeg + centerTagObservation.horizontalOffset().in(Degrees); + if (!Double.isFinite(angleRhDeg)) + { + return TriangulationResult.invalid(); + } + + return TriangulationResult.valid(angleRhDeg, rhDistanceMeters); + } + + public record DirectorContext(TurretState turretState, Angle currentFieldAngle, Angle robotHeading, Angle horizontalOffset, boolean hasTarget, TagObservation centerTagObservation, TagObservation offsetTagObservation) + { + } + + public record TurretAimSolution(boolean hasSetpoint, Angle fieldSetpoint, double distanceToHubMeters) + { + public static TurretAimSolution none() + { + return new TurretAimSolution(false, Degrees.zero(), 0.0); + } + + public static TurretAimSolution of(Angle fieldSetpoint, double distanceToHubMeters) + { + return new TurretAimSolution(true, fieldSetpoint, distanceToHubMeters); + } + } + + public record TagObservation(int id, Angle horizontalOffset, double distanceMeters) + { + } + + private record TriangulationResult(boolean valid, double relativeAngleDeg, double distanceMeters) + { + private static TriangulationResult invalid() + { + return new TriangulationResult(false, 0.0, 0.0); + } + + private static TriangulationResult valid(double relativeAngleDeg, double distanceMeters) + { + return new TriangulationResult(true, relativeAngleDeg, distanceMeters); + } + } +}