stuff for the first alpha

build.gradle.kts

@@ -8,7 +8,7 @@ plugins {
 
 allprojects {
     version = property("version") as String
-    group = "dev.nextftc"
+    group = "dev.nextftc.control"
 }
 
 subprojects {

control/build.gradle.kts

@@ -8,6 +8,8 @@ description = "A WPIMath inspired library for controls and other math classes an
 dependencies {
     api(project(":units"))
     api(project(":linalg"))
+
+    testImplementation(libs.bundles.kotest)
 }
 
 nextFTCPublishing {
@@ -22,6 +24,8 @@ kotlin {
     }
 }
 
+tasks.withType<Test>().configureEach { useJUnitPlatform() }
+
 spotless {
     kotlin {
         ktlint().editorConfigOverride(
@@ -30,7 +34,7 @@ spotless {
                 "indent_size" to "4",
                 "continuation_indent_size" to "4",
                 "ktlint_standard_no-wildcard-imports" to "disabled",
-                "max_line_length" to "100",
+                "max_line_length" to "120",
             ),
         )
     }

control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt

@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2025 NextFTC Team
+ *
+ *  Use of this source code is governed by an BSD-3-clause
+ *  license that can be found in the LICENSE.md file at the root of this repository or at
+ *  https://opensource.org/license/bsd-3-clause.
+ */
+
+@file:Suppress("ktlint:standard:property-naming")
+
+package dev.nextftc.control.feedback
+
+import dev.nextftc.control.model.LinearModel
+import dev.nextftc.control.util.discretizeAB
+import dev.nextftc.control.util.makeBrysonMatrix
+import dev.nextftc.control.util.solveDARE
+import dev.nextftc.linalg.Matrix
+import dev.nextftc.linalg.N1
+import dev.nextftc.linalg.Nat
+import dev.nextftc.linalg.Vector
+
+/**
+ * A Linear Quadratic Regulator (LQR) for controlling a system modeled by state-space equations.
+ *
+ * LQR is a form of optimal control that finds the best control input to apply to a system
+ * by minimizing a quadratic cost function. The cost function balances two competing goals:
+ * 1.  **State Error**: How far the system is from its desired target state (penalized by the `Q` matrix).
+ * 2.  **Control Effort**: How much energy or effort is used to control the system (penalized by the `R` matrix).
+ *
+ * The controller computes the optimal control input `u` using a simple state-feedback law: `u = -Kx`,
+ * where `x` is the system's state error and `K` is the optimal gain matrix.
+ **
+ * Thank you to Tyler Veness and WPILib!
+ *
+ * @param A The state matrix.
+ * @param B The input matrix.
+ * @param Q The state cost matrix.
+ * @param R The control cost matrix.
+ * @param dt The time step for the discrete-time model (your loop time)
+ *
+ * @see <a href="https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator">LQR on Wikipedia</a>
+ * @see <a href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#the-linear-quadratic-regulator">LQR in WPILib</a>
+ */
+class LQRController<States : Nat, Inputs : Nat, Outputs : Nat> @JvmOverloads constructor(
+    A: Matrix<States, States>,
+    B: Matrix<States, Inputs>,
+    Q: Matrix<States, States>,
+    R: Matrix<Inputs, Inputs>,
+    private val dt: Double = 0.05,
+) {
+    private val K: Matrix<Inputs, States>
+
+    init {
+        require(dt > 0) { "Time step (dt) must be positive" }
+        val (Ad, Bd) = discretizeAB(A, B, dt)
+        val (_, K) = computeLQRGain(Ad, Bd, Q, R)
+        this.K = K
+    }
+
+    /**
+     * Constructs a controller with the given coefficient matrices.
+     *
+     * @param A the state matrix
+     * @param B the input matrix
+     * @param Qelems the maximum state error for each state dimension
+     * @param Relems the maximum control effort for each control input dimension
+     * @param dt the time step for the discrete-time model (your loop time)
+     */
+    @JvmOverloads constructor(
+        A: Matrix<States, States>,
+        B: Matrix<States, Inputs>,
+        Qelems: Vector<States>,
+        Relems: Vector<Inputs>,
+        dt: Double = 0.05,
+    ) : this(A, B, makeBrysonMatrix(Qelems), makeBrysonMatrix(Relems), dt)
+
+    /**
+     * Constructs a controller with the given plant model and cost matrices.
+     *
+     * @param plant the plant model
+     * @param Qelems the maximum state error for each state dimension
+     * @param Relems the maximum control effort for each control input dimension
+     * @param dt the time step for the discrete-time model (your loop time)
+     */
+    @JvmOverloads constructor(
+        plant: LinearModel<States, Inputs, Outputs>,
+        Qelems: Vector<States>,
+        Relems: Vector<Inputs>,
+        dt: Double = 0.05,
+    ) : this(plant.A, plant.B, Qelems, Relems, dt)
+
+    /**
+     * Calculates the optimal control input to correct for the given state error.
+     *
+     * @param error The current state error of the system, represented as a Matrix.
+     * @return The calculated optimal control input as a Matrix.
+     * */
+    fun update(error: Matrix<States, N1>): Matrix<Inputs, N1> = -K * error
+}
+
+/**
+ * Computes the optimal gain matrix K using [dev.nextftc.control.util.solveDARE].
+ *
+ * @return Pair of DARE solution X and K.
+ */
+internal fun <States : Nat, Inputs : Nat> computeLQRGain(
+    Ad: Matrix<States, States>,
+    Bd: Matrix<States, Inputs>,
+    Q: Matrix<States, States>,
+    R: Matrix<Inputs, Inputs>,
+    maxIter: Int = -1,
+    epsilon: Double = 1e-6,
+): Pair<Matrix<States, States>, Matrix<Inputs, States>> {
+    val X = solveDARE(Ad, Bd, Q, R, maxIter, epsilon)
+
+    val btx = Bd.transpose * X
+    val btxb = btx * Bd
+    val K = (R + btxb).inverse * btx * Ad
+
+    return X to K
+}

control/src/main/kotlin/dev/nextftc/control/feedback/PIDController.kt

@@ -0,0 +1,153 @@
+/*
+ * Copyright (c) 2025 NextFTC Team
+ *
+ *  Use of this source code is governed by an BSD-3-clause
+ *  license that can be found in the LICENSE.md file at the root of this repository or at
+ *  https://opensource.org/license/bsd-3-clause.
+ */
+
+package dev.nextftc.control.feedback
+
+import kotlin.math.sign
+import kotlin.time.ComparableTimeMark
+import kotlin.time.DurationUnit
+
+/**
+ * Coefficients for a PID Controller/
+ *
+ * @param kP proportional gain, multiplied by the error
+ * @param kI integral gain, multiplied by the integral of the error over time
+ * @param kD derivative gain, multiplied by the derivative of the error
+ */
+data class PIDCoefficients @JvmOverloads constructor(
+    @JvmField var kP: Double,
+    @JvmField var kI: Double = 0.0,
+    @JvmField var kD: Double = 0.0,
+)
+
+/**
+ * Traditional proportional-integral-derivative controller.
+ *
+ * @param coefficients the [PIDCoefficients] that contains the PID gains
+ * @param resetIntegralOnZeroCrossover whether to reset the integral term when the error crosses
+ */
+class PIDController @JvmOverloads constructor(
+    val coefficients: PIDCoefficients,
+    val resetIntegralOnZeroCrossover: Boolean = true,
+) {
+    private var lastError: Double = 0.0
+    private var errorSum = 0.0
+    private var lastTimestamp: ComparableTimeMark? = null
+
+    /**
+     * Creates a PIDController with the given coefficients.
+     *
+     * @param kP proportional gain, multiplied by the error
+     * @param kI integral gain, multiplied by the integral of the error over time
+     * @param kD derivative gain, multiplied by the derivative of the error
+     */
+    @JvmOverloads constructor(
+        kP: Double,
+        kI: Double = 0.0,
+        kD: Double = 0.0,
+        resetIntegralOnZeroCrossover: Boolean = true,
+    ) : this(
+        PIDCoefficients(kP, kI, kD),
+        resetIntegralOnZeroCrossover,
+    )
+
+    /**
+     * Calculates the PID output
+     *
+     * @param timestamp the current time
+     * @param error the error in the target state; the difference between the desired
+     *  state and the current state
+     * @param errorDerivative the derivative of the error, or `null` to compute it automatically
+     *  from the change in error over time. This is typically the difference between the desired
+     *  and current velocity when controlling position, or the difference in acceleration when
+     *  controlling velocity.
+     *
+     * @return the PID output
+     */
+    fun calculate(timestamp: ComparableTimeMark, error: Double, errorDerivative: Double?): Double {
+        if (lastTimestamp == null) {
+            lastError = error
+            lastTimestamp = timestamp
+            // On first call with no derivative provided, ignore D term
+            val derivative = errorDerivative ?: 0.0
+            return coefficients.kP * error + coefficients.kD * derivative
+        }
+
+        if (resetIntegralOnZeroCrossover && lastError.sign != error.sign) {
+            errorSum = 0.0
+        }
+
+        val deltaT = (timestamp - lastTimestamp!!).toDouble(DurationUnit.NANOSECONDS)
+        errorSum += error * deltaT
+
+        val derivative = errorDerivative ?: ((error - lastError) / deltaT)
+
+        lastError = error
+        lastTimestamp = timestamp
+
+        return coefficients.kP * error + coefficients.kI * errorSum + coefficients.kD *
+            derivative
+    }
+
+    /**
+     * Calculates the PID output from a reference (setpoint) and measured value.
+     *
+     * This overload assumes the reference derivative is zero (i.e., the setpoint is constant).
+     *
+     * @param timestamp the current time
+     * @param reference the desired/target value (setpoint)
+     * @param measured the current measured value
+     * @param measuredDerivative the derivative of the measured value, or `null` to compute the
+     *  error derivative automatically from the change in error over time.
+     *
+     * @return the PID output
+     */
+    fun calculate(
+        timestamp: ComparableTimeMark,
+        reference: Double,
+        measured: Double,
+        measuredDerivative: Double?,
+    ): Double = calculate(
+        timestamp,
+        reference - measured,
+        measuredDerivative?.let { -it },
+    )
+
+    /**
+     * Calculates the PID output from a reference (setpoint) and measured value, with their
+     * respective derivatives.
+     *
+     * @param timestamp the current time
+     * @param reference the desired/target value (setpoint)
+     * @param measured the current measured value
+     * @param referenceDerivative the derivative of the reference value (e.g., desired velocity)
+     * @param measuredDerivative the derivative of the measured value (e.g., current velocity)
+     *
+     * @return the PID output
+     */
+    fun calculate(
+        timestamp: ComparableTimeMark,
+        reference: Double,
+        measured: Double,
+        referenceDerivative: Double,
+        measuredDerivative: Double,
+    ): Double = calculate(
+        timestamp,
+        reference - measured,
+        referenceDerivative - measuredDerivative,
+    )
+
+    /**
+     * Resets the PID controller
+     */
+    fun reset() {
+        errorSum = 0.0
+        lastError = 0.0
+        lastTimestamp = null
+    }
+}