Create a test harness that can communicate with Limelight and print detection data.

build.gradle

@@ -8,7 +8,7 @@ java {
     targetCompatibility = JavaVersion.VERSION_17
 }
 
-def ROBOT_MAIN_CLASS = "frc.robot.Main"
+def ROBOT_MAIN_CLASS = "frc.robot.commands.test.VisionTestHarness"
 
 // Define my targets (RoboRIO) and artifacts (deployable files)
 // This is added by GradleRIO's backing project DeployUtils.

src/main/java/frc/robot/Robot.java

@@ -1,6 +1,5 @@
 package frc.robot;
 
-import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.RobotController;
@@ -9,207 +8,93 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.robot.subsystems.Limelights.*;
-import frc.robot.subsystems.Limelights.LimelightHelpers.PoseEstimate;
+import frc.robot.subsystems.Limelights.Constants;
+import frc.robot.subsystems.Limelights.LimelightHelpers;
 import frc.robot.subsystems.Led.LEDSubsystem;
 
 public class Robot extends TimedRobot {
 
     private static final double kLowBatteryVoltage = 11.8;
-    private static final double kLowBatteryDisabledTime = 1.5; //seconds
+    private static final double kLowBatteryDisabledTime = 1.5; // seconds
 
     private final Timer batteryTimer = new Timer();
     private boolean lowBatteryAlert = false;
 
-
     private Command m_autonomousCommand;
 
     private final RobotContainer m_container = new RobotContainer();
     private final LEDSubsystem m_leds = new LEDSubsystem();
 
-    private static final boolean kUseLimelight = true; // Set to true to enable limelight for now
-
-    private double m_autonomousStartTime = -1.0;
+    private static final boolean kUseLimelight = true;
 
+    /**
+     * Initial configuration for all Limelights
+     */
     private void configureLimelights() {
         for (String limelightName : Constants.LimelightConstants.limelightNames) {
             LimelightHelpers.setLEDMode_PipelineControl(limelightName);
             LimelightHelpers.setPipelineIndex(limelightName, 0);
             System.out.println("Configured Limelight: " + limelightName);
         }
     }
-
-    private void updateLimelightTelemetry() {
-        for (String limelightName : Constants.LimelightConstants.limelightNames) {
-            boolean hasTarget = LimelightHelpers.hasTarget(limelightName);
-            SmartDashboard.putBoolean(limelightName + "/HasTarget", hasTarget);
-
-            if (hasTarget) {
-                SmartDashboard.putNumber(limelightName + "/TX", 
-                    LimelightHelpers.getTX(limelightName));
-                SmartDashboard.putNumber(limelightName + "/TY", 
-                    LimelightHelpers.getTY(limelightName));
-                SmartDashboard.putNumber(limelightName + "/TA", 
-                    LimelightHelpers.getTA(limelightName));
-                SmartDashboard.putNumber(limelightName + "/FiducialID", 
-                    LimelightHelpers.getFiducialID(limelightName));
-            }
-
-            SmartDashboard.putNumber(limelightName + "/Pipeline", 
-                LimelightHelpers.getCurrentPipelineIndex(limelightName));
-            SmartDashboard.putNumber(limelightName + "/Latency", 
-                LimelightHelpers.getLatency_Pipeline(limelightName) + 
-                LimelightHelpers.getLatency_Capture(limelightName));
-        }
-    }
-
-    private void updateVisionMeasurement() {
-        var driveState = m_container.drivetrain.getState();
-        var heading = driveState.Pose.getRotation().getDegrees();
-        var omega = driveState.Speeds.omegaRadiansPerSecond;
-
-        for (String limelightName : Constants.LimelightConstants.limelightNames) {
-            LimelightHelpers.setRobotOrientation(
-                limelightName, 
-                heading, 
-                omega * 180.0 / Math.PI,
-                0, 0, 0, 0
-            );
-
-            PoseEstimate llMeasurement = null;
-            var alliance = DriverStation.getAlliance();
-
-            if (alliance.isPresent()) {
-                if (alliance.get() == DriverStation.Alliance.Blue) {
-                    llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName);
-                } else {
-                    llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(limelightName);
-                }
-            } else {
-                llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName);
-            }
-
-            double currentTime = Timer.getFPGATimestamp() * 1000.0;
-
-            if (m_autonomousStartTime < 0 || currentTime - m_autonomousStartTime < 200.0) {
-                continue;
-            }
-
-            if (llMeasurement != null && 
-                llMeasurement.tagCount > 0 && 
-                Math.abs(omega) < 2.0) {
-
-                double xyStdDev = llMeasurement.avgTagDist * 0.5;
-                double rotStdDev = llMeasurement.avgTagDist * 0.5;
-
-                if (llMeasurement.tagCount >= 2) {
-                    xyStdDev *= 0.5;
-                    rotStdDev *= 0.5;
-                }
-
-                SmartDashboard.putNumber(limelightName + "/VisionTagCount", llMeasurement.tagCount);
-                SmartDashboard.putNumber(limelightName + "/VisionAvgDist", llMeasurement.avgTagDist);
-
-                m_container.drivetrain.addVisionMeasurement(
-                    llMeasurement.pose,
-                    llMeasurement.timestampSeconds,
-                    VecBuilder.fill(xyStdDev, xyStdDev, rotStdDev)
-                );
-                //TODO: Not sure if this is working or no
-            }
-        }
-    }
 
     @Override
-    public void testPeriodic() {}
-
     public void robotInit() {
         m_container.robotInit();
 
         if (kUseLimelight) {
             configureLimelights();
         }
 
-        //simulation joystick warning suppression
         if (RobotBase.isSimulation()) {
             DriverStation.silenceJoystickConnectionWarning(true);
         }
 
         batteryTimer.start();
-
     }
 
     @Override
     public void robotPeriodic() {
         CommandScheduler.getInstance().run();
 
-        // Removed getCANUsagePercent() as it doesn't exist in newer WPILib
+        // Standard Robot Telemetry
         SmartDashboard.putNumber("Voltage", RobotController.getBatteryVoltage());
         SmartDashboard.putNumber("CPU Temperature", RobotController.getCPUTemp());
         SmartDashboard.putBoolean("RSL", RobotController.getRSLState());
         SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime());
-
         SmartDashboard.putNumber("Code Runtime (ms)", Timer.getFPGATimestamp() * 1000.0);
 
-        //TODO: Vision updates
+        // Limelight Updates
+        // This handles all telemetry and MegaTag2 odometry fusion internally
         if (kUseLimelight) {
-            updateLimelightTelemetry();
-            updateVisionMeasurement();
-
-            // NOTE: You need to add LimelightHelpers.java to your project
-            // Download from: https://github.com/LimelightVision/limelightlib-wpijava
-            /*
-            for (String limelightName : Constants.LimelightConstants.limelightNames) {
-                LimelightHelpers.setRobotOrientation(limelightName, heading, 0, 0, 0, 0, 0);
-                var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName);
-                double currentTime = Timer.getFPGATimestamp() * 1000.0;
-                if (m_autonomousStartTime < 0 || currentTime - m_autonomousStartTime < 200.0) {
-                    // Skip
-                } else {
-                    if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omega) < 2) {
-                        m_container.drivetrain.addVisionMeasurement(
-                            llMeasurement.pose,
-                            llMeasurement.timestampSeconds,
-                            new double[]{llMeasurement.avgTagDist * 5, llMeasurement.avgTagDist * 5, llMeasurement.avgTagDist * 5}
-                        );
-                    }
-                }
-            }
-            */
+            m_container.visionSubsystem.updateOdometry(m_container.drivetrain);
         }
 
-        if(DriverStation.isEnabled()){
+        // Battery Alert Logic
+        if (DriverStation.isEnabled()) {
             batteryTimer.reset();
         }
-        double batteryVoltage = RobotController.getBatteryVoltage();
-
-        if(batteryVoltage<= kLowBatteryVoltage && batteryTimer.hasElapsed(kLowBatteryDisabledTime)){
-            lowBatteryAlert = true;
-        } else {
-            lowBatteryAlert = false;
-        }
 
+        lowBatteryAlert = (RobotController.getBatteryVoltage() <= kLowBatteryVoltage
+                && batteryTimer.hasElapsed(kLowBatteryDisabledTime));
         SmartDashboard.putBoolean("LowBattery", lowBatteryAlert);
     }
 
-    @Override
-    public void disabledInit() {
-        m_leds.setDisabledMode();
-    }
-
-    @Override
-    public void disabledPeriodic() {}
-
     @Override
     public void autonomousInit() {
-        m_autonomousStartTime = Timer.getFPGATimestamp() * 1000.0;
+        // Signal to VisionSubsystem to start the 200ms safety delay
+        if (kUseLimelight) {
+            m_container.visionSubsystem.resetAutoTimer();
+        }
+
         m_autonomousCommand = m_container.getAutonomousCommand();
         if (m_autonomousCommand != null) {
             m_autonomousCommand.schedule();
         }
+
         m_container.autonomousInit();
         m_leds.setAutonomousMode();
-        
+
         if (kUseLimelight) {
             for (String limelightName : Constants.LimelightConstants.limelightNames) {
                 LimelightHelpers.setPipelineIndex(limelightName, 0);
@@ -218,17 +103,15 @@ public void autonomousInit() {
         }
     }
 
-    @Override
-    public void autonomousPeriodic() {}
-
     @Override
     public void teleopInit() {
         if (m_autonomousCommand != null) {
             m_autonomousCommand.cancel();
         }
+
         m_container.teleopInit();
         m_leds.setTeleopMode();
-        
+
         if (kUseLimelight) {
             for (String limelightName : Constants.LimelightConstants.limelightNames) {
                 LimelightHelpers.setPipelineIndex(limelightName, 0);
@@ -238,17 +121,19 @@ public void teleopInit() {
     }
 
     @Override
-    public void teleopPeriodic() {}
+    public void disabledInit() {
+        m_leds.setDisabledMode();
+    }
 
     @Override
     public void testInit() {
         CommandScheduler.getInstance().cancelAll();
     }
 
-    @Override
-    public void simulationPeriodic() {
-        // This prevents the default message and gives you control
-        // If you have physics simulation, put it here
-        // For now: do nothing fast
-    }
-}
+    // Unused methods kept for structure
+    @Override public void testPeriodic() {}
+    @Override public void disabledPeriodic() {}
+    @Override public void autonomousPeriodic() {}
+    @Override public void teleopPeriodic() {}
+    @Override public void simulationPeriodic() {}
+}

src/main/java/frc/robot/RobotContainer.java

@@ -23,12 +23,12 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.VisionAlign;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.AutoCommands.AutoCommands;
 import frc.robot.subsystems.Climber.Climber;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
-import frc.robot.Superstructure;
-import frc.robot.NetworkTables;
+import frc.robot.subsystems.VisionSubsystem;
 
 
 public class RobotContainer extends TimedRobot {
@@ -53,6 +53,8 @@ public class RobotContainer extends TimedRobot {
 
     public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
 
+    public final VisionSubsystem visionSubsystem = new VisionSubsystem();
+
     public final Climber climber = new Climber(networkTables);
     private final Superstructure superstructure = new Superstructure(networkTables, drivetrain);
     private final AutoCommands autoCommands = new AutoCommands(superstructure, networkTables);
@@ -130,6 +132,13 @@ private void configureBindings() {
             })
         );
 
+        controller.x().whileTrue(new VisionAlign(
+                drivetrain,
+                visionSubsystem,
+                () -> -controller.getLeftY() * MAX_SPEED,
+                () -> -controller.getLeftX() * MAX_SPEED
+        ));
+
         new Trigger(controller.a().whileTrue(drivetrain.applyRequest(() -> brake)));
         new Trigger(controller.b().whileTrue(drivetrain.applyRequest(() -> point.withModuleDirection(new Rotation2d(-controller.getLeftY(), -controller.getLeftX())))));
 

src/main/java/frc/robot/commands/VisionAlign.java

@@ -0,0 +1,43 @@
+package frc.robot.commands;
+
+import java.util.function.DoubleSupplier;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.VisionSubsystem;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+public class VisionAlign extends Command {
+    private final CommandSwerveDrivetrain m_drivetrain;
+    private final VisionSubsystem m_vision;
+    private final DoubleSupplier m_throttleX, m_throttleY;
+
+    // PID constants for rotation alignment
+    private final PIDController m_controller = new PIDController(0.05, 0.0, 0.002);
+    private final SwerveRequest.FieldCentric m_driveRequest = new SwerveRequest.FieldCentric();
+
+    public VisionAlign(CommandSwerveDrivetrain drivetrain, VisionSubsystem vision,
+                       DoubleSupplier throttleX, DoubleSupplier throttleY) {
+        m_drivetrain = drivetrain;
+        m_vision = vision;
+        m_throttleX = throttleX;
+        m_throttleY = throttleY;
+        addRequirements(m_drivetrain);
+    }
+
+    @Override
+    public void execute() {
+        double rotationOutput = 0;
+
+        if (m_vision.hasTarget()) {
+            // Error is the horizontal offset from the crosshair
+            rotationOutput = m_controller.calculate(m_vision.getTX(), 0);
+        }
+
+        // Apply velocities: X and Y from sticks, Rotation from PID
+        m_drivetrain.setControl(m_driveRequest
+                .withVelocityX(m_throttleX.getAsDouble())
+                .withVelocityY(m_throttleY.getAsDouble())
+                .withRotationalRate(rotationOutput));
+    }
+}