From 6e46c37ef36fce707bb8f38fee411910b2f4f057 Mon Sep 17 00:00:00 2001 From: Josephine Otoo Date: Wed, 11 Feb 2026 19:10:37 -0500 Subject: [PATCH] finished loggings and need it checked --- .vscode/launch.json | 12 +++-- build.gradle | 12 ++++- src/main/java/frc/robot/Constants.java | 24 ++++++++++ src/main/java/frc/robot/Main.java | 1 + src/main/java/frc/robot/Robot.java | 24 +++++++++- src/main/java/frc/robot/RobotContainer.java | 14 +++++- src/main/java/frc/robot/Telemetry.java | 5 +- .../frc/robot/subsystems/LimelightSystem.java | 47 ++++++++++++++++--- .../frc/robot/subsystems/SwerveSystem.java | 27 +++++++++++ .../frc/robot/subsystems/TurretSystem.java | 26 +++++++++- vendordeps/AdvantageKit.json | 35 ++++++++++++++ 11 files changed, 209 insertions(+), 18 deletions(-) create mode 100644 vendordeps/AdvantageKit.json diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..41297e8 100644 --- a/.vscode/launch.json +++ b/.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 } ] } diff --git a/build.gradle b/build.gradle index f2ceadb..f0d4834 100644 --- a/build.gradle +++ b/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 + +} + diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4130dbe..10517da 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 + } } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index fe215d7..20b8aef 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -13,3 +13,4 @@ public static void main(String... args) { RobotBase.startRobot(Robot::new); } } + diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cea1ffc..a63a7c0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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() {} } + diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8138600..1550834 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -11,7 +12,7 @@ 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"); } } + + diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java index 2f0a07a..6a4457d 100644 --- a/src/main/java/frc/robot/Telemetry.java +++ b/src/main/java/frc/robot/Telemetry.java @@ -20,14 +20,14 @@ 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); } @@ -35,4 +35,5 @@ public static void logString(String key, String value) { private static String timestamp() { return String.format("%.3f", Timer.getFPGATimestamp()); } + } diff --git a/src/main/java/frc/robot/subsystems/LimelightSystem.java b/src/main/java/frc/robot/subsystems/LimelightSystem.java index e3f8f23..b5a5e85 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSystem.java +++ b/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 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; - + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveSystem.java b/src/main/java/frc/robot/subsystems/SwerveSystem.java index 2b88066..c326875 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSystem.java +++ b/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() { diff --git a/src/main/java/frc/robot/subsystems/TurretSystem.java b/src/main/java/frc/robot/subsystems/TurretSystem.java index 5e04073..885f7bc 100644 --- a/src/main/java/frc/robot/subsystems/TurretSystem.java +++ b/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; } } \ No newline at end of file diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..162ad66 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "26.0.0", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2026", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "26.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "26.0.0", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file