03/12/26 - 00 26

Author: AshOnDiscord

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/AutonManager.kt

@@ -12,11 +12,11 @@ import com.pedropathing.paths.PathBuilder
 import org.firstinspires.ftc.teamcode.common.subsystem.Drive
 import org.firstinspires.ftc.teamcode.opmode.OpMode
 
-// TODO: create the pedro subsystem and pass it so we can automatically the starting pose
-//  (find a way to do it cleanly since this is created after it)
-//  i think we let auton manager create pedro, drive can have pedro passed in. if not it'll create itself
-class AutonManager(val opMode: OpMode, val drive: Drive, val sequenceName: String, val isMirrored: Boolean = false) {
+open class BaseAutonManager(val opMode: OpMode, val drive: Drive, val isMirrored: Boolean = false) {
     val builder: PedroBuilder = PedroBuilder(isMirrored)
+}
+
+class AutonManager(opMode: OpMode, drive: Drive, val sequenceName: String, isMirrored: Boolean = false): BaseAutonManager(opMode, drive, isMirrored) {
     val loadedSequence: Sequence = PedroLoader.load("Paths/$sequenceName.pp")
     val loadedPath: List<Pair<com.millburnx.cmdxpedro.paths.path.Path, HeadingInterpolation>>
         get() = PedroLoader.sequenceToPath(loadedSequence)

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/auton/Close18BallAutonBezier.kt

@@ -0,0 +1,72 @@
+package org.firstinspires.ftc.teamcode.opmode.test.pedro
+
+import com.bylazar.configurables.annotations.Configurable
+import com.millburnx.cmdx.Command
+import com.millburnx.cmdx.commandGroups.Sequential
+import com.millburnx.cmdxpedro.paths.heading.LinearHeading
+import com.millburnx.cmdxpedro.paths.path.Line
+import com.millburnx.util.vector.Vec2d
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp
+import org.firstinspires.ftc.teamcode.common.subsystem.TeleOpDrive
+import org.firstinspires.ftc.teamcode.opmode.OpMode
+import org.firstinspires.ftc.teamcode.opmode.auton.BaseAutonManager
+
+@Configurable
+@TeleOp
+class LineTest : OpMode() {
+    override fun run() {
+        val drive = TeleOpDrive(this, true)
+
+        val autonManager = BaseAutonManager(this, drive, false)
+
+        val forward = {
+            autonManager.builder.PathCommand(
+                drive.follower, Line(
+                    Vec2d(0.0, 0.0),
+                    Vec2d(distance, 0.0),
+                ),
+                LinearHeading(0.0, 0.0),
+                { !isStopRequested }
+            )
+        }
+        val backward = {
+            autonManager.builder.PathCommand(
+                drive.follower, Line(
+                    Vec2d(distance, 0.0),
+                    Vec2d(0.0, 0.0),
+                ),
+                LinearHeading(0.0, 0.0),
+                { !isStopRequested }
+            )
+        }
+
+        var isBusy = false
+
+        scheduler.schedule(
+            Command {
+                while (!isStopRequested) {
+                    if (!gp1.prev.rightBumper && gp1.current.rightBumper) {
+                        isRunning = !isRunning
+                    }
+                    if (!isBusy && isRunning) {
+                        isBusy = true
+                        scheduler.schedule(Sequential {
+                            +forward()
+                            +backward()
+                            Command { isBusy = false }
+                        })
+                    }
+                    sync()
+                }
+            }
+        )
+    }
+
+    companion object {
+        @JvmField
+        var distance = 48.0
+
+        @JvmField
+        var isRunning = false
+    }
+}