Skip to content

Commit c61c97f

Browse files
zachwaffle4zachwaffle4
authored
stuff for the first alpha (#1)
* add: configure testing dependencies for control module and XML file updates Signed-off-by: Zach Harel <zach@zharel.me> * add: implement AngularState and LinearState classes for kinematic motion representation Signed-off-by: Zach Harel <zach@zharel.me> * add: introduce InchesPerSecond and inchesPerSecond extension for linear units Signed-off-by: Zach Harel <zach@zharel.me> * remove state classes Signed-off-by: Zach Harel <zach@zharel.me> * add: implement constructor for SizedVector from SizedMatrix Signed-off-by: Zach Harel <zach@zharel.me> * reformat copyright again Signed-off-by: Zach Harel <zach@zharel.me> * add: override toString() method to return short string representation for various unit classes Signed-off-by: Zach Harel <zach@zharel.me> * add: implement PID and SquID controller classes Signed-off-by: Zach Harel <zach@zharel.me> * add: implement MotionState data class for tracking position, velocity, and acceleration Signed-off-by: Zach Harel <zach@zharel.me> * refactor: rename SizedMatrix to Matrix and SizedVector to Vector to make it more clear that they are the default Signed-off-by: Zach Harel <zach@zharel.me> * add: implement GravityFeedforward and SimpleFeedforward controllers with associated tests Signed-off-by: Zach Harel <zach@zharel.me> * add: implement TrapezoidProfile class and associated tests for motion profiling Signed-off-by: Zach Harel <zach@zharel.me> * add: implement LinearModel class and Model interface for state-space representation Signed-off-by: Zach Harel <zach@zharel.me> * refactor: rename SizedVector and SizedMatrix to Vector and Matrix for clarity Signed-off-by: Zach Harel <zach@zharel.me> * add: introduce natural number representations for rows and columns in Matrix class Signed-off-by: Zach Harel <zach@zharel.me> * refactor: adjust line length limit to 120 and reformat Signed-off-by: Zach Harel <zach@zharel.me> * add: implement LQRController control using state-space representation Signed-off-by: Zach Harel <zach@zharel.me> * add: move utility functions for state-space representation to StateSpaceUtil and clean up LQRController Signed-off-by: Zach Harel <zach@zharel.me> * add: implement matrix exponential function using Padé approximant and add tests Signed-off-by: Zach Harel <zach@zharel.me> * add: move discretizeAB to new file and add discretizeAQ + tests Signed-off-by: Zach Harel <zach@zharel.me> * add: enhance matrix operations to support division by scalar Signed-off-by: Zach Harel <zach@zharel.me> * add: implement discretizeR function and improve formatting in Discretization and StateSpaceUtil Signed-off-by: Zach Harel <zach@zharel.me> * add: update scalar multiplication to accept Number type in DynamicVector and Vector classes Signed-off-by: Zach Harel <zach@zharel.me> * add: integrate discretization of matrices in LinearModel using discretizeAB function Signed-off-by: Zach Harel <zach@zharel.me> * add: implement Kalman filter for linear state estimation Signed-off-by: Zach Harel <zach@zharel.me> * add: update version to 0.0.1-alpha.1 in gradle.properties Signed-off-by: Zach Harel <zach@zharel.me> * run spotless oopsies Signed-off-by: Zach Harel <zach@zharel.me> * add: update group ID to 'dev.nextftc.control' in build.gradle.kts Signed-off-by: Zach Harel <zach@zharel.me> --------- Signed-off-by: Zach Harel <zach@zharel.me>
1 parent 3e82ede commit c61c97f

File tree

100 files changed

+3048
-273
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

100 files changed

+3048
-273
lines changed

‎.idea/codeStyles/codeStyleConfig.xml‎

Lines changed: 5 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎.idea/copyright/BSD_3_License_header.xml‎

Lines changed: 1 addition & 1 deletion
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎.idea/copyright/profiles_settings.xml‎

Lines changed: 2 additions & 2 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎build.gradle.kts‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ plugins {
88

99
allprojects {
1010
version = property("version") as String
11-
group = "dev.nextftc"
11+
group = "dev.nextftc.control"
1212
}
1313

1414
subprojects {

‎control/build.gradle.kts‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,8 @@ description = "A WPIMath inspired library for controls and other math classes an
88
dependencies {
99
api(project(":units"))
1010
api(project(":linalg"))
11+
12+
testImplementation(libs.bundles.kotest)
1113
}
1214

1315
nextFTCPublishing {
@@ -22,6 +24,8 @@ kotlin {
2224
}
2325
}
2426

27+
tasks.withType<Test>().configureEach { useJUnitPlatform() }
28+
2529
spotless {
2630
kotlin {
2731
ktlint().editorConfigOverride(
@@ -30,7 +34,7 @@ spotless {
3034
"indent_size" to "4",
3135
"continuation_indent_size" to "4",
3236
"ktlint_standard_no-wildcard-imports" to "disabled",
33-
"max_line_length" to "100",
37+
"max_line_length" to "120",
3438
),
3539
)
3640
}

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

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
/*
2+
* Copyright (c) 2025 NextFTC Team
3+
*
4+
* Use of this source code is governed by an BSD-3-clause
5+
* license that can be found in the LICENSE.md file at the root of this repository or at
6+
* https://opensource.org/license/bsd-3-clause.
7+
*/
8+
9+
@file:Suppress("ktlint:standard:property-naming")
10+
11+
package dev.nextftc.control.feedback
12+
13+
import dev.nextftc.control.model.LinearModel
14+
import dev.nextftc.control.util.discretizeAB
15+
import dev.nextftc.control.util.makeBrysonMatrix
16+
import dev.nextftc.control.util.solveDARE
17+
import dev.nextftc.linalg.Matrix
18+
import dev.nextftc.linalg.N1
19+
import dev.nextftc.linalg.Nat
20+
import dev.nextftc.linalg.Vector
21+
22+
/**
23+
* A Linear Quadratic Regulator (LQR) for controlling a system modeled by state-space equations.
24+
*
25+
* LQR is a form of optimal control that finds the best control input to apply to a system
26+
* by minimizing a quadratic cost function. The cost function balances two competing goals:
27+
* 1. **State Error**: How far the system is from its desired target state (penalized by the `Q` matrix).
28+
* 2. **Control Effort**: How much energy or effort is used to control the system (penalized by the `R` matrix).
29+
*
30+
* The controller computes the optimal control input `u` using a simple state-feedback law: `u = -Kx`,
31+
* where `x` is the system's state error and `K` is the optimal gain matrix.
32+
**
33+
* Thank you to Tyler Veness and WPILib!
34+
*
35+
* @param A The state matrix.
36+
* @param B The input matrix.
37+
* @param Q The state cost matrix.
38+
* @param R The control cost matrix.
39+
* @param dt The time step for the discrete-time model (your loop time)
40+
*
41+
* @see <a href="https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator">LQR on Wikipedia</a>
42+
* @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>
43+
*/
44+
class LQRController<States : Nat, Inputs : Nat, Outputs : Nat> @JvmOverloads constructor(
45+
A: Matrix<States, States>,
46+
B: Matrix<States, Inputs>,
47+
Q: Matrix<States, States>,
48+
R: Matrix<Inputs, Inputs>,
49+
private val dt: Double = 0.05,
50+
) {
51+
private val K: Matrix<Inputs, States>
52+
53+
init {
54+
require(dt > 0) { "Time step (dt) must be positive" }
55+
val (Ad, Bd) = discretizeAB(A, B, dt)
56+
val (_, K) = computeLQRGain(Ad, Bd, Q, R)
57+
this.K = K
58+
}
59+
60+
/**
61+
* Constructs a controller with the given coefficient matrices.
62+
*
63+
* @param A the state matrix
64+
* @param B the input matrix
65+
* @param Qelems the maximum state error for each state dimension
66+
* @param Relems the maximum control effort for each control input dimension
67+
* @param dt the time step for the discrete-time model (your loop time)
68+
*/
69+
@JvmOverloads constructor(
70+
A: Matrix<States, States>,
71+
B: Matrix<States, Inputs>,
72+
Qelems: Vector<States>,
73+
Relems: Vector<Inputs>,
74+
dt: Double = 0.05,
75+
) : this(A, B, makeBrysonMatrix(Qelems), makeBrysonMatrix(Relems), dt)
76+
77+
/**
78+
* Constructs a controller with the given plant model and cost matrices.
79+
*
80+
* @param plant the plant model
81+
* @param Qelems the maximum state error for each state dimension
82+
* @param Relems the maximum control effort for each control input dimension
83+
* @param dt the time step for the discrete-time model (your loop time)
84+
*/
85+
@JvmOverloads constructor(
86+
plant: LinearModel<States, Inputs, Outputs>,
87+
Qelems: Vector<States>,
88+
Relems: Vector<Inputs>,
89+
dt: Double = 0.05,
90+
) : this(plant.A, plant.B, Qelems, Relems, dt)
91+
92+
/**
93+
* Calculates the optimal control input to correct for the given state error.
94+
*
95+
* @param error The current state error of the system, represented as a Matrix.
96+
* @return The calculated optimal control input as a Matrix.
97+
* */
98+
fun update(error: Matrix<States, N1>): Matrix<Inputs, N1> = -K * error
99+
}
100+
101+
/**
102+
* Computes the optimal gain matrix K using [dev.nextftc.control.util.solveDARE].
103+
*
104+
* @return Pair of DARE solution X and K.
105+
*/
106+
internal fun <States : Nat, Inputs : Nat> computeLQRGain(
107+
Ad: Matrix<States, States>,
108+
Bd: Matrix<States, Inputs>,
109+
Q: Matrix<States, States>,
110+
R: Matrix<Inputs, Inputs>,
111+
maxIter: Int = -1,
112+
epsilon: Double = 1e-6,
113+
): Pair<Matrix<States, States>, Matrix<Inputs, States>> {
114+
val X = solveDARE(Ad, Bd, Q, R, maxIter, epsilon)
115+
116+
val btx = Bd.transpose * X
117+
val btxb = btx * Bd
118+
val K = (R + btxb).inverse * btx * Ad
119+
120+
return X to K
121+
}

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

Lines changed: 153 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,153 @@
1+
/*
2+
* Copyright (c) 2025 NextFTC Team
3+
*
4+
* Use of this source code is governed by an BSD-3-clause
5+
* license that can be found in the LICENSE.md file at the root of this repository or at
6+
* https://opensource.org/license/bsd-3-clause.
7+
*/
8+
9+
package dev.nextftc.control.feedback
10+
11+
import kotlin.math.sign
12+
import kotlin.time.ComparableTimeMark
13+
import kotlin.time.DurationUnit
14+
15+
/**
16+
* Coefficients for a PID Controller/
17+
*
18+
* @param kP proportional gain, multiplied by the error
19+
* @param kI integral gain, multiplied by the integral of the error over time
20+
* @param kD derivative gain, multiplied by the derivative of the error
21+
*/
22+
data class PIDCoefficients @JvmOverloads constructor(
23+
@JvmField var kP: Double,
24+
@JvmField var kI: Double = 0.0,
25+
@JvmField var kD: Double = 0.0,
26+
)
27+
28+
/**
29+
* Traditional proportional-integral-derivative controller.
30+
*
31+
* @param coefficients the [PIDCoefficients] that contains the PID gains
32+
* @param resetIntegralOnZeroCrossover whether to reset the integral term when the error crosses
33+
*/
34+
class PIDController @JvmOverloads constructor(
35+
val coefficients: PIDCoefficients,
36+
val resetIntegralOnZeroCrossover: Boolean = true,
37+
) {
38+
private var lastError: Double = 0.0
39+
private var errorSum = 0.0
40+
private var lastTimestamp: ComparableTimeMark? = null
41+
42+
/**
43+
* Creates a PIDController with the given coefficients.
44+
*
45+
* @param kP proportional gain, multiplied by the error
46+
* @param kI integral gain, multiplied by the integral of the error over time
47+
* @param kD derivative gain, multiplied by the derivative of the error
48+
*/
49+
@JvmOverloads constructor(
50+
kP: Double,
51+
kI: Double = 0.0,
52+
kD: Double = 0.0,
53+
resetIntegralOnZeroCrossover: Boolean = true,
54+
) : this(
55+
PIDCoefficients(kP, kI, kD),
56+
resetIntegralOnZeroCrossover,
57+
)
58+
59+
/**
60+
* Calculates the PID output
61+
*
62+
* @param timestamp the current time
63+
* @param error the error in the target state; the difference between the desired
64+
* state and the current state
65+
* @param errorDerivative the derivative of the error, or `null` to compute it automatically
66+
* from the change in error over time. This is typically the difference between the desired
67+
* and current velocity when controlling position, or the difference in acceleration when
68+
* controlling velocity.
69+
*
70+
* @return the PID output
71+
*/
72+
fun calculate(timestamp: ComparableTimeMark, error: Double, errorDerivative: Double?): Double {
73+
if (lastTimestamp == null) {
74+
lastError = error
75+
lastTimestamp = timestamp
76+
// On first call with no derivative provided, ignore D term
77+
val derivative = errorDerivative ?: 0.0
78+
return coefficients.kP * error + coefficients.kD * derivative
79+
}
80+
81+
if (resetIntegralOnZeroCrossover && lastError.sign != error.sign) {
82+
errorSum = 0.0
83+
}
84+
85+
val deltaT = (timestamp - lastTimestamp!!).toDouble(DurationUnit.NANOSECONDS)
86+
errorSum += error * deltaT
87+
88+
val derivative = errorDerivative ?: ((error - lastError) / deltaT)
89+
90+
lastError = error
91+
lastTimestamp = timestamp
92+
93+
return coefficients.kP * error + coefficients.kI * errorSum + coefficients.kD *
94+
derivative
95+
}
96+
97+
/**
98+
* Calculates the PID output from a reference (setpoint) and measured value.
99+
*
100+
* This overload assumes the reference derivative is zero (i.e., the setpoint is constant).
101+
*
102+
* @param timestamp the current time
103+
* @param reference the desired/target value (setpoint)
104+
* @param measured the current measured value
105+
* @param measuredDerivative the derivative of the measured value, or `null` to compute the
106+
* error derivative automatically from the change in error over time.
107+
*
108+
* @return the PID output
109+
*/
110+
fun calculate(
111+
timestamp: ComparableTimeMark,
112+
reference: Double,
113+
measured: Double,
114+
measuredDerivative: Double?,
115+
): Double = calculate(
116+
timestamp,
117+
reference - measured,
118+
measuredDerivative?.let { -it },
119+
)
120+
121+
/**
122+
* Calculates the PID output from a reference (setpoint) and measured value, with their
123+
* respective derivatives.
124+
*
125+
* @param timestamp the current time
126+
* @param reference the desired/target value (setpoint)
127+
* @param measured the current measured value
128+
* @param referenceDerivative the derivative of the reference value (e.g., desired velocity)
129+
* @param measuredDerivative the derivative of the measured value (e.g., current velocity)
130+
*
131+
* @return the PID output
132+
*/
133+
fun calculate(
134+
timestamp: ComparableTimeMark,
135+
reference: Double,
136+
measured: Double,
137+
referenceDerivative: Double,
138+
measuredDerivative: Double,
139+
): Double = calculate(
140+
timestamp,
141+
reference - measured,
142+
referenceDerivative - measuredDerivative,
143+
)
144+
145+
/**
146+
* Resets the PID controller
147+
*/
148+
fun reset() {
149+
errorSum = 0.0
150+
lastError = 0.0
151+
lastTimestamp = null
152+
}
153+
}

0 commit comments

Comments
 (0)