Merge branch 'master' into dependabot/github_actions/actions/checkout-6

MGross21 · web-flow · commit c044306e7a8b · 2025-12-06T00:29:46.000-07:00
diff --git a/TeamCode/src/main/kotlin/pioneer/Constants.kt b/TeamCode/src/main/kotlin/pioneer/Constants.kt
@@ -99,25 +99,33 @@ object Follower {
     /** The maximum drive velocity in cm per second. */
     const val MAX_DRIVE_VELOCITY = 100.0
 
+
     /** The maximum drive acceleration in cm per second squared. */
     const val MAX_DRIVE_ACCELERATION = 50.0
 
+    /** The maximum angular velocity in rad per second. */
+    const val MAX_ANGULAR_VELOCITY = 0.0
+
+    /** The maximum angular acceleration in rad per second squared. */
+    const val MAX_ANGULAR_ACCELERATION = 0.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
 
     // X-axis PID coefficients for the trajectory follower
     @JvmField var X_KP = 0.0
-
     @JvmField var X_KI = 0.0
-
     @JvmField var X_KD = 0.0
 
     // Y-axis PID coefficients for the trajectory follower
     @JvmField var Y_KP = 0.0
-
     @JvmField var Y_KI = 0.0
-
     @JvmField var Y_KD = 0.0
+
+    // Theta PID coefficients for the trajectory follower
+    @JvmField var THETA_KP = 0.0
+    @JvmField var THETA_KI = 0.0
+    @JvmField var THETA_KD = 0.0
 }
 
 object Camera {
diff --git a/TeamCode/src/main/kotlin/pioneer/helpers/MathUtils.kt b/TeamCode/src/main/kotlin/pioneer/helpers/MathUtils.kt
@@ -1,6 +1,8 @@
 package pioneer.helpers
 
 import kotlin.math.PI
+import kotlin.math.cos
+import kotlin.math.sin
 
 object MathUtils {
     /**
@@ -33,4 +35,20 @@ object MathUtils {
         val step = (end - start) / (num - 1)
         return List(num) { i -> start + i * step }
     }
+
+    /**
+     * Rotates a 2D vector by a given heading angle.
+     * @param x X component of the vector
+     * @param y Y component of the vector
+     * @param heading Angle in radians to rotate the vector
+     * @return Pair of rotated (x, y) components
+     */
+    fun rotateVector(x: Double, y: Double, heading: Double): Pair<Double, Double> {
+        val cos = cos(heading)
+        val sin = sin(heading)
+        return Pair(
+            x * cos - y * sin,
+            x * sin + y * cos
+        )
+    }
 }
diff --git a/TeamCode/src/main/kotlin/pioneer/pathing/follower/Follower.kt b/TeamCode/src/main/kotlin/pioneer/pathing/follower/Follower.kt
@@ -3,6 +3,7 @@ package pioneer.pathing.follower
 import com.qualcomm.robotcore.util.ElapsedTime
 import pioneer.hardware.MecanumBase
 import pioneer.helpers.FileLogger
+import pioneer.helpers.MathUtils
 import pioneer.helpers.PIDController
 import pioneer.helpers.Pose
 import pioneer.localization.Localizer
@@ -18,6 +19,8 @@ class Follower(
     private val mecanumBase: MecanumBase,
 ) {
     var motionProfile: MotionProfile? = null
+    var headingProfile: MotionProfile? = null
+
     private var elapsedTime: ElapsedTime = ElapsedTime()
     private var xPID =
         PIDController(
@@ -32,6 +35,13 @@ class Follower(
             kd = FollowerConstants.Y_KD,
         )
 
+    private var headingPID =
+        PIDController(
+            kp = FollowerConstants.THETA_KP,
+            ki = FollowerConstants.THETA_KI,
+            kd = FollowerConstants.THETA_KD,
+        )
+
     var path: Path? = null
         set(value) {
             field = value
@@ -61,17 +71,15 @@ class Follower(
         }
 
     fun update(dt: Double) {
-        if (motionProfile == null || path == null) {
+        if (motionProfile == null || headingProfile == null || path == null) {
 //            FileLogger.error("Follower", "No path or motion profile set")
             return
         }
         // Get the time since the follower started
         val t = elapsedTime.seconds().coerceAtMost(motionProfile!!.duration())
         // Get the target state from the motion profile
         val targetState = motionProfile!![t]
-
-        FileLogger.debug("Follower", "Target Velocity: " + targetState.v)
-        FileLogger.debug("Follower", "Target Acceleration " + targetState.a)
+        val headingTarget = headingProfile!![t]
 
         // Calculate the parameter t for the path based on the target state
         val pathT = path!!.getTFromLength(targetState.x)
@@ -92,34 +100,37 @@ class Follower(
                 vy = tangent.y * targetState.v,
                 ax = targetState.a * tangent.x + targetState.v.pow(2) * curvature * -tangent.y,
                 ay = targetState.a * tangent.y + targetState.v.pow(2) * curvature * tangent.x,
-                theta = localizer.pose.theta,
+                theta = headingTarget.x,
+                omega = headingTarget.v,
+                alpha = headingTarget.a
             )
 
-        // Calculate the position error and convert to robot-centric coordinates
-        val positionError =
+        // Calculate the error and convert to robot-centric coordinates
+        val error =
             Pose(
                 x = targetPose.x - localizer.pose.x,
                 y = targetPose.y - localizer.pose.y,
+                theta = MathUtils.normalizeRadians(headingTarget.x - localizer.pose.theta)
             )
 
         // Calculate the PID outputs
-        val xCorrection = xPID.update(positionError.x, dt)
-        val yCorrection = yPID.update(positionError.y, dt)
+        val xCorrection = xPID.update(error.x, dt)
+        val yCorrection = yPID.update(error.y, dt)
+        val turnCorrection = headingPID.update(error.theta, dt)
 
         // Apply corrections to velocity directly
         // Rotate to convert to robot-centric coordinates
-        val correctedPose =
-            targetPose
-                .copy(
-                    vx = targetPose.vx + xCorrection,
-                    vy = targetPose.vy + yCorrection,
-                ).rotate(-localizer.pose.theta)
-
-        FileLogger.debug("Follower", "Target pose: $targetPose")
-        FileLogger.debug("Follower", "Corrected pose: $correctedPose")
+        val (vxRobot, vyRobot) = MathUtils.rotateVector(
+            x = targetPose.vx + xCorrection,
+            y = targetPose.vy + yCorrection,
+            heading = -localizer.pose.theta
+        )
 
-        // TODO: Heading interpolation
-        // TODO: Add heading error correction
+        val correctedPose = targetPose.copy(
+            vx = vxRobot,
+            vy = vyRobot,
+            omega = targetPose.omega + turnCorrection
+        )
 
         mecanumBase.setDriveVA(correctedPose)
     }
@@ -129,15 +140,17 @@ class Follower(
         elapsedTime.reset()
         // Reset the PID controllers
         xPID.reset()
-        yPID.reset()
+                yPID.reset()
+        headingPID.reset()
     }
 
     private fun reset() {
         // Recalculate the motion profile when the path is set
-        calculateMotionProfile()
+        motionProfile = calculateMotionProfile()
+        headingProfile = calculateHeadingProfile()
     }
 
-    private fun calculateMotionProfile() {
+    private fun calculateMotionProfile(): MotionProfile? {
         if (path != null) {
             val totalDistance = path!!.getLength()
             val startState = MotionState(0.0, 0.0, 0.0)
@@ -157,15 +170,23 @@ class Follower(
                 // Constant acceleration constraint
                 FollowerConstants.MAX_DRIVE_ACCELERATION
             }
-            motionProfile =
-                MotionProfileGenerator.generateMotionProfile(
+            return MotionProfileGenerator.generateMotionProfile(
                     startState,
                     endState,
                     velocityConstraint,
                     accelerationConstraint,
                 )
         } else {
-            motionProfile = null
+            return null
         }
     }
+
+    private fun calculateHeadingProfile(): MotionProfile? {
+        return if (path != null) MotionProfileGenerator.generateMotionProfile(
+            MotionState(localizer.pose.theta, 0.0, 0.0),
+            MotionState(path!!.endPose.theta, 0.0, 0.0),
+            { FollowerConstants.MAX_ANGULAR_VELOCITY },
+            { FollowerConstants.MAX_ANGULAR_ACCELERATION },
+        ) else null
+    }
 }
diff --git a/TeamCode/src/main/kotlin/pioneer/pathing/paths/CompoundPath.kt b/TeamCode/src/main/kotlin/pioneer/pathing/paths/CompoundPath.kt
@@ -13,8 +13,6 @@ class CompoundPath(
 ) : Path {
     override var startPose: Pose = paths.first().startPose
     override var endPose: Pose = paths.last().endPose
-    override var headingInterpolationMode: Path.HeadingInterpolationMode =
-        Path.HeadingInterpolationMode.LINEAR
 
     init {
         // Validate that the paths are connected end-to-end
@@ -27,12 +25,6 @@ class CompoundPath(
 
     override fun getLength(): Double = paths.sumOf { it.getLength() }
 
-    override fun getHeading(t: Double): Double {
-        val pathIndex = (t * paths.size).toInt().coerceIn(0, paths.size - 1)
-        val localT = t * paths.size - pathIndex
-        return paths[pathIndex].getHeading(localT)
-    }
-
     override fun getPoint(t: Double): Pose {
         val pathIndex = (t * paths.size).toInt().coerceIn(0, paths.size - 1)
         val localT = t * paths.size - pathIndex
diff --git a/TeamCode/src/main/kotlin/pioneer/pathing/paths/HermitePath.kt b/TeamCode/src/main/kotlin/pioneer/pathing/paths/HermitePath.kt
@@ -2,7 +2,6 @@ package pioneer.pathing.paths
 
 import pioneer.helpers.Polynomial
 import pioneer.helpers.Pose
-import pioneer.pathing.paths.Path.HeadingInterpolationMode
 import kotlin.math.pow
 
 /**
@@ -18,9 +17,6 @@ class HermitePath(
     startVelocity: Pose = Pose(),
     endVelocity: Pose = Pose(),
 ) : Path {
-    // Enum for heading interpolation mode
-    override var headingInterpolationMode: HeadingInterpolationMode = HeadingInterpolationMode.LINEAR
-
     // Hermite basis functions
     private val basis00 = Polynomial(arrayOf(1.0, 0.0, -3.0, 2.0))
     private val basis01 = Polynomial(arrayOf(0.0, 1.0, -2.0, 1.0))
@@ -61,26 +57,16 @@ class HermitePath(
         return compoundPath.getTFromLength(length)
     }
 
-    override fun getHeading(t: Double): Double {
-        when (headingInterpolationMode) {
-            HeadingInterpolationMode.LINEAR -> {
-                return startPose.theta + (endPose.theta - startPose.theta) * t
-            }
-        }
-    }
-
     override fun getPoint(t: Double): Pose =
         Pose(
             x = xHermite.eval(t),
             y = yHermite.eval(t),
-            theta = getHeading(t),
         )
 
     override fun getPose(t: Double): Pose =
         Pose(
             x = xHermite.eval(t),
             y = yHermite.eval(t),
-            theta = getHeading(t),
             vx = xHermite.nDerEval(t, 1),
             vy = yHermite.nDerEval(t, 1),
             ax = xHermite.nDerEval(t, 2),
@@ -128,7 +114,7 @@ class HermitePath(
         }
 
         fun setTension(tension: Double): Builder {
-            if (tension < 0.0 || tension > 1.0) {
+            if (tension !in 0.0..1.0) {
                 throw IllegalArgumentException("Tension must be in the range [0, 1]")
             }
             this.t = tension
diff --git a/TeamCode/src/main/kotlin/pioneer/pathing/paths/LinearPath.kt b/TeamCode/src/main/kotlin/pioneer/pathing/paths/LinearPath.kt
@@ -1,8 +1,6 @@
 package pioneer.pathing.paths
 
-import pioneer.helpers.MathUtils
 import pioneer.helpers.Pose
-import pioneer.pathing.paths.Path.HeadingInterpolationMode
 
 /**
  * LinearPath class representing a straight line path in 2D space
@@ -15,37 +13,22 @@ class LinearPath(
 ) : Path {
     // Constructor overloads
     constructor(startX: Double, startY: Double, endX: Double, endY: Double) : this(Pose(startX, startY), Pose(endX, endY))
-
-    // Enum for heading interpolation mode
-    override var headingInterpolationMode: HeadingInterpolationMode = HeadingInterpolationMode.LINEAR
-
     override fun getLength(): Double = startPose.distanceTo(endPose)
 
     override fun getLengthSoFar(t: Double): Double = getLength() * t
 
     override fun getTFromLength(length: Double): Double = length / getLength()
 
-    override fun getHeading(t: Double): Double {
-        when (headingInterpolationMode) {
-            HeadingInterpolationMode.LINEAR -> {
-                val delta = MathUtils.normalizeRadians(endPose.theta - startPose.theta)
-                return startPose.theta + delta * t
-            }
-        }
-    }
-
     override fun getPoint(t: Double): Pose {
         val x = startPose.x + (endPose.x - startPose.x) * t
         val y = startPose.y + (endPose.y - startPose.y) * t
-        val heading = getHeading(t)
-        return Pose(x, y, heading)
+        return Pose(x, y)
     }
 
     override fun getPose(t: Double): Pose =
         Pose(
             x = startPose.x + (endPose.x - startPose.x) * t,
             y = startPose.y + (endPose.y - startPose.y) * t,
-            theta = getHeading(t),
             vx = (endPose.x - startPose.x) / getLength(),
             vy = (endPose.y - startPose.y) / getLength(),
         )
diff --git a/TeamCode/src/main/kotlin/pioneer/pathing/paths/Path.kt b/TeamCode/src/main/kotlin/pioneer/pathing/paths/Path.kt
@@ -6,18 +6,6 @@ import pioneer.helpers.Pose
  * Path interface representing a path in 2D space
  */
 interface Path {
-    /**
-     * Enum for heading interpolation modes
-     */
-    enum class HeadingInterpolationMode {
-        LINEAR,
-    }
-
-    /**
-     * Enum for heading interpolation mode
-     */
-    var headingInterpolationMode: HeadingInterpolationMode
-
     /**
      * The start and end poses of the path
      */
@@ -54,13 +42,6 @@ interface Path {
      */
     fun getTFromLength(length: Double): Double
 
-    /**
-     * Gets the heading at the given parameter t
-     * @param t The parameter value in the range [0, 1]
-     * @return The heading at the given parameter
-     */
-    fun getHeading(t: Double): Double
-
     /**
      * Gets the point at the given parameter t
      * @param t The parameter value in the range [0, 1]
@@ -88,17 +69,4 @@ interface Path {
      * @return The closest point on the path to the given position
      */
     fun getClosestPoint(position: Pose): Pose = getPoint(getClosestPointT(position))
-
-    /**
-     * Gets the heading goal at the given parameter t based on the interpolation mode
-     * @param t The parameter value in the range [0, 1]
-     * @return The heading goal at the given parameter
-     */
-    fun getHeadingGoal(t: Double): Double {
-        when (headingInterpolationMode) {
-            HeadingInterpolationMode.LINEAR -> {
-                return startPose.theta + (endPose.theta - startPose.theta) * t
-            }
-        }
-    }
 }
