11/11/25

AshOnDiscord · AshOnDiscord · commit ccf59846b2ef · 2025-11-11T13:55:00.000-05:00
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
@@ -21,8 +21,8 @@ android {
 dependencies {
     implementation project(':FtcRobotController')
 
-    implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:01bc6d6334'
-    implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:01bc6d6334'
+    implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdx:67adbc2b80'
+    implementation 'com.github.AshOnDiscord.FTC-OffSeason:cmdxpedro:67adbc2b80'
 
 //    maven local
 //    implementation('com.millburnx:cmdx:+')
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/GamepadManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/GamepadManager.kt
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/DPad.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/DPad.kt
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.common.hardware.gamepad
+
+import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d
+
+class DPad(var left: Boolean, var up: Boolean, var right: Boolean, var down: Boolean) {
+    val vector: Vec2d
+        get() {
+            val x = (if (right) 1.0 else 0.0) - (if (left) 1.0 else 0.0)
+            val y = (if (up) 1.0 else 0.0) - (if (down) 1.0 else 0.0)
+            return Vec2d(x, y)
+        }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/Gamepad.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/Gamepad.kt
@@ -0,0 +1,6 @@
+package org.firstinspires.ftc.teamcode.common.hardware.gamepad
+
+class Gamepad(private val gamepad: com.qualcomm.robotcore.hardware.Gamepad) {
+    var current: GamepadState = GamepadState(gamepad)
+    var prev: GamepadState = GamepadState(gamepad)
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadManager.kt
@@ -0,0 +1,31 @@
+package org.firstinspires.ftc.teamcode.common.hardware.gamepad
+
+import com.bylazar.gamepad.GamepadManager
+import com.bylazar.gamepad.PanelsGamepad
+import org.firstinspires.ftc.teamcode.opmode.OpMode
+
+class GamepadManager(private val opMode: OpMode) {
+    private val _panelsGamepad1: GamepadManager = PanelsGamepad.firstManager
+    private val _panelsGamepad2: GamepadManager = PanelsGamepad.secondManager
+
+    private val _gamepad1: com.qualcomm.robotcore.hardware.Gamepad
+        get() = _panelsGamepad1.asCombinedFTCGamepad(opMode.gamepad1)
+    private val _gamepad2: com.qualcomm.robotcore.hardware.Gamepad
+        get() = _panelsGamepad2.asCombinedFTCGamepad(opMode.gamepad2)
+
+    val gamepad1: Gamepad = Gamepad(_gamepad1)
+    val gamepad2: Gamepad = Gamepad(_gamepad2)
+
+    fun update() {
+        val oldState1 = gamepad1.current
+        val oldState2 = gamepad2.current
+
+        val newState1 = GamepadState(_gamepad1)
+        val newState2 = GamepadState(_gamepad2)
+        gamepad1.current = newState1
+        gamepad2.current = newState2
+
+        gamepad1.prev = oldState1
+        gamepad2.prev = oldState2
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadState.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/GamepadState.kt
@@ -0,0 +1,37 @@
+package org.firstinspires.ftc.teamcode.common.hardware.gamepad
+
+class GamepadState(private val gamepad: com.qualcomm.robotcore.hardware.Gamepad) {
+    val leftJoyStick = JoyStick(
+        gamepad.left_stick_x.toDouble(),
+        gamepad.left_stick_y.toDouble(),
+        gamepad.left_stick_button
+    )
+
+    val rightJoyStick = JoyStick(
+        gamepad.right_stick_x.toDouble(),
+        gamepad.right_stick_y.toDouble(),
+        gamepad.right_stick_button
+    )
+
+    val dPad = DPad(
+        gamepad.dpad_left,
+        gamepad.dpad_up,
+        gamepad.dpad_right,
+        gamepad.dpad_down
+    )
+
+    val a: Boolean = gamepad.a
+    val b: Boolean = gamepad.b
+    val x: Boolean = gamepad.x
+    val y: Boolean = gamepad.y
+
+    val guide: Boolean = gamepad.guide
+    val start: Boolean = gamepad.start
+    val back = gamepad.back
+
+    val leftBumper: Boolean = gamepad.left_bumper
+    val rightBumper: Boolean = gamepad.right_bumper
+
+    val leftTrigger: Double = gamepad.left_trigger.toDouble()
+    val rightTrigger: Double = gamepad.right_trigger.toDouble()
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/JoyStick.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/JoyStick.kt
@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.common.hardware.gamepad
+
+import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d
+
+class JoyStick(var x: Double, var y: Double, var down: Boolean) {
+    val vector: Vec2d
+        get() = Vec2d(x, y)
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/SlewRateLimiter.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/hardware/gamepad/SlewRateLimiter.kt
@@ -0,0 +1,26 @@
+package org.firstinspires.ftc.teamcode.common.hardware.gamepad
+
+import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d
+import com.millburnx.cmdxpedro.util.toDegrees
+import org.firstinspires.ftc.teamcode.common.normalizeDegrees
+import kotlin.math.abs
+
+object SlewRateLimiter {
+    fun limit(current: Double, target: Double, maxRate: Double): Double {
+        val rate = target - current
+        return current + rate.coerceIn(-maxRate, maxRate)
+    }
+
+    fun limit(current: Vec2d, target: Vec2d, maxMagnitudeRate: Double, maxRotationRate: Double): Vec2d {
+        // if angle diff < 90, slew angle and magnitude, if angle > thresh, focus on slewing magnitude (cross-over!)
+        val targetAngle = Vec2d().angleTo(target).toDegrees()
+        val currentAngle = Vec2d().angleTo(current).toDegrees()
+        val angleDiff = normalizeDegrees(targetAngle - currentAngle)
+
+        if (abs(angleDiff) <= 135) {
+            // TODO: Implement this stuff
+        }
+
+        return Vec2d()
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Apriltags.kt
@@ -34,6 +34,6 @@ class Apriltags : Subsystem("Apriltags") {
 }
 
 fun AprilTagDetection.toPose(currentPose: Pose2d): Pose2d {
-    val offset = Vec2d(ftcPose.range, 0).rotate(currentPose.radians + ftcPose.bearing.toRadians())
+    val offset = Vec2d(ftcPose.range).rotate(currentPose.radians + ftcPose.bearing.toRadians())
     return Pose2d(currentPose.position + offset, currentPose.heading + ftcPose.yaw)
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/AutoTargeting.kt
@@ -6,7 +6,7 @@ import com.millburnx.cmdxpedro.util.WaitFor
 import org.firstinspires.ftc.teamcode.common.roundTo
 import org.firstinspires.ftc.teamcode.opmode.OpMode
 
-class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsystem("Auto Targeting") {
+class AutoTargeting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsystem("Auto Targeting") {
 
     var enabled = false
     var target: Pose2d? = null
@@ -32,7 +32,7 @@ class AutoTargetting(opMode: OpMode, pedro: Pedro, apriltags: Apriltags) : Subsy
                 if (newLocked != pedro.isLocked) {
                     pedro.isLocked = newLocked
                     if (newLocked) {
-                        pedro.follower.turnTo(pedro.pose.position.angle(target!!.position))
+                        pedro.follower.turnTo(pedro.pose.angleTo(target!!))
                     } else {
                         pedro.follower.startTeleopDrive(true)
                     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/subsystem/Pedro.kt
@@ -5,23 +5,29 @@ import com.millburnx.cmdx.Command
 import com.millburnx.cmdxpedro.util.Pose2d
 import com.millburnx.cmdxpedro.util.WaitFor
 import com.millburnx.cmdxpedro.util.toDegrees
-import com.pedropathing.geometry.Pose
+import org.firstinspires.ftc.teamcode.common.hardware.gamepad.SlewRateLimiter
+import org.firstinspires.ftc.teamcode.common.simplifiedString
+import org.firstinspires.ftc.teamcode.common.toPedro
 import org.firstinspires.ftc.teamcode.opmode.OpMode
 import org.firstinspires.ftc.teamcode.pedro.Constants
 import kotlin.math.abs
 import kotlin.math.pow
+import kotlin.math.sign
 
 @Configurable
-class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro") {
+class Pedro(val opMode: OpMode, val startingPose: Pose2d = Pose2d(), var isTeleop: Boolean = false) : Subsystem("Pedro") {
     val follower = Constants.createFollower(opMode.hardwareMap).apply {
-        setStartingPose(Pose(0.0, 0.0, 0.0))
+        setStartingPose(startingPose.toPedro())
     }
     var isLocked = false;
 
     val pose: Pose2d
         get() = Pose2d(follower.pose.x, follower.pose.y, follower.pose.heading.toDegrees())
 
     override val run: suspend Command.() -> Unit = {
+        var prevX = 0.0
+        var prevY = 0.0
+
         with(opMode) {
             follower.update()
             if (isTeleop) follower.startTeleopDrive(true)
@@ -30,15 +36,21 @@ class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro")
                 follower.update()
                 if (isTeleop && !isLocked) {
                     // axis snapping
-                    val snappedX = snapTo(gp1.current.leftJoyStick.x, 0.0, snapThreshold)
-                    val snappedY = snapTo(gp1.current.leftJoyStick.y, 0.0, snapThreshold)
-
+                    val processedX = processInput(prevX, gp1.current.leftJoyStick.x)
+                    val processedY = processInput(prevY, gp1.current.leftJoyStick.y)
+                    val rx = gp1.current.rightJoyStick.x
                     follower.setTeleOpDrive(
-                        -snappedX.pow(3),
-                        -snappedY.pow(3),
-                        -gp1.current.rightJoyStick.x.pow(3)
+                        -processedY,
+                        -processedX,
+                        -(abs(rx.pow(2)) * sign(rx))
                     )
+                    tel.addData("px", processedX)
+                    tel.addData("py", processedY)
+
+                    prevX = processedX
+                    prevY = processedY
                 }
+                tel.addData("pose", pose.simplifiedString())
                 sync()
             }
         }
@@ -49,8 +61,25 @@ class Pedro(opMode: OpMode, var isTeleop: Boolean = false) : Subsystem("Pedro")
         return raw
     }
 
+    private fun processInput(prevValue: Double, newValue: Double): Double {
+        val prevSnapped = snapTo(prevValue, 0.0, snapThreshold)
+        val newSnapped = snapTo(newValue, 0.0, snapThreshold)
+
+        val dt = opMode.deltaTime
+        val slewedResult = SlewRateLimiter.limit(
+            prevSnapped,
+            newSnapped,
+            maxRate * dt
+        )
+
+        return abs(slewedResult.pow(2)) * sign(slewedResult)
+    }
+
     companion object {
         @JvmField
         var snapThreshold = 0.1;
+
+        @JvmField
+        var maxRate = 0.0;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/util.kt
@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.common
 
 import com.millburnx.cmdxpedro.util.Pose2d
 import com.millburnx.cmdxpedro.util.geometry.vector.Vec2d
+import com.pedropathing.geometry.Pose
 import java.util.*
 import kotlin.math.floor
 import kotlin.math.pow
@@ -21,4 +22,8 @@ fun Vec2d.simplifiedString(): String = String.format(Locale.US ,"(%.2f, %.2f)",
 fun Double.roundTo(n: Int): Double {
     val factor = 10.0.pow(n)
     return round(this * factor) / factor
+}
+
+fun Pose2d.toPedro(): Pose {
+    return Pose(this.x, this.y, this.radians)
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/common/vision/VisionManager.kt
@@ -1,7 +1,6 @@
 package org.firstinspires.ftc.teamcode.common.vision
 
 import android.util.Size
-import com.bylazar.camerastream.PanelsCameraStream
 import com.bylazar.configurables.annotations.Configurable
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName
 import org.firstinspires.ftc.teamcode.opmode.OpMode
@@ -18,17 +17,17 @@ object VisionManager {
     }
 
     fun build(opMode: OpMode): VisionPortal {
-        val panelsIntegration = PanelsIntegration()
+//        val panelsIntegration = PanelsIntegration()
         val portal = VisionPortal
             .Builder()
             .setCamera(opMode.hardwareMap["cam1"] as WebcamName)
-            .addProcessors(panelsIntegration, *processors.toTypedArray())
+            .addProcessors(*processors.toTypedArray())
             .setCameraResolution(Size(cameraSizeX, cameraSizeY))
             .setStreamFormat(VisionPortal.StreamFormat.MJPEG)
             .enableLiveView(true)
             .setAutoStopLiveView(true)
             .build()
-        PanelsCameraStream.startStream(panelsIntegration)
+//        PanelsCameraStream.startStream(panelsIntegration)
         return portal
     }
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/OpMode.kt
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Teleop.kt

Original file line number	Diff line number	Diff line change
`@@ -34,6 +34,6 @@ class Apriltags : Subsystem("Apriltags") {`
`34`	`34`	`}`
`35`	`35`
`36`	`36`	`fun AprilTagDetection.toPose(currentPose: Pose2d): Pose2d {`
`37`		`- val offset = Vec2d(ftcPose.range, 0).rotate(currentPose.radians + ftcPose.bearing.toRadians())`
	`37`	`+ val offset = Vec2d(ftcPose.range).rotate(currentPose.radians + ftcPose.bearing.toRadians())`
`38`	`38`	`return Pose2d(currentPose.position + offset, currentPose.heading + ftcPose.yaw)`
`39`	`39`	`}`