Implement heading profile in follower

Author: Braedeng0

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 {

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
+        )
+    }
 }

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
+    }
 }