Merge branch 'pathing-refactor'

# Conflicts:
#	TeamCode/src/main/kotlin/pioneer/decode/GoalTag.kt

Author: Braedeng0

TeamCode/src/main/kotlin/pioneer/Bot.kt

@@ -56,7 +56,7 @@ class Bot private constructor(
     val follower: Follower by lazy {
         Follower(
             localizer = pinpoint!!,
-            mecanumBase = mecanumBase!!,
+            drive = mecanumBase!!,
         )
     }
 

TeamCode/src/main/kotlin/pioneer/Constants.kt

@@ -62,8 +62,7 @@ object Constants {
 
         // limits
         const val MAX_MOTOR_VELOCITY_TPS = 2500.0
-        const val MAX_FWD_VEL_CMPS = 150.0
-        const val MAX_STRAFE_VEL_CMPS = 125.0
+        const val MAX_DRIVE_VEL_CMPS = 150.0
         const val DEFAULT_POWER = 0.7
 
         // Feedforward gains using Pose(x,y,theta)
@@ -75,8 +74,8 @@ object Constants {
 //        @JvmField var kAT = 0.0
 
         val kV = Pose(x = 0.0063, y = 0.0055, theta = -0.147)
-        val kA = Pose(x = 0.0025, y = 0.001, theta = 0.0)
-        val kS = Pose(x = 0.0, y = 0.0, theta = 0.0)
+        val kA = Pose(x = 0.00125, y = 0.001, theta = 0.0)
+        val kS = Pose(x = 0.11, y = 0.06, theta = 0.0)
 
         val MOTOR_CONFIG =
             mapOf(
@@ -104,16 +103,16 @@ object Constants {
     @Config
     object Follower {
         /** The threshold in cm to consider the target reached. */
-        const val POSITION_THRESHOLD = 0.5
+        const val POSITION_THRESHOLD = 1.0
 
         /** The threshold in radians to consider the target heading reached. */
         const val ROTATION_THRESHOLD = 0.05
 
         /** The maximum drive velocity in cm per second. */
-        const val MAX_DRIVE_VELOCITY = 100.0
+        const val MAX_DRIVE_VELOCITY = 110.0
 
         /** The maximum drive acceleration in cm per second squared. */
-        const val MAX_DRIVE_ACCELERATION = 50.0
+        const val MAX_DRIVE_ACCELERATION = 30.0
 
         /** The maximum centripetal acceleration that the robot can handle in cm/s^2. */
         const val MAX_CENTRIPETAL_ACCELERATION = (70.0 * 70.0) / 25.0
@@ -125,24 +124,18 @@ object Constants {
         const val MAX_ANGULAR_ACCELERATION = 10.0
 
         // X-axis PID coefficients for the trajectory follower
-        @JvmField var X_KP = 10.0
-
+        @JvmField var X_KP = 7.5
         @JvmField var X_KI = 0.0
-
         @JvmField var X_KD = 0.0
 
         // Y-axis PID coefficients for the trajectory follower
-        @JvmField var Y_KP = 10.0
-
+        @JvmField var Y_KP = 7.5
         @JvmField var Y_KI = 0.0
-
         @JvmField var Y_KD = 0.0
 
         // Theta PID coefficients for heading interpolation
-        @JvmField var THETA_KP = 0.5
-
+        @JvmField var THETA_KP = 5.0
         @JvmField var THETA_KI = 0.0
-
         @JvmField var THETA_KD = 0.0
     }
 

TeamCode/src/main/kotlin/pioneer/decode/GoalTag.kt

@@ -62,7 +62,7 @@ enum class GoalTag(
  * @property tagId The ID of the goal AprilTag to process.
  */
 object GoalTagProcessor {
-    private val validTags = GoalTag.values().map { it.id }.toSet()
+    private val validTags = GoalTag.entries.map { it.id }.toSet()
 
     fun isValidGoalTag(aprilTagId: Int): Boolean = aprilTagId in validTags
 
@@ -97,7 +97,7 @@ object GoalTagProcessor {
     fun getRobotFieldPose(detections: List<AprilTagDetection>): Pose? {
         val tag =
             detections.firstNotNullOfOrNull { detection ->
-                GoalTag.values().firstOrNull { it.id == detection.id }
+                GoalTag.entries.firstOrNull { it.id == detection.id }
             }
 
         return tag?.pose?.let { tagPose ->
@@ -106,8 +106,8 @@ object GoalTagProcessor {
                     GoalTag.BLUE.id, GoalTag.RED.id -> {
                         detection.ftcPose?.let { ftcPose ->
                             Pose(
-                                x = tagPose.x + MathUtils.inToCM(ftcPose.x.toDouble()),
-                                y = tagPose.y + MathUtils.inToCM(ftcPose.y.toDouble()),
+                                x = tagPose.x + MathUtils.inToCM(ftcPose.x),
+                                y = tagPose.y + MathUtils.inToCM(ftcPose.y),
                                 theta = tagPose.theta + ftcPose.bearing,
                             )
                         }

TeamCode/src/main/kotlin/pioneer/decode/Motif.kt

@@ -16,7 +16,6 @@ class Motif(
     // Returns the next artifact in the motif's sequence, cycling back to the start if needed
     fun getNextArtifact(): Artifact? {
         if (artifacts.isEmpty()) return null
-
         val artifact = artifacts[currentIndex]
         currentIndex = (currentIndex + 1) % artifacts.size
         return artifact

TeamCode/src/main/kotlin/pioneer/decode/Points.kt

@@ -31,28 +31,30 @@ class Points(
     fun Pose.T(c: AllianceColor): Pose =
         when (c) {
             AllianceColor.RED -> this
-            AllianceColor.BLUE -> Pose(-this.x, this.y, -this.theta)
+            AllianceColor.BLUE -> Pose(-this.x, this.y, theta=-this.theta)
             AllianceColor.NEUTRAL -> this
         }
 
     // Key positions on the field
     // Written in ACTION_POSITION format
-    val START_GOAL = Pose(130.0, 137.0, theta = 3 * PI / 4).T(color)
+    val START_GOAL = Pose(133.0, 134.0, theta = 0.67).T(color)
     val START_FAR = Pose(43.0, -157.0, theta = 0.0).T(color)
 
-    val SHOOT_GOAL_CLOSE = Pose(60.0, 60.0, theta = 0.0).T(color)
+    val SHOOT_GOAL_CLOSE = Pose(55.0, 30.0, theta = -PI/2).T(color)
     val SHOOT_GOAL_FAR = Pose(43.0, -145.0, theta = 0.0).T(color)
 
+    val LEAVE_POSITION = Pose(55.0, 120.0).T(color)
+
     private fun collectY(i: Int) = 30 - (i * 60.0) // 30, -30, -90
 
-    private val prepCollectX = 70.0
+    private val prepCollectX = 75.0
     private val collectTheta = -PI / 2 // Point to right
-    val PREP_COLLECT_GOAL = Pose(prepCollectX, collectY(0), collectTheta).T(color)
-    val PREP_COLLECT_MID = Pose(prepCollectX, collectY(1), collectTheta).T(color)
-    val PREP_COLLECT_AUDIENCE = Pose(prepCollectX, collectY(2), collectTheta).T(color)
-
-    private val collectX = 130.0
-    val COLLECT_GOAL = Pose(collectX, collectY(0), collectTheta).T(color)
-    val COLLECT_MID = Pose(collectX, collectY(1), collectTheta).T(color)
-    val COLLECT_AUDIENCE = Pose(collectX, collectY(2), collectTheta).T(color)
+    val PREP_COLLECT_GOAL = Pose(prepCollectX, collectY(0), theta=collectTheta).T(color)
+    val PREP_COLLECT_MID = Pose(prepCollectX, collectY(1), theta=collectTheta).T(color)
+    val PREP_COLLECT_AUDIENCE = Pose(prepCollectX, collectY(2), theta=collectTheta).T(color)
+
+    private val collectX = 120.0
+    val COLLECT_GOAL = Pose(collectX, collectY(0), theta=collectTheta).T(color)
+    val COLLECT_MID = Pose(collectX, collectY(1), theta=collectTheta).T(color)
+    val COLLECT_AUDIENCE = Pose(collectX, collectY(2), theta=collectTheta).T(color)
 }

TeamCode/src/main/kotlin/pioneer/hardware/Launcher.kt

@@ -13,8 +13,8 @@ class Launcher(
     private val servoName: String = Constants.HardwareNames.LAUNCH_SERVO,
 ) : HardwareComponent {
     companion object {
-        private const val SERVO_CYCLE_TIME_MS = 670L
-        private const val RESET_THRESHOLD_MS = 750.0
+        private const val SERVO_CYCLE_TIME_MS = 250L
+        private const val RESET_THRESHOLD_MS = 100.0
     }
 
     private lateinit var launchServo: Servo
@@ -23,14 +23,15 @@ class Launcher(
     private val isTriggered = AtomicBoolean(false)
 
     val isReset: Boolean
-        get() = resetTimer.milliseconds() > RESET_THRESHOLD_MS
+        get() = !isTriggered.get() && resetTimer.milliseconds() > RESET_THRESHOLD_MS
 
     override fun init() {
         launchServo = hardwareMap.get(Servo::class.java, servoName)
         launchServo.position = Constants.ServoPositions.LAUNCHER_REST
     }
 
     fun triggerLaunch() {
+        if (!isReset) return
         if (!isTriggered.compareAndSet(false, true)) return
 
         launchServo.position = Constants.ServoPositions.LAUNCHER_TRIGGERED

TeamCode/src/main/kotlin/pioneer/hardware/MecanumBase.kt

@@ -6,15 +6,23 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple
 import com.qualcomm.robotcore.hardware.HardwareMap
 import pioneer.Constants
 import pioneer.helpers.Pose
+import pioneer.pathing.follower.RobotFeedforward
 import kotlin.math.abs
-import kotlin.math.sign
 
 class MecanumBase(
     private val hardwareMap: HardwareMap,
     private val motorConfig: Map<String, DcMotorSimple.Direction> = Constants.Drive.MOTOR_CONFIG,
 ) : HardwareComponent {
     private lateinit var motors: Map<String, DcMotorEx>
 
+    private val feedforward = RobotFeedforward(
+        Constants.Drive.kV.x, Constants.Drive.kA.x,
+        Constants.Drive.kV.y, Constants.Drive.kA.y,
+        Constants.Drive.kV.theta, Constants.Drive.kA.theta,
+        Constants.Drive.kS.x, Constants.Drive.kS.y,
+        Constants.Drive.kS.theta,
+    )
+
     override fun init() {
         motors =
             motorConfig.mapValues { (name, direction) ->
@@ -61,14 +69,9 @@ class MecanumBase(
      * Feedforward control for motion profiling
      */
     fun setDriveVA(pose: Pose) {
-        val ffX = calculateFeedforward(pose.vx, pose.ax, Constants.Drive.kV.x, Constants.Drive.kA.x, Constants.Drive.kS.x)
-        val ffY = calculateFeedforward(pose.vy, pose.ay, Constants.Drive.kV.y, Constants.Drive.kA.y, Constants.Drive.kS.y)
-        val ffTheta =
-            calculateFeedforward(pose.omega, pose.alpha, Constants.Drive.kV.theta, Constants.Drive.kA.theta, Constants.Drive.kS.theta)
-
-//        val ffX = calculateFeedforward(pose.vx, pose.ax, Constants.Drive.kV.x, Constants.Drive.kAX, Constants.Drive.kS.x)
-//        val ffY = calculateFeedforward(pose.vy, pose.ay, Constants.Drive.kV.y, Constants.Drive.kAY, Constants.Drive.kS.y)
-//        val ffTheta = calculateFeedforward(pose.omega, pose.alpha, Constants.Drive.kV.theta, Constants.Drive.kAT, Constants.Drive.kS.theta)
+        val ffX = feedforward.calculateX(pose.vx, pose.ax)
+        val ffY = feedforward.calculateY(pose.vy, pose.ay)
+        val ffTheta = feedforward.calculateTheta(pose.omega, pose.alpha)
 
         val motorPowers = calculateMotorPowers(Pose(vx = ffX, vy = ffY, omega = ffTheta))
         motors.values.forEachIndexed { index, motor ->
@@ -84,14 +87,6 @@ class MecanumBase(
         return listOf(leftFrontPower, leftBackPower, rightFrontPower, rightBackPower)
     }
 
-    private fun calculateFeedforward(
-        v: Double,
-        a: Double,
-        kV: Double,
-        kA: Double,
-        kS: Double,
-    ): Double = v * kV + a * kA + if (abs(v) > 1e-3) kS * sign(v) else 0.0
-
     fun stop() {
         motors.values.forEach { it.power = 0.0 }
     }

TeamCode/src/main/kotlin/pioneer/hardware/Spindexer.kt

@@ -72,6 +72,36 @@ class Spindexer(
                     MotorPosition.OUTTAKE_3,
                 )
 
+    val closestMotorState: MotorPosition
+        get() {
+            var closestPosition = MotorPosition.INTAKE_1
+            var smallestError = Double.MAX_VALUE
+
+            for (position in MotorPosition.entries) {
+                val targetTicks = (position.radians * ticksPerRadian).toInt()
+                val error = wrapTicks(targetTicks - currentMotorPosition).toDouble()
+                if (abs(error) < abs(smallestError)) {
+                    smallestError = error
+                    closestPosition = position
+                }
+            }
+
+            return closestPosition
+        }
+
+    val reachedOuttakePosition: Boolean
+        get() {
+            // Check if motor position matches any outtake position
+            for (position in outtakePositions) {
+                val targetTicks = (position.radians * ticksPerRadian).toInt()
+                val error = wrapTicks(targetTicks - currentMotorPosition)
+                if (abs(error) < 300.0) {
+                    return true
+                }
+            }
+            return false
+        }
+
     val isIntakePosition: Boolean
         get() =
             motorState in
@@ -225,6 +255,14 @@ class Spindexer(
         if (isEmpty) moveToNextOpenIntake()
         return artifact
     }
+    
+    fun popArtifact(index: Int): Artifact? {
+        val artifact = _artifacts[index]
+        _artifacts[index] = null
+        // Automatically move to intake if empty
+        if (isEmpty) moveToNextOpenIntake()
+        return artifact
+    }
 
     fun cancelLastIntake() {
         artifacts[lastStoredIndex] = null

TeamCode/src/main/kotlin/pioneer/helpers/Feedforward.kt

@@ -0,0 +1,26 @@
+package pioneer.helpers
+
+/**
+ * Simple motor feedforward: v + a (+ optionally static)
+ * output = kV * velocity + kA * acceleration
+ * Units: velocity in cm/s, acceleration in cm/s^2
+ */
+class Feedforward(val kV: Double, val kA: Double, val kStatic: Double = 0.0) {
+    /**
+     * Compute feedforward for given velocity and acceleration
+     */
+    fun calculate(velocity: Double, acceleration: Double): Double {
+        val signV = if (velocity >= 0.0) 1.0 else -1.0
+        return kV * velocity + kA * acceleration + kStatic * signV
+    }
+
+    /**
+     * Compute feedforward for an array of wheel velocities & accelerations
+     */
+    fun calculate(wheelV: DoubleArray, wheelA: DoubleArray? = null): DoubleArray {
+        return wheelV.mapIndexed { i, v ->
+            val a = wheelA?.get(i) ?: 0.0
+            calculate(v, a)
+        }.toDoubleArray()
+    }
+}

TeamCode/src/main/kotlin/pioneer/helpers/MathUtils.kt

@@ -89,4 +89,8 @@ object MathUtils {
 
         return YawPitchRollAngles(AngleUnit.RADIANS, yaw, pitch, roll, 0)
     }
+
+    fun lerp(x1: Double, x2: Double, t: Double) : Double {
+        return x1 + (x2-x1) * t
+    }
 }