diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d1ec3a8..01b3d32 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -37,7 +37,7 @@ public static class TelemetryConstants { // this will fully disable logging even when FMS is connected. public static final boolean killswitch = false; // If true, data won't be sent over network even when not connected to FMS - public static final boolean disableNetworkLogging = true; + public static final boolean disableNetworkLogging = false; // ONLY ENABLE IN DEV (this *should* be overwritten when connected to FMS, but that's untested) public static final boolean disableDatalog = false; // Prefix in NetworkTables, must end with a '/' diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 77e4213..a03190f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -68,22 +68,22 @@ public final class RobotContainer { private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Telemetry logger = new Telemetry(); + private final Telemetry logger = new Telemetry(); - private final CommandXboxController m_Controller = new CommandXboxController(0); - private final CommandXboxController m_ScuffedController = new CommandXboxController(2); - private final CommandXboxController m_TuningController = new CommandXboxController(4); - private final CommandJoystick m_OperatorController = new CommandJoystick(1); + private final CommandXboxController m_Controller = new CommandXboxController(0); + private final CommandXboxController m_ScuffedController = new CommandXboxController(2); + private final CommandXboxController m_TuningController = new CommandXboxController(4); + private final CommandJoystick m_OperatorController = new CommandJoystick(1); - public final CommandSwerveDrivetrain drivetrain; + public final CommandSwerveDrivetrain drivetrain; public final BotType botType = RobotDiscoverer.getRobot(); -// private SwerveSubsystem m_Swerve; + // private SwerveSubsystem m_Swerve; private GlobalVisionSubsystem m_GlobalVision; + private TrigVisionSubsystem m_TrigVision; private LEDSubsystem m_LedSubsystem = new LEDSubsystem(); private ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem(); - private TrigVisionSubsystem m_TrigVision = new TrigVisionSubsystem(m_LedSubsystem); private ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(botType); private ClawSubsystemTurbo m_ClawSubsystem = new ClawSubsystemTurbo(botType, m_ElevatorSubsystem); public PivotSubsystem m_PivotSubsystem = new PivotSubsystem(botType, m_ElevatorSubsystem.elevator); @@ -108,15 +108,17 @@ public RobotContainer(Rev2mDistanceSensor distance) { DataLogManager.start(); DriverStation.startDataLog(DataLogManager.getLog()); } - + if (botType == BotType.ALPHA_BOT) { drivetrain = TunerConstantsAlpha.createDrivetrain(); } else { drivetrain = TunerConstants.createDrivetrain(); } + sysIdChooser = new SysIdChooser(drivetrain, m_ElevatorSubsystem, m_PivotSubsystem); - - initSubsystems(); + + m_GlobalVision = GlobalVisionSubsystem.configure(drivetrain); + m_TrigVision = new TrigVisionSubsystem(m_LedSubsystem, m_GlobalVision); switch (botType) { case MAIN_BOT: @@ -134,7 +136,7 @@ public RobotContainer(Rev2mDistanceSensor distance) { initManualAutos(); SmartDashboard.putData("Auto Chooser", autoChooser); - + // SignalLogger.setPath("/ctre-logs/"); SignalLogger.start(); if (RobotBase.isReal()) @@ -150,32 +152,33 @@ public double getTravelDir() { return -1; } - + private void initManualAutos() { autoChooser.setDefaultOption("None", Commands.none()); autoChooser.addOption("Travel", drivetrain.applyRequest(() -> drive.withVelocityY(getTravelDir())).withTimeout(2)); - autoChooser.addOption("L1", CoralAutoBuilder.build(AutoType.One, distance, drivetrain, m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision)); - autoChooser.addOption("L1 + Intake Algae", CoralAutoBuilder.build(AutoType.OneAndHalf, distance, drivetrain, m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision)); - autoChooser.addOption("L1 + Shoot Algae", CoralAutoBuilder.build(AutoType.Two, distance, drivetrain, m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision)); - - } - - private void initSubsystems() { + autoChooser.addOption("L1", CoralAutoBuilder.build(AutoType.One, distance, drivetrain, m_PivotSubsystem, + m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision)); + autoChooser.addOption("L1 + Intake Algae", CoralAutoBuilder.build(AutoType.OneAndHalf, distance, drivetrain, + m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision)); + autoChooser.addOption("L1 + Shoot Algae", CoralAutoBuilder.build(AutoType.Two, distance, drivetrain, + m_PivotSubsystem, m_ElevatorSubsystem, m_ClawSubsystem, m_TrigVision)); } private void changeLevel(boolean moveUp) { - if(moveUp) + if (moveUp) algaeLevel++; else algaeLevel--; - - if(algaeLevel > 4) + + if (algaeLevel > 4) algaeLevel = 4; - if(algaeLevel < 0) + if (algaeLevel < 0) algaeLevel = 0; - levelLog.update((double)algaeLevel); - // ParallelCommandGroup parallelLevelCommands = new ParallelCommandGroup(m_PivotSubsystem.goToAngle(algaeLevel), m_ElevatorSubsystem.goToHeight(algaeLevel)); + levelLog.update((double) algaeLevel); + // ParallelCommandGroup parallelLevelCommands = new + // ParallelCommandGroup(m_PivotSubsystem.goToAngle(algaeLevel), + // m_ElevatorSubsystem.goToHeight(algaeLevel)); // parallelLevelCommands.schedule(); } @@ -189,7 +192,7 @@ public SwerveRequest.FieldCentric getFieldCentricDriveReq() { if (m_ElevatorSubsystem.getElevatorHeight() > 15) { multiplier /= 2; } - + if (m_ElevatorSubsystem.getElevatorHeight() > 30) { multiplier /= 2; } @@ -211,12 +214,12 @@ private void configureSwerveBindings() { // and Y is defined as to the left according to WPILib convention. drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain.applyRequest(this::getDriveReq) - ); + drivetrain.applyRequest(this::getDriveReq)); // m_Controller.a().whileTrue(drivetrain.applyRequest(() -> brake)); // m_Controller.b().whileTrue(drivetrain.applyRequest(() -> - // point.withModuleDirection(new Rotation2d(-m_Controller.getLeftY(), -m_Controller.getLeftX())) + // point.withModuleDirection(new Rotation2d(-m_Controller.getLeftY(), + // -m_Controller.getLeftX())) // )); // // reset the field-centric heading on left bumper press @@ -234,8 +237,10 @@ private void configureBindings() { configureTuningBindings(); configureScuffedBindings(); configureOperatorBindings(); - - // new Trigger(m_ClawSubsystem.beambreak::get).negate().whileTrue(m_LedSubsystem.solidColor(new Color(0, 155, 255))); + + // new + // Trigger(m_ClawSubsystem.beambreak::get).negate().whileTrue(m_LedSubsystem.solidColor(new + // Color(0, 155, 255))); // m_Controller.rightBumper().whileTrue(m_ClawSubsystem.intakeWithBeambreak()); // m_Controller.rightTrigger().whileTrue(m_PivotSubsystem.setAngle(Rotations.of(0.3)).until(m_PivotSubsystem::closeEnough).andThen(m_ClawSubsystem.shoot())); @@ -254,16 +259,18 @@ private void configureBindings() { m_Controller.leftBumper().whileTrue(m_ClimberSubsystem.runClimberup()); m_Controller.povDown().whileTrue(m_ElevatorSubsystem.autoHonePose().withName("Elevator Hone Command")); - + // m_Controller.rightBumper().onTrue( - // ( - // ( - // scuffedElevator(ElevatorConstantb s.stowHeight).alongWith(scuffedPivot(Rotations.of(0.055), false)) - // ).andThen(new WaitCommand(1)) - // ) - // .until(() -> m_PivotSubsystem.closeEnough()) - // .andThen(() -> { m_PivotSubsystem.setDefaultCommand(m_PivotSubsystem.stop());}) - // .andThen(m_PivotSubsystem.stop())); + // ( + // ( + // scuffedElevator(ElevatorConstantb + // s.stowHeight).alongWith(scuffedPivot(Rotations.of(0.055), false)) + // ).andThen(new WaitCommand(1)) + // ) + // .until(() -> m_PivotSubsystem.closeEnough()) + // .andThen(() -> { + // m_PivotSubsystem.setDefaultCommand(m_PivotSubsystem.stop());}) + // .andThen(m_PivotSubsystem.stop())); } public Command scuffedElevator(double rotations) { @@ -271,12 +278,18 @@ public Command scuffedElevator(double rotations) { } public void configureScuffedBindings() { - m_ScuffedController.y().onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.bargeAngle))); - m_ScuffedController.a().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.floorAngle))); - m_ScuffedController.x().onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.stowAngle))); - m_ScuffedController.b().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.processorAngle))); - m_ScuffedController.povDown().onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefOneAngle))); - m_ScuffedController.povUp().onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefTwoAngle))); + m_ScuffedController.y().onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.bargeAngle))); + m_ScuffedController.a().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.floorAngle))); + m_ScuffedController.x().onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.stowAngle))); + m_ScuffedController.b().onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.processorAngle))); + m_ScuffedController.povDown().onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefOneAngle))); + m_ScuffedController.povUp().onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefTwoAngle))); // m_ScuffedController.povLeft().onTrue(scuffedElevator(Constants.ElevatorConstants.onCoralHeight).andThen(scuffedPivot(Constants.PivotConstants.onCoralAngle))); m_ScuffedController.leftTrigger().whileTrue(m_ClawSubsystem.intakeWithBeambreak()); @@ -284,7 +297,8 @@ public void configureScuffedBindings() { m_ScuffedController.rightTrigger().whileTrue(m_ClawSubsystem.shootWithBeambreak()); m_ScuffedController.rightBumper().whileTrue(m_ClawSubsystem.shoot()); - m_ScuffedController.povRight().whileTrue(scuffedElevator(Constants.ElevatorConstants.coralIntake).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.coralIntake))); + m_ScuffedController.povRight().whileTrue(scuffedElevator(Constants.ElevatorConstants.coralIntake) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.coralIntake))); m_ScuffedController.povLeft().whileTrue(m_ElevatorSubsystem.autoHonePose().withName("Elevator Hone Command")); } @@ -302,12 +316,18 @@ private void configureSimBindings() { } private void configureOperatorBindings() { - m_OperatorController.button(7).onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.bargeAngle))); - m_OperatorController.button(5).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.floorAngle))); - m_OperatorController.button(8).onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.stowAngle))); - m_OperatorController.button(2).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.processorAngle))); - m_OperatorController.button(4).onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefOneAngle))); - m_OperatorController.button(1).onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight).andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefTwoAngle))); + m_OperatorController.button(7).onTrue(scuffedElevator(Constants.ElevatorConstants.bargeHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.bargeAngle))); + m_OperatorController.button(5).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.floorAngle))); + m_OperatorController.button(8).onTrue(scuffedElevator(Constants.ElevatorConstants.stowHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.stowAngle))); + m_OperatorController.button(2).onTrue(scuffedElevator(Constants.ElevatorConstants.floorHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.processorAngle))); + m_OperatorController.button(4).onTrue(scuffedElevator(Constants.ElevatorConstants.reefOneHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefOneAngle))); + m_OperatorController.button(1).onTrue(scuffedElevator(Constants.ElevatorConstants.reefTwoHeight) + .andThen(m_PivotSubsystem.scuffedPivot(Constants.PivotConstants.reefTwoAngle))); m_OperatorController.button(6).whileTrue(m_ClawSubsystem.intakeWithBeambreak()); m_OperatorController.button(9).whileTrue(m_ClawSubsystem.intake()); // m_OperatorController.button().whileTrue(m_ClawSubsystem.shootWithBeambreak()); @@ -324,7 +344,7 @@ private void configureTuningBindings() { public Command getAutonomousCommand() { final double sign; - + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) { sign = -1; } else { @@ -337,7 +357,9 @@ public Command getAutonomousCommand() { m_PivotSubsystem.reseedEncoder(); }, drivetrain)).andThen(m_ElevatorSubsystem.autoHonePose().asProxy().raceWith(m_ClimberSubsystem.runClimberupAuto())); // if (Robot.isReal()) { - // base = base.andThen(Commands.run(() -> {}).until(m_PivotSubsystem::closeEnough)); //.andThen(m_ElevatorSubsystem.autoHonePose().asProxy()); + // base = base.andThen(Commands.run(() -> + // {}).until(m_PivotSubsystem::closeEnough)); + // //.andThen(m_ElevatorSubsystem.autoHonePose().asProxy()); // } Command cmd = autoChooser.getSelected().asProxy(); cmd.addRequirements(drivetrain); diff --git a/src/main/java/frc/robot/commands/AlignBarge.java b/src/main/java/frc/robot/commands/AlignBarge.java index 6b5426e..dc1eab6 100644 --- a/src/main/java/frc/robot/commands/AlignBarge.java +++ b/src/main/java/frc/robot/commands/AlignBarge.java @@ -44,7 +44,8 @@ public class AlignBarge extends Command { private final ProfiledPIDController profiledPIDController = new ProfiledPIDController(AutoConstants.Turbo.kTranslationP, AutoConstants.Turbo.kTranslationI, AutoConstants.Turbo.kTranslationD, new TrapezoidProfile.Constraints(AutoConstants.MaxSpeed, AutoConstants.MaxAcceleration)); - private final SwerveRequest.FieldCentricFacingAngle driveRequest = new SwerveRequest.FieldCentricFacingAngle().withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); + private final SwerveRequest.FieldCentricFacingAngle driveRequest = new SwerveRequest.FieldCentricFacingAngle() + .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); private final UltraDoubleLog logger = new UltraDoubleLog("Auto/Barge Auto Align Pid Output"); private DoublePublisher posePublisher = NetworkTableInstance.getDefault() @@ -58,25 +59,34 @@ public AlignBarge(TrigVisionSubsystem vision, CommandSwerveDrivetrain drivetrain addRequirements(drivetrain); - driveRequest.HeadingController.setPID(AutoConstants.Turbo.kRotationP, AutoConstants.Turbo.kRotationI, AutoConstants.Turbo.kRotationD); + driveRequest.HeadingController.setPID(AutoConstants.Turbo.kRotationP, AutoConstants.Turbo.kRotationI, + AutoConstants.Turbo.kRotationD); } private double sign = 1; @Override public void execute() { - Optional distanceOptional = vision.getLateralDistanceToBarge(); + var offset = vision.getRobotToTag(); + double pidOutput = 0; - if (!distanceOptional.isEmpty()) { - pidOutput = -profiledPIDController.calculate(distanceOptional.get().in(Meters), AutoConstants.targetDistanceFromBarge.in(Meters)); - posePublisher.set(distanceOptional.get().in(Meters)); - setPosePublisher.set(AutoConstants.targetDistanceFromBarge.in(Meters)); + if (offset.isPresent()) { + var parallelDistance = offset.get().getMeasureX(); + double bargeDistance = parallelDistance.in(Meters); + double goalDistance = AutoConstants.targetDistanceFromBarge.in(Meters); + + pidOutput = -profiledPIDController.calculate(bargeDistance, goalDistance); + + posePublisher.set(bargeDistance); + setPosePublisher.set(goalDistance); } + logger.update(pidOutput); - Optional tagID = vision.getTagID(); - if (tagID.isPresent()) { - if (tagID.get() == 4 || tagID.get() == 5) { + var tag = vision.getBestTag(); + if (tag != null) { + int id = tag.ID; + if (id == 4 || id == 5) { sign = -1; } else { sign = 1; @@ -89,7 +99,6 @@ public void execute() { if (profiledPIDController.atGoal()) { vision.isAlignedTimestamp = RobotController.getFPGATime(); } - driveRequest.withVelocityX(Meters.of(sign * pidOutput).per(Second)); driveRequest.withVelocityY(horizontalInputSupplier.getAsDouble()); @@ -102,5 +111,5 @@ public void execute() { public boolean isFinished() { return super.isFinished(); } - + } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 7e9c2b5..7c7f624 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -99,7 +99,7 @@ public class ElevatorSubsystem extends SubsystemBase { private final UltraSupplierLog rightPosePub = new UltraSupplierLog("Elevator/Right Pose", followerMotor.getPosition()); private final Alert estopAlert = new Alert("Elevator E-Stopped", AlertType.kError); - private boolean estop = false; + private boolean estop = true; // there will be at least one limit switch and an encoder to track the position of the elevator public ElevatorSubsystem(BotType bot) { diff --git a/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java b/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java index ef1e3d5..4d132d8 100644 --- a/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/GlobalVisionSubsystem.java @@ -1,11 +1,16 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; + import java.io.IOException; import java.util.HashSet; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -21,6 +26,21 @@ public final class GlobalVisionSubsystem implements Subsystem { public static final double kMaxAmbiguity = 0.7; public static final double kMaxDistance = Units.feetToMeters(10); + public static final Translation3d kBargeCameraTranslation = new Translation3d( + Inches.of(-8), + Inches.of(11), + Inches.of(10.25)); + + public static final Rotation3d kBargeCameraRotation = new Rotation3d( + Degrees.of(0), + Degrees.of(75), + Degrees.of(90)); + + public static final Transform3d kBargeCameraTransform = new Transform3d(kBargeCameraTranslation, kBargeCameraRotation); + public static final CameraDescription[] kCameras = new CameraDescription[] { + new CameraDescription("limelight-barge", CameraType.LIMELIGHT, kBargeCameraTransform, null) + }; + public static enum CameraType { PHOTONVISION, LIMELIGHT @@ -31,9 +51,17 @@ public static class CameraDescription { CameraType type; Transform3d offset; Specification spec; + + public CameraDescription(String name, CameraType type, Transform3d offset, Specification spec) { + this.name = name; + this.type = type; + this.offset = offset; + this.spec = spec; + } }; private static class CameraData { + CameraType type; Camera camera; Result result; Simulator sim; @@ -44,10 +72,9 @@ private static class CameraData { private HashSet m_ViableResults; private int m_Frame; + private AprilTagFieldLayout m_Layout; public static GlobalVisionSubsystem configure(CommandSwerveDrivetrain swerve) { - var cameras = new CameraDescription[] { /* cameras */ }; - AprilTagFieldLayout layout; try { layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); @@ -56,7 +83,7 @@ public static GlobalVisionSubsystem configure(CommandSwerveDrivetrain swerve) { layout = null; } - return new GlobalVisionSubsystem(swerve, cameras, layout); + return new GlobalVisionSubsystem(swerve, layout); } private static Camera createCamera(CameraDescription desc, AprilTagFieldLayout layout) { @@ -70,16 +97,17 @@ private static Camera createCamera(CameraDescription desc, AprilTagFieldLayout l } } - public GlobalVisionSubsystem(CommandSwerveDrivetrain swerve, CameraDescription[] cameras, AprilTagFieldLayout layout) { + public GlobalVisionSubsystem(CommandSwerveDrivetrain swerve, AprilTagFieldLayout layout) { m_Swerve = swerve; m_ViableResults = new HashSet<>(); m_Frame = 0; - m_Cameras = new CameraData[cameras.length]; - for (int i = 0; i < cameras.length; i++) { - var desc = cameras[i]; + m_Cameras = new CameraData[kCameras.length]; + for (int i = 0; i < kCameras.length; i++) { + var desc = kCameras[i]; var data = new CameraData(); + data.type = desc.type; data.camera = createCamera(desc, layout); data.result = new Result(); data.sim = null; @@ -87,6 +115,8 @@ public GlobalVisionSubsystem(CommandSwerveDrivetrain swerve, CameraDescription[] if (Robot.isSimulation() && desc.spec != null) { data.sim = data.camera.createSimulator(desc.spec); } + + m_Cameras[i] = data; } } @@ -106,10 +136,43 @@ private static boolean isResultViable(Result result) { return true; } + public AprilTagFieldLayout getLayout() { + return m_Layout; + } + + public int getCameraCount() { + return m_Cameras.length; + } + + public Camera getCamera(int camera) { + return m_Cameras[camera].camera; + } + + public CameraType getCameraType(int camera) { + return m_Cameras[camera].type; + } + public Result getResult(int camera) { return m_Cameras[camera].result; } + /** + * Finds the first camera of the given type. Returns -1 on failure. + * + * @param type Camera type to search for. + * @return The index of the camera. + */ + public int getCameraByType(CameraType type) { + for (int i = 0; i < m_Cameras.length; i++) { + var data = m_Cameras[i]; + if (data.type == type) { + return i; + } + } + + return -1; + } + @Override public void periodic() { m_ViableResults.clear(); diff --git a/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java b/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java index 146e962..07e5095 100644 --- a/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TrigVisionSubsystem.java @@ -1,20 +1,14 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Microseconds; import static edu.wpi.first.units.Units.Milliseconds; import static edu.wpi.first.units.Units.Seconds; +import java.util.HashSet; import java.util.Optional; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; -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.LEDPattern; @@ -24,20 +18,42 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.AutoConstants; -import frc.robot.vision.LimeLightAprilTag; -import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.GlobalVisionSubsystem.CameraType; +import frc.robot.vision.Camera.Tag; public final class TrigVisionSubsystem extends SubsystemBase { - private LimeLightAprilTag tag; - private StructPublisher tagRelativePosePublisher = NetworkTableInstance.getDefault() - .getStructTopic("Vision/Position to Barge", Pose2d.struct).publish(); - private Optional lastRecordedPose = Optional.empty(); + private StructPublisher tagRelativePosePublisher = NetworkTableInstance.getDefault() + .getStructTopic("Vision/Position to Barge", Transform3d.struct).publish(); + private Optional lastRecordedOffset = Optional.empty(); private Time timeSinceTagSeen = Seconds.of(0); - private LEDSubsystem m_LedSubsystem;; + private LEDSubsystem leds; + private GlobalVisionSubsystem globalVision; + private int cameraIndex; - public TrigVisionSubsystem(LEDSubsystem m_LedSubsystem) { - tag = new LimeLightAprilTag(); - this.m_LedSubsystem = m_LedSubsystem; + private static final HashSet kValidIDs; + + static { + kValidIDs = new HashSet<>(); + + var validIDs = new int[] { + 14, + 15, + 4, + 5, + 2 + }; + + for (int i = 0; i < validIDs.length; i++) { + kValidIDs.add(validIDs[i]); + } + } + + public TrigVisionSubsystem(LEDSubsystem leds, GlobalVisionSubsystem globalVision) { + this.leds = leds; + this.globalVision = globalVision; + + // just use the limelight + cameraIndex = globalVision.getCameraByType(CameraType.LIMELIGHT); } LEDPattern seeingColor = LEDPattern.solid(new Color(255, 0, 255)); @@ -52,67 +68,77 @@ public boolean isAligned() { @Override public void periodic() { - SmartDashboard.putNumber("Vision Aligned Timestamp", Math.abs(RobotController.getFPGATime() - isAlignedTimestamp) * 1e6); - if (tag.hasTarget()) { + SmartDashboard.putNumber("Vision Aligned Timestamp", + Math.abs(RobotController.getFPGATime() - isAlignedTimestamp) * 1e6); + + var tag = getBestTag(); + if (tag != null) { if (!isAligned()) { - this.m_LedSubsystem.applyPatternOnce(seeingColor); + leds.applyPatternOnce(seeingColor); } else { - this.m_LedSubsystem.applyPatternOnce(alignedColor); + leds.applyPatternOnce(alignedColor); } + var camera = globalVision.getCamera(cameraIndex); + var robotToCamera = camera.getOffset(); + var cameraToTag = tag.transform; + var robotToTag = robotToCamera.plus(cameraToTag); + timeSinceTagSeen = Seconds.of(0); - lastRecordedPose = Optional.of(new Pose2d(Meters.of(tag.getVerticalDistanceMeters()), Meters.of(tag.getLateralDistanceMeters()), new Rotation2d())); - tagRelativePosePublisher.set(lastRecordedPose.get()); + lastRecordedOffset = Optional.of(robotToTag); + tagRelativePosePublisher.set(robotToTag); } else { - if (timeSinceTagSeen.gt(AutoConstants.lastPoseTimeout) && lastRecordedPose.isPresent()) { - lastRecordedPose = Optional.empty(); + if (timeSinceTagSeen.gt(AutoConstants.lastPoseTimeout)) { + lastRecordedOffset = Optional.empty(); } + timeSinceTagSeen = timeSinceTagSeen.plus(Milliseconds.of(20)); - this.m_LedSubsystem.applyPatternOnce(this.m_LedSubsystem.allianceColorGetter()); + leds.applyPatternOnce(leds.allianceColorGetter()); } } - public Command resetLastPose() { - return run(() -> { lastRecordedPose = Optional.empty(); }); + public Command reset() { + return run(() -> { + lastRecordedOffset = Optional.empty(); + }); } - public Optional getTagID() { - long tid = tag.getTargetID(); - if (tid == -1) { - return Optional.empty(); + public Tag getBestTag() { + if (cameraIndex < 0) { + return null; } - return Optional.of(tid); - } - - public Optional getHorizontalRotation() { - if (tag.hasTarget()) { - return Optional.of(Degrees.of(tag.getHorizontalOffset())); - } else { - return Optional.empty(); + var result = globalVision.getResult(cameraIndex); + if (!result.isNew) { + return null; } - } - public Optional getLateralDistanceToBarge() { - if (lastRecordedPose.isPresent()) { - return Optional.of(lastRecordedPose.get().getMeasureY()); - } else { - return Optional.empty(); + int bestTag = -1; + Distance minDistance = null; + + for (int i = 0; i < result.tags.length; i++) { + var tag = result.tags[i]; + if (!kValidIDs.contains(tag.ID)) { + continue; + } + + if (minDistance == null || tag.cameraDistance.lt(minDistance)) { + bestTag = i; + minDistance = tag.cameraDistance; + } } + + return bestTag < 0 ? null : result.tags[bestTag]; } - public Optional getVerticalDistanceToBarge() { - if (lastRecordedPose.isPresent()) { - return Optional.of(lastRecordedPose.get().getMeasureX()); - } else { - return Optional.empty(); - } + /** + * Returns the last calculated position relative to the barge. + */ + public Optional getRobotToTag() { + return lastRecordedOffset; } public boolean canSeeTag() { - return tag.hasTarget(); - } - public boolean hasLastPosition() { - return lastRecordedPose.isPresent(); + return lastRecordedOffset.isPresent(); } } diff --git a/src/main/java/frc/robot/vision/Camera.java b/src/main/java/frc/robot/vision/Camera.java index 9e92bc5..7d4477c 100644 --- a/src/main/java/frc/robot/vision/Camera.java +++ b/src/main/java/frc/robot/vision/Camera.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.units.measure.Distance; public interface Camera { public static interface Simulator { @@ -12,13 +13,17 @@ public static interface Simulator { public static class Tag { public int ID; - public double cameraDistance; + public Distance cameraDistance; + public double ambiguity; + public Transform3d transform; + public double area; } public static class Result { public Pose2d pose; public double maxAmbiguity, maxDistance, minDistance; public Tag[] tags; + public int bestTagIndex; public boolean isNew; public double timestamp; diff --git a/src/main/java/frc/robot/vision/LimeLightAprilTag.java b/src/main/java/frc/robot/vision/LimeLightAprilTag.java deleted file mode 100644 index 94662af..0000000 --- a/src/main/java/frc/robot/vision/LimeLightAprilTag.java +++ /dev/null @@ -1,92 +0,0 @@ -package frc.robot.vision; - -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Radians; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import frc.robot.Constants.AutoConstants; - -public class LimeLightAprilTag { - // Get the LimeLight NetworkTable. Adjust the table name if necessary. - private final NetworkTable limeTable = NetworkTableInstance.getDefault().getTable("limelight"); - // Camera and target configuration (modify these to match your setup) - private final double cameraHeightMeters = Inches.of(10.5).in(Meters); // Height of the camera off the ground in meters - private final double targetHeightMeters = Inches.of(73).in(Meters); // Height of the AprilTag on the field in meters - private final double cameraAngleDegrees = AutoConstants.cameraAngle.in(Degrees); // Angle at which the camera is mounted - // 3, 4.75, 10.5 - - /** -Checks if a valid target is detected. -@return true if target is detected; false otherwise. - */ - public boolean hasTarget() { - return limeTable.getEntry("tv").getDouble(0) == 1.0; - } - /** -Gets the horizontal offset (tx) in degrees. -@return Horizontal offset from the crosshair. - */ - public double getHorizontalOffset() { - return limeTable.getEntry("tx").getDouble(0); - } - /** -Gets the vertical offset (ty) in degrees. -@return Vertical offset from the crosshair. - */ - public double getVerticalOffset() { - return limeTable.getEntry("ty").getDouble(0); - } - /** -Gets the target area (ta). -@return The area of the detected target. - */ - public double getTargetArea() { - return limeTable.getEntry("ta").getDouble(0); - } - /** -Gets the target id (tod). -@return The ID of the detected target. - */ - public long getTargetID() { - return limeTable.getEntry("tid").getInteger(-1); - } - /** -Computes an approximate distance to the target using the vertical offset. -Uses the formula: - distance = (targetHeight - cameraHeight) / tan(cameraAngle + ty) -Make sure to convert angles to radians. - * -@return Estimated distance in meters. - */ - public double getLateralDistanceMeters() { - double ty = getVerticalOffset(); - // Combine the mounting angle with the offset - double angleToTargetRadians = Math.toRadians(cameraAngleDegrees + ty); - return (targetHeightMeters - cameraHeightMeters) / Math.tan(angleToTargetRadians); - } - public double getVerticalDistanceMeters() { - double tx = getHorizontalOffset(); - double verticalDist = getLateralDistanceMeters(); - - return verticalDist * Math.tan(Degrees.of(tx).in(Radians)); - } - /** -Example method to update and print the current LimeLight data. - */ - public void update() { - if (hasTarget()) { - double tx = getHorizontalOffset(); - double ty = getVerticalOffset(); - double distance = getVerticalDistanceMeters(); - System.out.println("Target Detected!"); - System.out.println("Horizontal Offset (tx): " + tx + " degrees"); - System.out.println("Vertical Offset (ty): " + ty + " degrees"); - System.out.println("Estimated Distance: " + distance + " meters"); - } else { - System.out.println("No target detected."); - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/LimelightCamera.java b/src/main/java/frc/robot/vision/LimelightCamera.java index 4e99e5e..9c8f2d1 100644 --- a/src/main/java/frc/robot/vision/LimelightCamera.java +++ b/src/main/java/frc/robot/vision/LimelightCamera.java @@ -1,6 +1,11 @@ package frc.robot.vision; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import frc.lib.LimelightHelpers; @@ -45,14 +50,24 @@ public void update(Result result) { result.pose = estimate.pose; result.timestamp = estimate.timestampSeconds; result.isNew = true; + result.bestTagIndex = -1; + + var limelightResults = LimelightHelpers.getLatestResults(m_Name); + double minAmbiguity = Double.MAX_VALUE; result.maxAmbiguity = Double.MIN_VALUE; result.maxDistance = Double.MIN_VALUE; result.minDistance = Double.MAX_VALUE; result.tags = new Tag[estimate.tagCount]; - for (int i = 0; i < estimate.tagCount; i++) { + for (int i = 0; i < result.tags.length; i++) { var fiducial = estimate.rawFiducials[i]; + var processed = limelightResults.targets_Fiducials[i]; + + if (fiducial.ambiguity < minAmbiguity) { + result.bestTagIndex = i; + minAmbiguity = fiducial.ambiguity; + } result.maxAmbiguity = Math.max(result.maxAmbiguity, fiducial.ambiguity); result.maxDistance = Math.max(result.maxDistance, fiducial.distToCamera); @@ -60,7 +75,11 @@ public void update(Result result) { var tag = new Tag(); tag.ID = fiducial.id; - tag.cameraDistance = fiducial.distToCamera; + tag.cameraDistance = Meters.of(fiducial.distToCamera); + tag.ambiguity = fiducial.ambiguity; + tag.area = fiducial.ta; + tag.transform = processed.getTargetPose_CameraSpace().minus(new Pose3d()); + result.tags[i] = tag; } } diff --git a/src/main/java/frc/robot/vision/PhotonlibCamera.java b/src/main/java/frc/robot/vision/PhotonlibCamera.java index aec3f1e..124b12e 100644 --- a/src/main/java/frc/robot/vision/PhotonlibCamera.java +++ b/src/main/java/frc/robot/vision/PhotonlibCamera.java @@ -1,5 +1,8 @@ package frc.robot.vision; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; + import java.util.Optional; import org.photonvision.EstimatedRobotPose; @@ -13,6 +16,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; public final class PhotonlibCamera implements Camera { @@ -94,10 +98,12 @@ private void update(Result result, PhotonPipelineResult cameraResult) { result.isNew = true; result.pose = pose.estimatedPose.toPose2d(); result.timestamp = pose.timestampSeconds; + result.bestTagIndex = -1; result.minDistance = Double.MAX_VALUE; result.maxDistance = Double.MIN_VALUE; result.maxAmbiguity = Double.MIN_VALUE; + double minAmbiguity = Double.MAX_VALUE; var targets = cameraResult.getTargets(); result.tags = new Tag[targets.size()]; @@ -108,13 +114,22 @@ private void update(Result result, PhotonPipelineResult cameraResult) { double distance = transform.getTranslation().getNorm(); double ambiguity = target.getPoseAmbiguity(); + if (ambiguity < minAmbiguity) { + result.bestTagIndex = i; + minAmbiguity = ambiguity; + } + result.minDistance = Math.min(result.minDistance, distance); result.maxDistance = Math.max(result.maxDistance, distance); result.maxAmbiguity = Math.max(result.maxAmbiguity, ambiguity); var tag = new Tag(); tag.ID = target.getFiducialId(); - tag.cameraDistance = distance; + tag.cameraDistance = Meters.of(distance); + tag.ambiguity = ambiguity; + tag.area = target.area; + tag.transform = transform; + result.tags[i] = tag; }