Add Advantage Kit Framework (#24)

Co-authored-by: Alex Resnick <adr8292@gmail.com>

.gitignore

@@ -176,3 +176,5 @@ logs/
 
 # Folder that has CTRE Phoenix Sim device config storage
 ctre_sim/
+
+BuildConstants.java

build.gradle

@@ -1,6 +1,7 @@
 plugins {
     id "java"
     id "edu.wpi.first.GradleRIO" version "2024.2.1"
+    id "com.peterabeles.gversion" version "1.10"
 }
 
 java {
@@ -49,7 +50,31 @@ def includeDesktopSupport = false
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 5.
+
+repositories {
+    maven {
+        url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit")
+        credentials {
+            username = "Mechanical-Advantage-Bot"
+            password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051"
+        }
+    }
+    mavenLocal()
+}
+
+configurations.all {
+    exclude group: "edu.wpi.first.wpilibj"
+}
+
+task(checkAkitInstall, dependsOn: "classes", type: JavaExec) {
+    mainClass = "org.littletonrobotics.junction.CheckInstall"
+    classpath = sourceSets.main.runtimeClasspath
+}
+compileJava.finalizedBy checkAkitInstall
+
 dependencies {
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
 
@@ -101,4 +126,15 @@ javadoc {
             "  src=\"https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.7/MathJax.js?config=TeX-MML-AM_CHTML\">" +
             "</script>"
     }
-}
+}
+
+// Create version file
+project.compileJava.dependsOn(createVersionFile)
+gversion {
+    srcDir       = "src/main/java/"
+    classPackage = "frc.robot"
+    className    = "BuildConstants"
+    dateFormat   = "yyyy-MM-dd HH:mm:ss z"
+    timeZone     = "America/New_York"
+    indent       = "  "
+}

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

@@ -4,18 +4,39 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+import java.util.Scanner;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 /**
  * Runs tasks on Roborio in this file.
  */
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
     private RobotContainer robotContainer;
     private Command autoChooser;
 
+    /**
+     * Robnot Run type
+     */
+    public static enum RobotRunType {
+        /** Real Robot. */
+        kReal,
+        /** Simulation runtime. */
+        kSimulation,
+        /** Replay runtime. */
+        kReplay;
+    }
 
+    public RobotRunType robotRunType = RobotRunType.kReal;
 
     // private Ultrasonic ultrasonic = new Ultrasonic();
     /**
@@ -24,11 +45,56 @@ public class Robot extends TimedRobot {
      */
     @Override
     public void robotInit() {
-        // Instantiate our RobotContainer. This will perform all our button bindings,
-        // and put our
-        // autonomous chooser on the dashboard.
-        robotContainer = new RobotContainer();
+        // Record metadata
+        Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+        Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+        Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
+        Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+        Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+        switch (BuildConstants.DIRTY) {
+            case 0:
+                Logger.recordMetadata("GitDirty", "All changes committed");
+                break;
+            case 1:
+                Logger.recordMetadata("GitDirty", "Uncommitted changes");
+                break;
+            default:
+                Logger.recordMetadata("GitDirty", "Unknown");
+                break;
+        }
+
 
+
+        if (isReal()) {
+            Logger.addDataReceiver(new WPILOGWriter("/media/sda1")); // Log to a USB stick
+            Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+            // new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
+            setUseTiming(true);
+            robotRunType = RobotRunType.kReal;
+        } else {
+            String logPath = findReplayLog();
+            if (logPath == null) {
+                Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+                setUseTiming(true);
+                robotRunType = RobotRunType.kSimulation;
+            } else {
+                // (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
+                setUseTiming(false); // Run as fast as possible
+                robotRunType = RobotRunType.kReplay;
+
+            }
+        }
+        // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the
+        // "Understanding Data Flow" page
+        Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values
+
+        // Instantiate our RobotContainer. This will perform all our button bindings,
+        // and put our autonomous chooser on the dashboard.
+        robotContainer = new RobotContainer(robotRunType);
     }
 
     /**
@@ -39,6 +105,7 @@ public void robotInit() {
      * This runs after the mode specific periodic functions, but before LiveWindow and
      * SmartDashboard integrated updating.
      */
+
     @Override
     public void robotPeriodic() {
         // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
@@ -97,4 +164,38 @@ public void testInit() {
     /** This function is called periodically during test mode. */
     @Override
     public void testPeriodic() {}
+
+
+    private static final String environmentVariable = "AKIT_LOG_PATH";
+    private static final String advantageScopeFileName = "akit-log-path.txt";
+
+    /**
+     * Finds the path to a log file for replay, using the following priorities: 1. The value of the
+     * "AKIT_LOG_PATH" environment variable, if set 2. The file currently open in AdvantageScope, if
+     * available 3. The result of the prompt displayed to the user
+     */
+    public static String findReplayLog() {
+        // Read environment variables
+        String envPath = System.getenv(environmentVariable);
+        if (envPath != null) {
+            System.out.println("Using log from " + environmentVariable
+                + " environment variable - \"" + envPath + "\"");
+            return envPath;
+        }
+
+        // Read file from AdvantageScope
+        Path advantageScopeTempPath =
+            Paths.get(System.getProperty("java.io.tmpdir"), advantageScopeFileName);
+        String advantageScopeLogPath = null;
+        try (Scanner fileScanner = new Scanner(advantageScopeTempPath)) {
+            advantageScopeLogPath = fileScanner.nextLine();
+        } catch (IOException e) {
+            System.out.println("Something went wrong");
+        }
+        if (advantageScopeLogPath != null) {
+            System.out.println("Using log from AdvantageScope - \"" + advantageScopeLogPath + "\"");
+            return advantageScopeLogPath;
+        }
+        return null;
+    }
 }

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

@@ -5,8 +5,13 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Robot.RobotRunType;
+import frc.robot.subsystems.drive.Drivetrain;
+import frc.robot.subsystems.drive.DrivetrainIO;
+import frc.robot.subsystems.drive.DrivetrainVictorSP;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -20,21 +25,27 @@ public class RobotContainer {
     private final CommandXboxController operator = new CommandXboxController(Constants.operatorID);
 
     // Initialize AutoChooser Sendable
-    private final SendableChooser<Command> autoChooser = new SendableChooser<>();
-
-    // Field Relative and openLoop Variables
-    boolean fieldRelative;
-    boolean openLoop;
-
+    private final SendableChooser<String> autoChooser = new SendableChooser<>();
 
     /* Subsystems */
+    private Drivetrain drivetrain;
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
-    public RobotContainer() {
+    public RobotContainer(RobotRunType runtimeType) {
         SmartDashboard.putData("Choose Auto: ", autoChooser);
-        autoChooser.setDefaultOption("Do Nothing", new WaitCommand(1));
+        autoChooser.setDefaultOption("Wait 1 Second", "wait");
+        switch (runtimeType) {
+            case kReal:
+                drivetrain = new Drivetrain(new DrivetrainVictorSP());
+                break;
+            case kSimulation:
+                // drivetrain = new Drivetrain(new DrivetrainSim() {});
+                break;
+            default:
+                drivetrain = new Drivetrain(new DrivetrainIO() {});
+        }
         // Configure the button bindings
         configureButtonBindings();
     }
@@ -53,8 +64,15 @@ private void configureButtonBindings() {}
      * @return Returns autonomous command selected.
      */
     public Command getAutonomousCommand() {
-        // return new P1_3B(swerveDrive, shooter, innerMagazine, outerMagazine, intake, turret,
-        // vision);
-        return autoChooser.getSelected();
+        Command autocommand;
+        String stuff = autoChooser.getSelected();
+        switch (stuff) {
+            case "wait":
+                autocommand = new WaitCommand(1.0);
+                break;
+            default:
+                autocommand = new InstantCommand();
+        }
+        return autocommand;
     }
 }

src/main/java/frc/robot/subsystems/drive/Drivetrain.java

@@ -0,0 +1,27 @@
+package frc.robot.subsystems.drive;
+
+import org.littletonrobotics.junction.Logger;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * Drivetrain subsystem.
+ */
+
+public class Drivetrain extends SubsystemBase {
+    private DrivetrainIO io;
+    private DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged();
+
+    /**
+     * Create Wrist Intake Subsystem
+     */
+    public Drivetrain(DrivetrainIO io) {
+        this.io = io;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Drivetrain", inputs);
+    }
+}
+

src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java

@@ -0,0 +1,25 @@
+package frc.robot.subsystems.drive;
+
+import org.littletonrobotics.junction.AutoLog;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/**
+ * DrivetrainIO interface
+ */
+public interface DrivetrainIO {
+    /**
+     * Drivetrain IO
+     */
+    @AutoLog
+    public static class DrivetrainIOInputs {
+        public Rotation2d gyroYaw = new Rotation2d();
+    }
+
+    /** Updates the set of loggable inputs. */
+    public default void updateInputs(DrivetrainIOInputs inputs) {}
+
+    /** Run the motor at the specified voltage. */
+    public default void setDriveVoltage(double lvolts, double rvolts) {}
+
+
+}

src/main/java/frc/robot/subsystems/drive/DrivetrainVictorSP.java

@@ -0,0 +1,43 @@
+package frc.robot.subsystems.drive;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
+
+/**
+ * Drivetrain VictorSP
+ */
+public class DrivetrainVictorSP implements DrivetrainIO {
+    private final VictorSP left1 = new VictorSP(4);
+    private final VictorSP left2 = new VictorSP(5);
+
+    private final VictorSP right1 = new VictorSP(6);
+    private final VictorSP right2 = new VictorSP(7);
+
+    private AHRS gyro = new AHRS(SPI.Port.kMXP);
+
+    /**
+     * Drivetrain VictorSP
+     */
+    public DrivetrainVictorSP() {
+        left1.addFollower(left2);
+        right1.addFollower(right2);
+        right1.setInverted(true);
+        right2.setInverted(true);
+    }
+
+    @Override
+    public void updateInputs(DrivetrainIOInputs inputs) {
+        inputs.gyroYaw = Rotation2d.fromDegrees(gyro.getYaw());
+    }
+
+    /**
+     * Drive Voltage
+     */
+    public void setDriveVoltage(double lvolts, double rvolts) {
+        left1.set(lvolts);
+        right1.set(rvolts);
+    }
+
+}