add: NextFTC Pedro Extension (#1)

* refactor: remove unit and instrumented tests from pedro module

* chore: change group id to dev.nextftc.extensions

* feat: add PedroComponent

* chore: remove unused import in build.gradle.kts

* feat: add FollowPath

* feat: add PedroComponent.gyro

* feat: add PedroDriverControlled

* feat: add Turn and TurnTo

* fix: move files from src/main to src/main/kotlin

* feat: add descriptive name to Turn and TurnTo

- name of Turn is Turn(angle)
- name of TurnTo is TurnTo(angle)

* refactor: change PedroComponent to an object

* fix: make PedroComponent a class again

* fix(pedro): fix gradle

- make displayName and description correct
- use versions.pedro property for version

* refactor: rename Turn to TurnBy

* fix: rename Turn to TurnBy in the command name

---------

Co-authored-by: Zach Harel <zach@zharel.me>

Author: BeepBot99

.idea/misc.xml

@@ -12,7 +12,6 @@ plugins {
 }
 
 allprojects {
-    version = property("version") as String
     group = "dev.nextftc.extensions"
 }
 

build.gradle.kts

@@ -6,4 +6,5 @@ automaticMavenCentralSync=true
 
 android.useAndroidX=true
 
-versions.roadrunner=0.1.25-1-local.3
+versions.roadrunner=1.0.0
+versions.pedro=1.0.0

gradle.properties

@@ -30,15 +30,13 @@ dependencies {
     implementation(libs.bundles.nextftc)
     implementation(libs.pedro)
     compileOnly(libs.bundles.ftc)
-
-    testImplementation(libs.bundles.kotest)
-    testImplementation(libs.mockk)
 }
 
+version = property("versions.pedro") as String
 description =
-    "The hardware library for NextFTC, a user-friendly library for FTC. Includes hardware interfaces, wrapper implementations, and hardware commands."
+    "NextFTC's extension to add support for Pedro Pathing."
 
 nextFTCPublishing {
-    displayName = "NextFTC Hardware"
+    displayName = "NextFTC Extensions - Pedro"
     logoPath = "../assets/logo-icon.svg"
 }

pedro/build.gradle.kts

@@ -0,0 +1,37 @@
+package dev.nextftc.extensions.pedro
+
+import com.pedropathing.paths.Path
+import com.pedropathing.paths.PathChain
+import dev.nextftc.core.commands.Command
+import dev.nextftc.extensions.pedro.PedroComponent.Companion.follower
+
+class FollowPath @JvmOverloads constructor(
+    private val path: PathChain,
+    private val holdEnd: Boolean? = null,
+    private val maxPower: Double? = null
+) : Command() {
+
+    init {
+        require(maxPower == null || holdEnd != null) { "If maxPower is passed, holdEnd must be passed as well." }
+    }
+
+    @JvmOverloads
+    constructor(path: Path, holdEnd: Boolean? = null, maxPower: Double? = null) : this(
+        PathChain(path),
+        holdEnd,
+        maxPower
+    )
+
+    override val isDone: Boolean
+        get() = !follower.isBusy
+
+    override fun start() {
+        if (holdEnd !== null && maxPower !== null) follower.followPath(path, maxPower, holdEnd)
+        else if (holdEnd !== null) follower.followPath(path, holdEnd)
+        else follower.followPath(path)
+    }
+
+    override fun stop(interrupted: Boolean) {
+        if (interrupted) follower.breakFollowing()
+    }
+}

pedro/src/androidTest/java/dev/nextftc/extensions/ExampleInstrumentedTest.kt

@@ -0,0 +1,37 @@
+package dev.nextftc.extensions.pedro
+
+import com.pedropathing.follower.Follower
+import com.qualcomm.robotcore.hardware.HardwareMap
+import dev.nextftc.core.components.Component
+import dev.nextftc.core.units.Angle
+import dev.nextftc.core.units.rad
+import dev.nextftc.ftc.ActiveOpMode
+import java.util.function.Supplier
+
+class PedroComponent(private val followerFactory: (HardwareMap) -> Follower) : Component {
+
+    override fun preInit() {
+        _follower = followerFactory(ActiveOpMode.hardwareMap)
+    }
+
+    override fun preWaitForStart() = follower.update()
+    override fun preUpdate() = follower.update()
+
+    override fun postStop() {
+        _follower = null
+    }
+
+    companion object {
+        private var _follower: Follower? = null
+
+        @get:JvmName("follower")
+        @JvmStatic
+        val follower: Follower
+            get() = _follower ?: error("Follower not initialized! Make sure you added PedroComponent to your OpMode.")
+
+        @get:JvmName("gyro")
+        @JvmStatic
+        val gyro: Supplier<Angle> = Supplier { follower.totalHeading.rad.normalized }
+
+    }
+}

pedro/src/main/kotlin/dev/nextftc/extensions/pedro/FollowPath.kt

@@ -0,0 +1,26 @@
+package dev.nextftc.extensions.pedro
+
+import dev.nextftc.extensions.pedro.PedroComponent.Companion.follower
+import dev.nextftc.hardware.driving.DriverControlledCommand
+import java.util.function.Supplier
+
+class PedroDriverControlled @JvmOverloads constructor(
+    drivePower: Supplier<Double>,
+    strafePower: Supplier<Double>,
+    turnPower: Supplier<Double>,
+    private val robotCentric: Boolean = true
+) : DriverControlledCommand(drivePower, strafePower, turnPower) {
+
+    override fun start() {
+        follower.startTeleopDrive()
+    }
+
+    override fun calculateAndSetPowers(powers: DoubleArray) {
+        val (drive, strafe, turn) = powers
+        follower.setTeleOpDrive(drive, strafe, turn, robotCentric)
+    }
+
+    override fun stop(interrupted: Boolean) {
+        if (interrupted) follower.breakFollowing()
+    }
+}

pedro/src/main/kotlin/dev/nextftc/extensions/pedro/PedroComponent.kt

@@ -0,0 +1,20 @@
+package dev.nextftc.extensions.pedro
+
+import dev.nextftc.core.commands.Command
+import dev.nextftc.core.units.Angle
+import dev.nextftc.core.units.abs
+import dev.nextftc.extensions.pedro.PedroComponent.Companion.follower
+
+class TurnBy(private val angle: Angle) : Command() {
+
+    init {
+        named("TurnBy($angle)")
+    }
+
+    override val isDone: Boolean
+        get() = !follower.isTurning
+
+    override fun start() {
+        follower.turn(abs(angle).inRad, angle.sign > 0)
+    }
+}

pedro/src/main/kotlin/dev/nextftc/extensions/pedro/PedroDriverControlled.kt

@@ -0,0 +1,19 @@
+package dev.nextftc.extensions.pedro
+
+import dev.nextftc.core.commands.Command
+import dev.nextftc.core.units.Angle
+import dev.nextftc.extensions.pedro.PedroComponent.Companion.follower
+
+class TurnTo(private val angle: Angle) : Command() {
+
+    init {
+        named("TurnTo($angle)")
+    }
+
+    override val isDone: Boolean
+        get() = !follower.isTurning
+
+    override fun start() {
+        follower.turnTo(angle.inRad)
+    }
+}