Add Blaze to NextFTC

gradle/libs.versions.toml

@@ -23,6 +23,7 @@ ftc-common = { group = "org.firstinspires.ftc", name = "FtcCommon", version.ref
 sloth = { module = "dev.frozenmilk.sinister:Sloth", version.ref = "sloth" }
 kotlin-reflect = { module = "org.jetbrains.kotlin:kotlin-reflect", version.ref = "kotlin" }
 ivy = { module = "com.pedropathing.ivy:core", version.ref = "ivy" }
+blaze = { module = "dev.anygeneric:blazeftc", version = "0.1.62"}
 
 [bundles]
 kotest = ["kotest-runner", "kotest-assertations", "kotest-property", "kotest-data-driven"]

hardware/build.gradle.kts

@@ -23,6 +23,7 @@ dependencies {
   api(libs.functional.interfaces)
   compileOnly(libs.bundles.ftc)
   implementation(libs.sloth)
+  compileOnly(libs.blaze)
 
   testImplementation(libs.bundles.kotest)
   testImplementation(libs.mockk)

hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt

@@ -114,6 +114,12 @@ object RobotController : OnCreateEventLoop {
   val auxiliaryVoltage: Voltage
     get() = controlHub.auxiliaryVoltage
 
+  /**
+   * Blaze has been enabled
+   */
+  @JvmStatic
+  var blazeEnabled: Boolean = false
+
   /**
    * Lifecycle callback used to initialize runtime context for hardware access.
    *

hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt

@@ -8,10 +8,13 @@
 
 package dev.nextftc.hardware.actuators
 
+import com.qualcomm.hardware.lynx.LynxController
+import com.qualcomm.hardware.lynx.LynxModule
 import com.qualcomm.robotcore.hardware.DcMotor
 import com.qualcomm.robotcore.hardware.DcMotorImplEx
 import com.qualcomm.robotcore.hardware.DcMotorSimple
 import com.qualcomm.robotcore.hardware.Servo
+import dev.anygeneric.blazeftc.BlazeFTC
 import dev.nextftc.control.feedback.PIDCoefficients
 import dev.nextftc.control.feedback.PIDController
 import dev.nextftc.control.feedforward.SimpleFFCoefficients
@@ -155,6 +158,17 @@ class NextMotor @JvmOverloads constructor(
   var controlType: ControlType = ControlType.Throttle(0.0)
     private set
 
+  private var hubId: Int = -1
+    get() = if (field == -1) {
+      val lcField = LynxController::class.java.getField("module")
+      lcField.isAccessible = true
+      val module = lcField.get(motor.controller) as LynxModule
+      hubId = module.moduleAddress
+      field
+    } else {
+      field
+    }
+
   /**
    * Raw motor power (throttle) in the range [-1.0, 1.0].
    *
@@ -163,7 +177,12 @@ class NextMotor @JvmOverloads constructor(
    */
   private var power by Caching(cacheTolerance) {
     if (it != null) {
-      motor.power = it
+      if (RobotController.blazeEnabled) {
+        val port = this.motor.portNumber
+        BlazeFTC.setMotorPower(hubId, port, it)
+      } else {
+        motor.power = it
+      }
     }
   }
 

robot/build.gradle.kts

@@ -21,6 +21,7 @@ dependencies {
   compileOnly(libs.bundles.ftc)
   implementation(libs.sloth)
   implementation(libs.kotlin.reflect)
+  compileOnly(libs.blaze)
 
   testImplementation(libs.bundles.kotest)
   testImplementation(kotlin("test"))

robot/src/main/kotlin/dev/nextftc/robot/opmode/NextFTCOpModeScanner.kt

@@ -44,13 +44,16 @@ object NextFTCOpModeScanner : OpModeScanner() {
       RobotLog.info("Skipping disabled NextFTC OpMode class: $kcls")
       return
     }
+    val enableBlaze = BlazeOpMode::class.java.isAssignableFrom(cls)
 
     when (val metaResult = opModeMetaFromClass(kcls)) {
       is OpModeMetaCheckResult.FoundAnnotation -> {
         when (val constructorResult = opModeConstructorFromClass(kcls)) {
           is OpModeConstructorCheckResult.FoundConstructor -> {
             RobotLog.info("Found NextFTC OpMode class: $cls")
-            registrationHelper.register(metaResult.meta) { BoundNextOpMode(constructorResult.constructor) }
+            registrationHelper.register(metaResult.meta) {
+              BoundNextOpMode(constructorResult.constructor, enableBlaze)
+            }
           }
           is OpModeConstructorCheckResult.NoConstructorFound -> {
             RobotLog.Global.addWarning(

robot/src/main/kotlin/dev/nextftc/robot/opmode/NextOpMode.kt

@@ -3,6 +3,7 @@ package dev.nextftc.robot.opmode
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
 import com.qualcomm.robotcore.hardware.Gamepad
 import com.qualcomm.robotcore.hardware.HardwareMap
+import dev.nextftc.hardware.RobotController
 import dev.nextftc.robot.NextFTCException
 import dev.nextftc.robot.NextRobot
 import dev.nextftc.robot.RobotLog
@@ -70,19 +71,42 @@ abstract class NextOpMode internal constructor(internal val hooks: MutableList<O
   }
 }
 
-internal class BoundNextOpMode(val opModeConstructor: () -> NextOpMode) : LinearOpMode() {
+/**
+ * Base class for NextFTC OpModes that run with Blaze enabled.
+ *
+ * Extending this class instead of [NextOpMode] routes motor writes through Blaze for the whole
+ * OpMode and exposes callbacks for data Blaze provides.
+ *
+ * @param robot The automatically resolved [dev.nextftc.robot.NextRobot] instance.
+ * @param hooks Additional custom hooks to execute during the OpMode lifecycle.
+ */
+abstract class BlazeOpMode(robot: NextRobot, vararg hooks: OpModeHook) : NextOpMode(robot, *hooks) {
+  /** Run when Blaze provides new Control Hub Bulk Data. Requires a [BlazeBulkReadHook]. */
+  open fun onControlHubBulkData() {}
+
+  /** Run when Blaze provides new Expansion Hub Bulk Data. Requires a [BlazeBulkReadHook]. */
+  open fun onExpansionHubBulkData() {}
+
+  /** Run when Blaze provides new localization data */
+  open fun onLocalizationData() {}
+}
+
+internal class BoundNextOpMode(val opModeConstructor: () -> NextOpMode, val blaze: Boolean = false) :
+  LinearOpMode() {
   override fun runOpMode() {
     NextOpMode.activeGamepad1 = this.gamepad1
     NextOpMode.activeGamepad2 = this.gamepad2
     NextOpMode.activeTelemetry = this.telemetry
     NextOpMode.activeHardwareMap = this.hardwareMap
 
+    if (blaze) RobotController.blazeEnabled = true
+
     var opMode: NextOpMode? = null
 
     try {
       opMode = opModeConstructor()
-
-      opMode.hooks.forEach(OpModeHook::afterConstruction)
+      if (blaze) opMode.hooks.add(0, BlazeHook)
+      opMode.hooks.forEach { it.afterConstruction(opMode) }
       while (opModeInInit()) {
         opMode.hooks.forEach(OpModeHook::beforeDisabled)
         opMode.disabledPeriodic()
@@ -101,15 +125,18 @@ internal class BoundNextOpMode(val opModeConstructor: () -> NextOpMode) : Linear
       opMode.end()
     } finally {
       // even if the OpMode didn't finish execution we still want to clean up the hooks
-      // and clear the static references to the SDK objects
-      opMode?.hooks?.forEach { hook ->
+      // and clear the static references to the SDK objects.
+      // hooks are torn down in reverse so hooks set up first are torn down last
+      opMode?.hooks?.asReversed()?.forEach { hook ->
         try {
           hook.afterEnd()
         } catch (throwable: Throwable) {
           RobotLog.warn("${hook::class.displayName} threw while cleaning up after the OpMode.", throwable)
         }
       }
 
+      RobotController.blazeEnabled = false
+
       NextOpMode.activeGamepad1 = null
       NextOpMode.activeGamepad2 = null
       NextOpMode.activeTelemetry = null

robot/src/main/kotlin/dev/nextftc/robot/opmode/OpModeHook.kt

@@ -10,8 +10,12 @@ package dev.nextftc.robot.opmode
 
 import com.pedropathing.ivy.Scheduler
 import com.qualcomm.hardware.lynx.LynxModule
+import dev.anygeneric.blazeftc.BlazeDummyPlug
+import dev.anygeneric.blazeftc.BlazeFTC
+import dev.anygeneric.blazeftc.Hub
 import dev.nextftc.hardware.RobotController
 import dev.nextftc.hardware.actuators.NextMotor
+import dev.nextftc.hardware.lynx.NextLynxModule
 import dev.nextftc.robot.Mechanism
 import dev.nextftc.robot.NextRobot
 import dev.nextftc.robot.Telemetry
@@ -26,7 +30,7 @@ import dev.nextftc.robot.triggers.Trigger
 interface OpModeHook {
 
   /** Called immediately after the OpMode's onInit phase. */
-  fun afterConstruction() {}
+  fun afterConstruction(opMode: NextOpMode) {}
 
   /** Called immediately before the OpMode's disabledPeriodic (init_loop) phase. */
   fun beforeDisabled() {}
@@ -136,3 +140,52 @@ object BulkReadHook : OpModeHook {
     lynxHubs.forEach { it.clearBulkCache() }
   }
 }
+
+/**
+ * Internal hook responsible for the Blaze lifecycle. Added to the front of the hook list by
+ * [BoundNextOpMode] for [BlazeOpMode]s, so it is set up before and torn
+ * down after every other hook.
+ */
+internal object BlazeHook : OpModeHook {
+  override fun afterConstruction(opMode: NextOpMode) {
+    BlazeDummyPlug.initializeBlazeFTC(RobotController.hardwareMap)
+  }
+
+  override fun beforeStart() {
+    BlazeFTC.run(0)
+  }
+
+  override fun afterEnd() {
+    BlazeDummyPlug.closeBlazeFTC()
+    RobotController.blazeEnabled = false
+  }
+}
+
+/**
+ * Hook to enable Blaze control of Bulk Reads on a single hub. Do not run this with BulkReadHook.
+ * fastMode controls the level of concurrency. false will run at ~500 hz and is very stable.
+ * true is stable but if you enable it and then do too many writes, it could become unstable.
+ * Do not put two hooks on the same hub. This hook does nothing if the OpMode isn't a [BlazeOpMode].
+ */
+class BlazeBulkReadHook(val hub: NextLynxModule.Type, val fastMode: Boolean) : OpModeHook {
+  override fun afterConstruction(opMode: NextOpMode) {
+    if (opMode !is BlazeOpMode) {
+      return
+    }
+    BlazeFTC.load()
+    val hw = RobotController.hardwareMap
+    val hub = if (hub == NextLynxModule.Type.CONTROL_HUB) {
+      Hub.CtrlHub
+    } else {
+      Hub.ExHub
+    }
+    val packets = if (fastMode) 2 else 1
+    BlazeDummyPlug.engageBulkReadAcceleration(hw, hub, packets) {
+      if (hub == Hub.CtrlHub) {
+        opMode.onControlHubBulkData()
+      } else {
+        opMode.onExpansionHubBulkData()
+      }
+    }
+  }
+}

settings.gradle.kts

@@ -21,6 +21,7 @@ dependencyResolutionManagement {
     google()
     maven("https://repo.dairy.foundation/releases")
     maven("https://repo.dairy.foundation/snapshots")
+    maven("https://maven.anygeneric.dev/")
     mavenLocal()
   }
 }