diff --git a/gradle/libs.versions.toml b/gradle/libs.versions.toml index 69fb04b5..6280a4e9 100644 --- a/gradle/libs.versions.toml +++ b/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"] diff --git a/hardware/build.gradle.kts b/hardware/build.gradle.kts index 25c8e860..9f79b175 100644 --- a/hardware/build.gradle.kts +++ b/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) diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt index 8a278d67..2b6ed20f 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt +++ b/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. * diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt index 575303a2..4448c6f0 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt +++ b/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 + } } } diff --git a/robot/build.gradle.kts b/robot/build.gradle.kts index d3e7122c..db808f92 100644 --- a/robot/build.gradle.kts +++ b/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")) diff --git a/robot/src/main/kotlin/dev/nextftc/robot/opmode/NextFTCOpModeScanner.kt b/robot/src/main/kotlin/dev/nextftc/robot/opmode/NextFTCOpModeScanner.kt index d31f6616..4f9fdf6b 100644 --- a/robot/src/main/kotlin/dev/nextftc/robot/opmode/NextFTCOpModeScanner.kt +++ b/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( diff --git a/robot/src/main/kotlin/dev/nextftc/robot/opmode/NextOpMode.kt b/robot/src/main/kotlin/dev/nextftc/robot/opmode/NextOpMode.kt index 77d7cc99..2740af68 100644 --- a/robot/src/main/kotlin/dev/nextftc/robot/opmode/NextOpMode.kt +++ b/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 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,8 +125,9 @@ 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) { @@ -110,6 +135,8 @@ internal class BoundNextOpMode(val opModeConstructor: () -> NextOpMode) : Linear } } + RobotController.blazeEnabled = false + NextOpMode.activeGamepad1 = null NextOpMode.activeGamepad2 = null NextOpMode.activeTelemetry = null diff --git a/robot/src/main/kotlin/dev/nextftc/robot/opmode/OpModeHook.kt b/robot/src/main/kotlin/dev/nextftc/robot/opmode/OpModeHook.kt index ed471085..aca223aa 100644 --- a/robot/src/main/kotlin/dev/nextftc/robot/opmode/OpModeHook.kt +++ b/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() + } + } + } +} diff --git a/settings.gradle.kts b/settings.gradle.kts index 5174cdfd..4f411ec8 100644 --- a/settings.gradle.kts +++ b/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() } }