finished loggings and need it checked

.vscode/launch.json

@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Main",
+      "request": "launch",
+      "mainClass": "frc.robot.Main",
+      "projectName": "RA26_RobotCode"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2026.1.1"
+    id "edu.wpi.first.GradleRIO" version "2026.2.1"
 }
 
 java {
@@ -73,6 +73,9 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 }
 
 test {
@@ -104,4 +107,11 @@ wpi.java.configureTestTasks(test)
 // Configure string concat to always inline compile
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
+
 }
+task(replayWatch, type: JavaExec) {
+    mainClass = "org.littletonrobotics.junction.ReplayWatch"
+    classpath = sourceSets.main.runtimeClasspath
+
+}
+

src/main/java/frc/robot/Constants.java

@@ -35,4 +35,28 @@ public static class FieldConstants {
         public final static double k_width = Units.feetToMeters(26.0) + Units.inchesToMeters(5);
         public final static double k_length = Units.feetToMeters(57.0) + Units.inchesToMeters(6.0 + (7.0 / 8.0));
     }
+
+    /**
+     * Logging-related global constants. Toggle features here so the rest of the code
+     * can read a single source of truth for telemetry / autolog / recorder configuration.
+     */
+    public static class LoggingConstants {
+        /** Enable simple Telemetry (Telemetry.log / SmartDashboard) */
+        public static final boolean k_enableTelemetry = true;
+
+        /** Enable AutoLog / recording (if you add an AutoLog/recorder integration elsewhere) */
+        public static final boolean k_enableAutolog = false;
+
+        /** Enable junction Logger recordings (org.littletonrobotics.junction.Logger) */
+        public static final boolean k_enableJunctionLogger = true;
+
+        /** Directory on the robot (or host) where logs should be written if recording is enabled. */
+        public static final String k_logDirectory = "/home/lvuser/logs";
+
+        /** Base filename to use for recordings (timestamp/extension may be added by recorder). */
+        public static final String k_logFileBaseName = "robotLog";
+
+        /** Target logging period in seconds for periodic recording (0 disables periodic flush). */
+        public static final double k_logPeriodSeconds = 0.02; // 20 ms
+    }
 }

src/main/java/frc/robot/Main.java

@@ -13,3 +13,4 @@ public static void main(String... args) {
     RobotBase.startRobot(Robot::new);
   }
 }
+

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

@@ -4,17 +4,36 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+import org.littletonrobotics.junction.Logger;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
 
   private final RobotContainer m_robotContainer;
 
   public Robot() {
     m_robotContainer = new RobotContainer();
+    Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value
+
+if (isReal()) {
+    Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
+    Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+} else {
+    setUseTiming(false); // Run as fast as possible
+    String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
+    Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
+    Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
+}
+
+Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
   }
 
   @Override
@@ -70,3 +89,4 @@ public void testPeriodic() {}
   @Override
   public void testExit() {}
 }
+

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

@@ -4,14 +4,15 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 
 import frc.robot.controls.DriverController;
 import frc.robot.subsystems.SwerveSystem;
 
 public class RobotContainer {
-  private final SwerveSystem m_swerve = new SwerveSystem();;
+  private final SwerveSystem m_swerve = new SwerveSystem();
 
   public RobotContainer() {
     configureBindings();
@@ -23,13 +24,24 @@ public RobotContainer() {
         DriverController.getController().getRightX() * -1 
       ) 
     );
+
+    DataLogManager.log("RobotContainer: Initializing...");
+
+    configureBindings();
   }
 
+
   private void configureBindings() {
     DriverController.configure(Constants.ControllerConstants.kDriverControllerPort, m_swerve);
+    DataLogManager.log("Button X Pressed - Activating Intake");
+    DataLogManager.log("Button Y Pressed - Activating Intake");
+    DataLogManager.log("Button A Pressed - Activating Intake");
+    DataLogManager.log("Button B Pressed - Activating Intake");
   }
 
   public Command getAutonomousCommand() {
     return Commands.print("No autonomous command configured");
   }
 }
+
+

src/main/java/frc/robot/Telemetry.java

@@ -20,19 +20,20 @@ public static void log(String message) {
     /** Log a number to SmartDashboard. */
     public static void logNumber(String key, double value) {
         if (!enabled) return;
-
+        
         SmartDashboard.putNumber(key, value);
     }
 
     /** Log a string to SmartDashboard. */
     public static void logString(String key, String value) {
         if (!enabled) return;
-
+        
         SmartDashboard.putString(key, value);
     }
 
     /** Timestamp helper. */
     private static String timestamp() {
         return String.format("%.3f", Timer.getFPGATimestamp());
     }
+
 }

src/main/java/frc/robot/subsystems/LimelightSystem.java

@@ -3,6 +3,8 @@
 import java.util.Optional;
 
 import static edu.wpi.first.units.Units.DegreesPerSecond;
+import frc.robot.Telemetry;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import limelight.Limelight;
@@ -29,6 +31,7 @@ public class LimelightSystem extends SubsystemBase {
     private SwerveDrive swerveDrive; 
     private Optional<PoseEstimate> visionEstimate;
     private boolean allowed;
+    private String lastRejectReason = "";
 
     public LimelightSystem(SwerveDrive swerve){
         limelight = new Limelight("limelight");
@@ -50,13 +53,32 @@ public LimelightSystem(SwerveDrive swerve){
 
     @Override
     public void periodic(){
-        //if the pose is there
+        // if the pose is there
         visionEstimate.ifPresent((PoseEstimate poseEstimate) -> {
             this.allowed = this.exceptions(poseEstimate);
+
+            // Publish numeric telemetry to SmartDashboard via Telemetry helper
+            Telemetry.logNumber("Limelight/tagCount", (double) poseEstimate.tagCount);
+            Telemetry.logNumber("Limelight/poseX_m", poseEstimate.pose.getX());
+            Telemetry.logNumber("Limelight/poseY_m", poseEstimate.pose.getY());
+            Telemetry.logNumber("Limelight/poseZ_m", poseEstimate.pose.getZ());
+            Telemetry.logNumber("Limelight/poseTimestamp", poseEstimate.timestampSeconds);
+            Telemetry.logNumber("Robot/vx_mps", swerveDrive.getRobotVelocity().vxMetersPerSecond);
+            Telemetry.logNumber("Robot/vy_mps", swerveDrive.getRobotVelocity().vyMetersPerSecond);
+
+            // Extra single telemetry: latency (seconds)
+            double latency = Timer.getFPGATimestamp() - poseEstimate.timestampSeconds;
+            Telemetry.logNumber("Limelight/latency_s", latency);
+
+            Telemetry.logString("Limelight/accepted", this.allowed ? "true" : "false");
+
             if (this.allowed) {
                 swerveDrive.addVisionMeasurement(
                     poseEstimate.pose.toPose2d(),
                     poseEstimate.timestampSeconds);
+            } else {
+                // always write reject reason to dashboard (human readable)
+                Telemetry.logString("Limelight/rejectReason", lastRejectReason);
             }
         });
     }
@@ -69,15 +91,26 @@ public void onEnabled(){
     }
 
     public boolean exceptions(PoseEstimate foo) {
-        if (foo.tagCount <= 0) { return false; }
-        if (foo.pose.getX() <= 0 || foo.pose.getX() > Constants.FieldConstants.k_length) { return false; }
-        if (foo.pose.getY() <= 0 || foo.pose.getY() > Constants.FieldConstants.k_width) { return false;}
-        if (Math.abs(swerveDrive.getRobotVelocity().vxMetersPerSecond) > 720) { return false; }
-        if (Math.abs(swerveDrive.getRobotVelocity().vyMetersPerSecond) > 720) { return false; }
+        if (foo.tagCount <= 0) { 
+            lastRejectReason = "no tags";
+            return false; }
+        if (foo.pose.getX() <= 0 || foo.pose.getX() > Constants.FieldConstants.k_length) { 
+            lastRejectReason = "x out of bounds: " + foo.pose.getX();
+            return false; }
+        if (foo.pose.getY() <= 0 || foo.pose.getY() > Constants.FieldConstants.k_width) { 
+            lastRejectReason = "y out of bounds: " + foo.pose.getY();
+            return false;}
+        if (Math.abs(swerveDrive.getRobotVelocity().vxMetersPerSecond) > 720) { 
+            lastRejectReason = "vx too high: " + swerveDrive.getRobotVelocity().vxMetersPerSecond;
+            return false; }
+        if (Math.abs(swerveDrive.getRobotVelocity().vyMetersPerSecond) > 720) { 
+            lastRejectReason = "vy too high: " + swerveDrive.getRobotVelocity().vyMetersPerSecond;
+            return false; }
 
         // TODO make sure the april tag area is legibi
+        lastRejectReason = "";
 
         return true;
-        
+
     }
 }

src/main/java/frc/robot/subsystems/SwerveSystem.java

@@ -22,6 +22,7 @@
 import swervelib.SwerveModule;
 import frc.robot.Telemetry;
 import swervelib.SwerveInputStream;
+import org.littletonrobotics.junction.Logger;
 
 public class SwerveSystem extends SubsystemBase {
     SwerveDriveKinematics m_kinematics;
@@ -57,6 +58,8 @@ public SwerveSystem() {
 
         Command onEnable = Commands.runOnce(() -> {
             Telemetry.log("running...");
+            // Record that swerve was enabled
+            Logger.recordOutput("Swerve/onEnabled", 1.0);
             m_limelight.onEnabled();
         });
 
@@ -75,6 +78,29 @@ public SwerveSystem() {
     @Override
     public void periodic(){
         m_limelight.periodic();
+        // Publish useful telemetry for debugging and logging
+        try {
+            // Robot chassis velocities
+            Telemetry.logNumber("Robot/vx_mps", swerveDrive.getRobotVelocity().vxMetersPerSecond);
+            Telemetry.logNumber("Robot/vy_mps", swerveDrive.getRobotVelocity().vyMetersPerSecond);
+
+            Logger.recordOutput("Robot/vx_mps", swerveDrive.getRobotVelocity().vxMetersPerSecond);
+            Logger.recordOutput("Robot/vy_mps", swerveDrive.getRobotVelocity().vyMetersPerSecond);
+
+            // Odometry pose
+            Pose2d pose = this.odometry.getPoseMeters();
+            Telemetry.logNumber("Robot/poseX_m", pose.getX());
+            Telemetry.logNumber("Robot/poseY_m", pose.getY());
+            Telemetry.logNumber("Robot/poseHeading_deg", pose.getRotation().getDegrees());
+
+            Logger.recordOutput("Robot/poseX_m", pose.getX());
+            Logger.recordOutput("Robot/poseY_m", pose.getY());
+            Logger.recordOutput("Robot/poseHeading_deg", pose.getRotation().getDegrees());
+        } catch (Exception ex) {
+            // Avoid throwing from periodic if telemetry fails; still print for visibility
+            Telemetry.logString("Swerve/telemetryError", ex.getMessage() == null ? "unknown" : ex.getMessage());
+        }
+
     }
 
     @Override
@@ -91,6 +117,7 @@ public Command driveCommand(double translationX, double translationY,
             true,
             false);
         });
+
     }
 
     public SwerveDrive getSwerveDrive() {

src/main/java/frc/robot/subsystems/TurretSystem.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import org.littletonrobotics.junction.Logger;
 
 
 public class TurretSystem extends SubsystemBase {
@@ -16,14 +17,35 @@ public static double calculateLaunchVelocity(double launchAngle, double xDisplac
         double sin = Math.sin(Math.toRadians(launchAngle));
         double divisor = 2.0 * cos * (xDisplacement * sin - yDisplacement * cos);
         double radicand = Constants.TurretConstants.k_gravitationalAcceleration / divisor;
-        return xDisplacement * Math.sqrt(radicand);
+        double velocity = xDisplacement * Math.sqrt(radicand);
+
+        // Log inputs and intermediate values
+        Logger.recordOutput("Turret/launchAngle_deg", launchAngle);
+        Logger.recordOutput("Turret/xDisp_m", xDisplacement);
+        Logger.recordOutput("Turret/yDisp_m", yDisplacement);
+        Logger.recordOutput("Turret/launch/divisor", divisor);
+        Logger.recordOutput("Turret/launch/radicand", radicand);
+        Logger.recordOutput("Turret/launchVelocity_mps", velocity);
+
+        return velocity;
     }
     public static double calculateLaunchAngle(double launchVelocity, double xDisplacement, double yDisplacement) {
+        // Placeholder implementation; log inputs and return 0.0 for now
+        Logger.recordOutput("Turret/launchVelocity_mps", launchVelocity);
+        Logger.recordOutput("Turret/xDisp_m", xDisplacement);
+        Logger.recordOutput("Turret/yDisp_m", yDisplacement);
+        Logger.recordOutput("Turret/launchAngle_deg", 0.0);
         return 0.0;
     }
     public static double calculateMaxHeightFromLaunch(double launchAngle, double launchVelocity) {
         double vYSquared = Math.pow(launchVelocity * Math.sin(Math.toRadians(launchAngle)), 2.0);
         double twoG = 2.0 * Constants.TurretConstants.k_gravitationalAcceleration;
-        return vYSquared / twoG;
+        double maxHeight = vYSquared / twoG;
+
+        Logger.recordOutput("Turret/launchAngle_deg", launchAngle);
+        Logger.recordOutput("Turret/launchVelocity_mps", launchVelocity);
+        Logger.recordOutput("Turret/maxHeight_m", maxHeight);
+
+        return maxHeight;
     }
 }