diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fd5f5e5..f6f1a2c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,9 +39,13 @@ public void robotInit() { // for (int port = 5800; port <= 5807; port++) { // PortForwarder.add(port, "limelight.local", port); // } - PortForwarder.add(5800, "limelight.local", 5800); - PortForwarder.add(5801, "limelight.local", 5801); - PortForwarder.add(5805, "limelight.local", 5805); + PortForwarder.add(5800, "limelight-main.local", 5800); + PortForwarder.add(5801, "limelight-main.local", 5801); + PortForwarder.add(5805, "limelight-main.local", 5805); + + // PortForwarder.add(5810, "limelight-intake.local", 5810); + // PortForwarder.add(5811, "limelight-intake.local", 5811); + // PortForwarder.add(5815, "limelight-intake.local", 5815); // CameraServer.startAutomaticCapture(0); UtilFuncs.LoadField(); diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 5a3f676..c5b8738 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -220,6 +220,8 @@ private boolean updateBotpose() { // UPDATE BOTPOSE WITH VISION if (visionBotpose.isPresent()) { + System.out.println("PRESENT"); + LimelightHelper.SetRobotOrientation( Limelights.MAIN, getHeading().getDegrees(), @@ -232,6 +234,8 @@ private boolean updateBotpose() { if (Math.abs(_gyro.getRate()) >= 500) return false; + visionPublisher.set(visionBotpose.get().pose); + _estimator.addVisionMeasurement( visionBotpose.get().pose, visionBotpose.get().timestampSeconds