feat(refactoring): restructure, add AdvantageKit into the fray

build.gradle

@@ -77,6 +77,9 @@ dependencies {
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
     implementation "org.jetbrains.kotlin:kotlin-stdlib-jdk8"
+
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
 }
 
 test {
@@ -109,6 +112,14 @@ tasks.withType(JavaCompile) {
 }
 repositories {
     mavenCentral()
+    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()
 }
 kotlin {
     jvmToolchain(17)
@@ -127,4 +138,13 @@ task updateVendordeps(type: Exec) {
         println errorOutput
     }
 }
-tasks.build.dependsOn updateVendordeps
+tasks.build.dependsOn updateVendordeps
+
+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

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

@@ -4,24 +4,62 @@
 package frc.robot
 
 import com.ctre.phoenix6.SignalLogger
+import com.pathplanner.lib.commands.PathfindingCommand
+import com.pathplanner.lib.pathfinding.Pathfinding
 import edu.wpi.first.wpilibj.DriverStation
-import edu.wpi.first.wpilibj.TimedRobot
+import edu.wpi.first.wpilibj.PowerDistribution
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.CommandScheduler
+import frc.robot.util.LocalADStarAK
+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
 
-class Robot : TimedRobot() {
+
+class Robot : LoggedRobot() {
     private var m_autonomousCommand: Command? = null
 
     private var m_robotContainer: RobotContainer? = null
 
     override fun robotInit() {
+
+        Pathfinding.setPathfinder(LocalADStarAK())
+
+        Logger.recordMetadata("ProjectName", "FRC2024-offseason") // Set a metadata value
+
+        if (isReal()) {
+            Logger.addDataReceiver(WPILOGWriter()) // Log to a USB stick ("/U/logs") TODO get usb stick
+            Logger.addDataReceiver(NT4Publisher()) // Publish data to NetworkTables
+            PowerDistribution(1, PowerDistribution.ModuleType.kRev) // Enables power distribution logging
+        } else {
+            setUseTiming(false) // Run as fast as possible
+            val logPath = LogFileUtil.findReplayLog() // Pull the replay log from AdvantageScope (or prompt the user)
+            Logger.setReplaySource(WPILOGReader(logPath)) // Read replay log
+            Logger.addDataReceiver(
+                WPILOGWriter(
+                    LogFileUtil.addPathSuffix(
+                        logPath,
+                        "_sim"
+                    )
+                )
+            ) // Save outputs to a new log
+        }
+
+
+// Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the "Understanding Data Flow" page
+        Logger.start() // Start logging! No more data receivers, replay sources, or metadata values may be added.
         m_robotContainer = RobotContainer()
 
         m_robotContainer!!.drivetrain.daqThread.setThreadPriority(99)
 
 
         DriverStation.silenceJoystickConnectionWarning(true)
         SignalLogger.start()
+
+        PathfindingCommand.warmupCommand().schedule();
     }
 
     override fun robotPeriodic() {

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

@@ -14,26 +14,25 @@ import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.math.geometry.Translation2d
 import edu.wpi.first.wpilibj.DriverStation
-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.Commands
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import edu.wpi.first.wpilibj2.command.button.Trigger
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
-import frc.robot.subsystems.vision.Limelight
+import frc.robot.subsystems.vision.VisionSubsystem
 import frc.robot.generated.TunerConstants
 import frc.robot.subsystems.ArmSubsystem
-import frc.robot.subsystems.CommandSwerveDrivetrain
+import frc.robot.subsystems.drive.CommandSwerveDrivetrain
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser
 import java.util.function.Supplier
 import kotlin.math.atan2
 import kotlin.math.cos
 import kotlin.math.sin
 
 class RobotContainer {
-    private val autoChooser: SendableChooser<Command>
-    private val controlChooser: SendableChooser<String> = SendableChooser<String>()
-    private val speedChooser: SendableChooser<Double> = SendableChooser<Double>()
+    private val autoChooser: LoggedDashboardChooser<Command>
+    private val controlChooser: LoggedDashboardChooser<String> = LoggedDashboardChooser<String>("Control Method Chooser")
+    private val speedChooser: LoggedDashboardChooser<Double> = LoggedDashboardChooser<Double>("Speed Chooser")
     private var MaxSpeed: Double = TunerConstants.kSpeedAt12VoltsMps // Initial max is true top speed
     private val TurtleSpeed = 0.1 // Reduction in speed from Max Speed, 0.1 = 10%
     private val MaxAngularRate =
@@ -47,7 +46,7 @@ class RobotContainer {
     var op: CommandXboxController = CommandXboxController(1) // operator xbox controller
     var drivetrain: CommandSwerveDrivetrain = TunerConstants.DriveTrain // drivetrain
     val armSubsystem: ArmSubsystem = ArmSubsystem
-    val limelight: Limelight = Limelight(drivetrain)
+    val visionSubsystem: VisionSubsystem = VisionSubsystem(drivetrain)
 
 
     // Slew Rate Limiters to limit acceleration of joystick inputs
@@ -66,7 +65,7 @@ class RobotContainer {
     var forwardStraight: SwerveRequest.RobotCentric = SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage)
     var point: SwerveRequest.PointWheelsAt = SwerveRequest.PointWheelsAt()
 
-    var vision: Limelight = Limelight(drivetrain)
+    var vision: VisionSubsystem = VisionSubsystem(drivetrain)
 
     var logger: Telemetry = Telemetry(MaxSpeed)
 
@@ -108,20 +107,23 @@ class RobotContainer {
         )
         drv.leftBumper().onFalse(
             Commands.runOnce({
-                MaxSpeed = TunerConstants.kSpeedAt12VoltsMps * speedChooser.getSelected()
+                MaxSpeed = TunerConstants.kSpeedAt12VoltsMps * speedChooser.get()
             })
                 .andThen({ AngularRate = MaxAngularRate })
         )
 
+        drv.leftBumper().onTrue(drivetrain.rotateToAngle(visionSubsystem.getNoteCamAngle()))
+        /* TODO make sure this works and probably also see if we can't get it to also move the robot forward a bit to center it w/ the arm */
+
         if (Utils.isSimulation()) {
             drivetrain.seedFieldRelative(Pose2d(Translation2d(), Rotation2d.fromDegrees(0.0)))
         }
         drivetrain.registerTelemetry { state: SwerveDrivetrain.SwerveDriveState -> logger.telemeterize(state) }
 
-        val controlPick = Trigger { lastControl !== controlChooser.getSelected() }
+        val controlPick = Trigger { lastControl !== controlChooser.get() }
         controlPick.onTrue(Commands.runOnce({ newControlStyle() }))
 
-        val speedPick = Trigger { lastSpeed !== speedChooser.getSelected() }
+        val speedPick = Trigger { lastSpeed !== speedChooser.get() }
         speedPick.onTrue(Commands.runOnce({ newSpeed() }))
 
         drv.x().and(drv.pov(0)).whileTrue(drivetrain.runDriveQuasiTest(SysIdRoutine.Direction.kForward))
@@ -140,21 +142,20 @@ class RobotContainer {
         drv.back().and(drv.pov(0)).whileTrue(drivetrain.runDriveSlipTest())
 
 
-        drv.rightBumper().whileTrue(drivetrain.rotateToAngle(limelight.getNoteCamAngle()))
+        drv.rightBumper().whileTrue(drivetrain.rotateToAngle(visionSubsystem.getNoteCamAngle()))
     }
 
     init {
         // Detect if controllers are missing / Stop multiple warnings
         DriverStation.silenceJoystickConnectionWarning(true)
 
         // Build an auto chooser. This will use Commands.none() as the default option.
-        autoChooser = AutoBuilder.buildAutoChooser()
+        autoChooser = LoggedDashboardChooser("Auto Chooser", AutoBuilder.buildAutoChooser())
 
-        controlChooser.setDefaultOption("2 Joysticks", "2 Joysticks")
+        controlChooser.addDefaultOption("2 Joysticks", "2 Joysticks")
         controlChooser.addOption("1 Joystick Rotation Triggers", "1 Joystick Rotation Triggers")
         controlChooser.addOption("Split Joysticks Rotation Triggers", "Split Joysticks Rotation Triggers")
         controlChooser.addOption("2 Joysticks with Gas Pedal", "2 Joysticks with Gas Pedal")
-        SmartDashboard.putData("Control Chooser", controlChooser)
 
         speedChooser.addOption("100%", 1.0)
         speedChooser.addOption("95%", 0.95)
@@ -163,25 +164,22 @@ class RobotContainer {
         speedChooser.addOption("80%", 0.8)
         speedChooser.addOption("75%", 0.75)
         speedChooser.addOption("70%", 0.7)
-        speedChooser.setDefaultOption("65%", 0.65)
+        speedChooser.addDefaultOption("65%", 0.65)
         speedChooser.addOption("60%", 0.6)
         speedChooser.addOption("55%", 0.55)
         speedChooser.addOption("50%", 0.5)
         speedChooser.addOption("35%", 0.35)
-        SmartDashboard.putData("Speed Limit", speedChooser)
-
-        SmartDashboard.putData("Auto Chooser", autoChooser)
 
         configureBindings()
     }
 
     val autoCommand: Command?
         get() =/* First put the drivetrain into auto run mode, then run the auto */
-            autoChooser.getSelected()
+            autoChooser.get()
 
     private fun newControlStyle() {
-        lastControl = controlChooser.getSelected()
-        when (controlChooser.getSelected()) {
+        lastControl = controlChooser.get()
+        when (controlChooser.get()) {
             "2 Joysticks" -> controlStyle = Supplier<SwerveRequest?> {
                 drive.withVelocityX(-drv.leftY * MaxSpeed) // Drive forward -Y
                     .withVelocityY(-drv.leftX * MaxSpeed) // Drive left with negative X (left)
@@ -216,7 +214,7 @@ class RobotContainer {
     }
 
     private fun newSpeed() {
-        lastSpeed = speedChooser.getSelected()
+        lastSpeed = speedChooser.get()
         MaxSpeed = TunerConstants.kSpeedAt12VoltsMps * lastSpeed
     }
 

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

@@ -12,6 +12,8 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj.util.Color
 import edu.wpi.first.wpilibj.util.Color8Bit
+import org.littletonrobotics.junction.AutoLog
+import org.littletonrobotics.junction.AutoLogOutput
 
 class Telemetry
 /**
@@ -38,6 +40,7 @@ class Telemetry
     private val odomFreq: DoublePublisher = driveStats.getDoubleTopic("Odometry Frequency").publish()
 
     /* Keep a reference of the last pose to calculate the speeds */
+    @AutoLogOutput
     private var m_lastPose = Pose2d()
     private var lastTime = Utils.getCurrentTimeSeconds()
 

src/main/java/frc/robot/generated/TunerConstants.kt

@@ -7,7 +7,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory
 import edu.wpi.first.math.util.Units
-import frc.robot.subsystems.CommandSwerveDrivetrain
+import frc.robot.subsystems.drive.CommandSwerveDrivetrain
 
 // Generated by the Tuner X Swerve Project Generator
 // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html

src/main/java/frc/robot/subsystems/ArmSubsystem.kt

@@ -9,6 +9,7 @@ import edu.wpi.first.math.controller.PIDController
 import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 
 import edu.wpi.first.wpilibj2.command.Command

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.kt → src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt

@@ -1,4 +1,4 @@
-package frc.robot.subsystems
+package frc.robot.subsystems.drive
 
 import com.ctre.phoenix6.Utils
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain
@@ -7,7 +7,6 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest
 import com.ctre.phoenix6.signals.NeutralModeValue
 import com.pathplanner.lib.auto.AutoBuilder
-import com.pathplanner.lib.commands.FollowPathCommand
 import com.pathplanner.lib.commands.PathPlannerAuto
 import com.pathplanner.lib.controllers.PPHolonomicDriveController
 import com.pathplanner.lib.path.GoalEndState
@@ -29,9 +28,10 @@ import edu.wpi.first.wpilibj.RobotController
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Subsystem
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
-import frc.robot.Util.ModifiedSignalLogger
-import frc.robot.Util.SwerveVoltageRequest
+import frc.robot.util.ModifiedSignalLogger
+import frc.robot.util.SwerveVoltageRequest
 import frc.robot.generated.TunerConstants
+import org.littletonrobotics.junction.AutoLogOutput
 import java.util.*
 import java.util.function.Supplier
 import kotlin.math.PI
@@ -126,6 +126,21 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
         return PathPlannerAuto(pathName)
     }
 
+    fun driveToPose(pose: Pose2d): Command {
+        // Create the constraints to use during pathfinding
+        val constraints = PathConstraints(9.46, 2.0, 2 * PI, 4 * PI)
+
+        return AutoBuilder.pathfindToPose(pose, constraints, 0.0, 0.0)
+    }
+
+    fun pathfindThenFollowPath(pathName: String): Command {
+        val path = PathPlannerPath.fromPathFile(pathName)
+        // Create the constraints to use during pathfinding
+        val constraints = PathConstraints(9.46, 2.0, 2 * PI, 4 * PI)
+        // Since AutoBuilder is configured, we can use it to build pathfinding commands
+        return AutoBuilder.pathfindThenFollowPath(path, constraints, 0.0)
+    }
+
     fun rotateToAngle(angle: Double): Command {
         // this takes a lot of building up
         // first get the current angle
@@ -139,17 +154,7 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
             PathPlannerPath.bezierFromPoses(targetPose), constraints, GoalEndState(0.0, Rotation2d(angleDifference))
         )
         // follow path command
-        val followPathCommand = FollowPathCommand(
-            path,
-            { this.state.Pose },
-            { this.currentRobotChassisSpeeds },
-            { speeds -> useSpeeds(speeds) },
-            getHolonomicDriveController(),
-            getReplanningConfig(),
-            getAlliance(),
-            this
-        )
-        return followPathCommand // i hate this so much
+        return AutoBuilder.pathfindToPose(targetPose, constraints, 0.0, 0.0)
     }
 
     var constraints = PathConstraints(9.46, 2.0, 2 * PI, 4 * PI)
@@ -185,7 +190,7 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
         updateSimState(0.02, RobotController.getBatteryVoltage())
     }
 
-
+    @get:AutoLogOutput
     val currentRobotChassisSpeeds: ChassisSpeeds
         get() = m_kinematics.toChassisSpeeds(*state.ModuleStates)