diff --git a/.idea/codeStyles/codeStyleConfig.xml b/.idea/codeStyles/codeStyleConfig.xml new file mode 100644 index 0000000..a55e7a1 --- /dev/null +++ b/.idea/codeStyles/codeStyleConfig.xml @@ -0,0 +1,5 @@ + + + + \ No newline at end of file diff --git a/.idea/copyright/BSD_3_License_header.xml b/.idea/copyright/BSD_3_License_header.xml index 6862309..550881b 100644 --- a/.idea/copyright/BSD_3_License_header.xml +++ b/.idea/copyright/BSD_3_License_header.xml @@ -1,6 +1,6 @@ - \ No newline at end of file diff --git a/.idea/copyright/profiles_settings.xml b/.idea/copyright/profiles_settings.xml index eb08c84..aee8f3f 100644 --- a/.idea/copyright/profiles_settings.xml +++ b/.idea/copyright/profiles_settings.xml @@ -1,7 +1,7 @@ - + - + \ No newline at end of file diff --git a/build.gradle.kts b/build.gradle.kts index dc1f6d2..1313446 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -8,7 +8,7 @@ plugins { allprojects { version = property("version") as String - group = "dev.nextftc" + group = "dev.nextftc.control" } subprojects { diff --git a/control/build.gradle.kts b/control/build.gradle.kts index 189b634..df235e4 100644 --- a/control/build.gradle.kts +++ b/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().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", ), ) } diff --git a/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt b/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt new file mode 100644 index 0000000..8d0154a --- /dev/null +++ b/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 LQR on Wikipedia + * @see LQR in WPILib + */ +class LQRController @JvmOverloads constructor( + A: Matrix, + B: Matrix, + Q: Matrix, + R: Matrix, + private val dt: Double = 0.05, +) { + private val K: Matrix + + 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, + B: Matrix, + Qelems: Vector, + Relems: Vector, + 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, + Qelems: Vector, + Relems: Vector, + 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): Matrix = -K * error +} + +/** + * Computes the optimal gain matrix K using [dev.nextftc.control.util.solveDARE]. + * + * @return Pair of DARE solution X and K. + */ +internal fun computeLQRGain( + Ad: Matrix, + Bd: Matrix, + Q: Matrix, + R: Matrix, + maxIter: Int = -1, + epsilon: Double = 1e-6, +): Pair, Matrix> { + 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 +} diff --git a/control/src/main/kotlin/dev/nextftc/control/feedback/PIDController.kt b/control/src/main/kotlin/dev/nextftc/control/feedback/PIDController.kt new file mode 100644 index 0000000..c476678 --- /dev/null +++ b/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 + } +} diff --git a/control/src/main/kotlin/dev/nextftc/control/feedback/SquIDController.kt b/control/src/main/kotlin/dev/nextftc/control/feedback/SquIDController.kt new file mode 100644 index 0000000..4ff4d5a --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/feedback/SquIDController.kt @@ -0,0 +1,146 @@ +/* + * 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.abs +import kotlin.math.sign +import kotlin.math.sqrt +import kotlin.time.ComparableTimeMark +import kotlin.time.DurationUnit + +/** + * Square-root P + ID (aka SquID) controller. + * + * @param coefficients the [PIDCoefficients] that contains the PID gains + * @param resetIntegralOnZeroCrossover whether to reset the integral term when the error crosses zero + */ +class SquIDController @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 SquIDController with the given coefficients. + * + * @param kP proportional gain, multiplied by the square root of 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 + * @param resetIntegralOnZeroCrossover whether to reset the integral term when the error crosses zero + */ + @JvmOverloads constructor( + kP: Double, + kI: Double = 0.0, + kD: Double = 0.0, + resetIntegralOnZeroCrossover: Boolean = true, + ) : this( + PIDCoefficients(kP, kI, kD), + resetIntegralOnZeroCrossover, + ) + + /** + * Calculates the SquID 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 SquID 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 * sqrt(abs(error)) * error.sign + + 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 * sqrt(abs(error)) * error.sign + + coefficients.kI * errorSum + + coefficients.kD * derivative + } + + /** + * Calculates the SquID 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 SquID output + */ + fun calculate( + timestamp: ComparableTimeMark, + reference: Double, + measured: Double, + measuredDerivative: Double?, + ): Double = calculate( + timestamp, + reference - measured, + measuredDerivative?.let { -it }, + ) + + /** + * Calculates the SquID 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 SquID output + */ + fun calculate( + timestamp: ComparableTimeMark, + reference: Double, + measured: Double, + referenceDerivative: Double, + measuredDerivative: Double, + ): Double = calculate( + timestamp, + reference - measured, + referenceDerivative - measuredDerivative, + ) + + /** + * Resets the SquID controller + */ + fun reset() { + errorSum = 0.0 + lastError = 0.0 + lastTimestamp = null + } +} diff --git a/control/src/main/kotlin/dev/nextftc/control/feedforward/GravityFeedforward.kt b/control/src/main/kotlin/dev/nextftc/control/feedforward/GravityFeedforward.kt new file mode 100644 index 0000000..d7ed6f8 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/feedforward/GravityFeedforward.kt @@ -0,0 +1,101 @@ +/* + * 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.feedforward + +import dev.nextftc.control.model.MotionState +import dev.nextftc.units.unittypes.Inches +import dev.nextftc.units.unittypes.InchesPerSecond +import dev.nextftc.units.unittypes.InchesPerSecondSquared +import kotlin.math.cos +import kotlin.math.sign + +/** + * Parameters for [ElevatorFeedforward] and [ArmFeedforward] + * + * @param kG gravity value, added to overcome gravity + * @param kS static gain, used to overcome static friction (multiplied by the sign of velocity) + * @param kV velocity gain, multiplied by the target velocity + * @param kA acceleration gain, multiplied by the target acceleration + */ +data class GravityFeedforwardParameters @JvmOverloads constructor( + @JvmField var kG: Double = 0.0, + @JvmField var kS: Double = 0.0, + @JvmField var kV: Double = 0.0, + @JvmField var kA: Double = 0.0, +) + +/** + * Feedforward controller for elevator mechanisms. + * + * This feedforward applies a constant gravity compensation term plus velocity and acceleration + * feedforward terms. It is suitable for linear mechanisms like elevators where gravity exerts + * a constant force regardless of position. + * + * @param coefficients the [GravityFeedforwardParameters] containing the feedforward gains + */ +class ElevatorFeedforward(val coefficients: GravityFeedforwardParameters) { + /** + * Calculates the feedforward output for the given velocity and acceleration. + * + * @param velocity the target velocity + * @param acceleration the target acceleration + * @return the feedforward output: `kG + kS * sign(velocity) + kV * velocity + kA * acceleration` + */ + fun calculate(velocity: Double, acceleration: Double): Double = coefficients.kG + + coefficients.kS * velocity.sign + + coefficients.kV * velocity + + coefficients.kA * acceleration + + /** + * Calculates the feedforward output from a [MotionState]. + * + * @param state the target motion state containing velocity and acceleration + * @return the feedforward output + */ + fun calculate(state: MotionState) = calculate( + state.velocity.into(InchesPerSecond), + state.acceleration.into(InchesPerSecondSquared), + ) +} + +/** + * Feedforward controller for arm mechanisms. + * + * This feedforward applies a gravity compensation term that varies with arm angle (using cosine), + * plus velocity and acceleration feedforward terms. It is suitable for rotational mechanisms + * like arms where the gravity torque depends on the arm's angular position. + * + * @param coefficients the [GravityFeedforwardParameters] containing the feedforward gains + */ +class ArmFeedforward(val coefficients: GravityFeedforwardParameters) { + /** + * Calculates the feedforward output for the given position, velocity, and acceleration. + * + * @param position the arm position in radians (0 = horizontal, π/2 = vertical up) + * @param velocity the target angular velocity + * @param acceleration the target angular acceleration + * @return the feedforward output: `kG * cos(position) + kS * sign(velocity) + kV * velocity + kA * acceleration` + */ + fun calculate(position: Double, velocity: Double, acceleration: Double) = coefficients.kG * cos(position) + + coefficients.kS * velocity.sign + + coefficients.kV * velocity + + coefficients.kA * acceleration + + /** + * Calculates the feedforward output from a [MotionState]. + * + * @param state the target motion state containing position, velocity, and acceleration + * @return the feedforward output + */ + fun calculate(state: MotionState) = calculate( + state.position.into(Inches), + state.velocity.into(InchesPerSecond), + state.acceleration.into(InchesPerSecondSquared), + ) +} diff --git a/control/src/main/kotlin/dev/nextftc/control/feedforward/SimpleFeedforward.kt b/control/src/main/kotlin/dev/nextftc/control/feedforward/SimpleFeedforward.kt new file mode 100644 index 0000000..2592ffd --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/feedforward/SimpleFeedforward.kt @@ -0,0 +1,113 @@ +/* + * 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.feedforward + +import dev.nextftc.control.model.MotionState +import dev.nextftc.units.unittypes.InchesPerSecond +import dev.nextftc.units.unittypes.InchesPerSecondSquared +import kotlin.math.sign + +/** + * Coefficients for a simple feedforward controller. + * + * These coefficients model the relationship between desired motion and motor output: + * `output = kS * sign(velocity) + kV * velocity + kA * acceleration` + * + * @property kS Static friction gain. The minimum output needed to overcome static friction + * and start moving. Applied in the direction of desired velocity. + * @property kV Velocity gain. Multiplied by the desired velocity to produce the output + * needed to maintain that velocity against back-EMF and viscous friction. + * @property kA Acceleration gain. Multiplied by the desired acceleration to produce the + * additional output needed to accelerate the system (overcomes inertia). + */ +data class SimpleFFCoefficients @JvmOverloads constructor( + @JvmField var kS: Double, + @JvmField var kV: Double, + @JvmField var kA: Double = 0.0, +) + +/** + * A simple feedforward controller for velocity control. + * + * Feedforward control predicts the motor output needed to achieve a desired motion, + * without waiting for error to accumulate (unlike feedback/PID control). This is + * typically used in combination with a feedback controller for best results. + * + * The feedforward equation is: + * ``` + * output = kS * sign(velocity) + kV * velocity + kA * acceleration + * ``` + * + * Where: + * - `kS` compensates for static friction + * - `kV` compensates for back-EMF and viscous friction + * - `kA` compensates for inertia during acceleration + * + * @param coefficients The [SimpleFFCoefficients] containing kS, kV, and kA gains. + */ +class SimpleFeedforward(val coefficients: SimpleFFCoefficients) { + + /** + * Calculates the feedforward output for a desired velocity and acceleration. + * + * @param velocity The desired velocity. + * @param acceleration The desired acceleration (defaults to 0.0 for constant velocity). + * @return The feedforward output value. + */ + @JvmOverloads + fun calculate(velocity: Double, acceleration: Double = 0.0): Double = + coefficients.kS * velocity.sign + coefficients.kV * velocity + + coefficients.kA * acceleration + + fun calculate(state: MotionState) = calculate( + state.velocity.into(InchesPerSecond), + state.acceleration.into(InchesPerSecondSquared), + ) + + /** + * Calculates the maximum achievable velocity given voltage and acceleration constraints. + * + * @param maxVoltage The maximum voltage (or output) available. + * @param acceleration The current or desired acceleration. + * @return The maximum velocity achievable under these constraints. + */ + fun maxAchievableVelocity(maxVoltage: Double, acceleration: Double): Double = + (maxVoltage - coefficients.kS - acceleration * coefficients.kA) / coefficients.kV + + /** + * Calculates the minimum achievable velocity given voltage and acceleration constraints. + * + * @param maxVoltage The maximum voltage (or output) available. + * @param acceleration The current or desired acceleration. + * @return The minimum (most negative) velocity achievable under these constraints. + */ + fun minAchievableVelocity(maxVoltage: Double, acceleration: Double): Double = + (-maxVoltage + coefficients.kS - acceleration * coefficients.kA) / coefficients.kV + + /** + * Calculates the maximum achievable acceleration given voltage and velocity constraints. + * + * @param maxVoltage The maximum voltage (or output) available. + * @param velocity The current or desired velocity. + * @return The maximum acceleration achievable under these constraints. + */ + fun maxAchievableAcceleration(maxVoltage: Double, velocity: Double): Double = + (maxVoltage - coefficients.kS * velocity.sign - velocity * coefficients.kV) / + coefficients.kA + + /** + * Calculates the minimum achievable acceleration given voltage and velocity constraints. + * + * @param maxVoltage The maximum voltage (or output) available. + * @param velocity The current or desired velocity. + * @return The minimum (most negative) acceleration achievable under these constraints. + */ + fun minAchievableAcceleration(maxVoltage: Double, velocity: Double): Double = + maxAchievableAcceleration(-maxVoltage, velocity) +} diff --git a/control/src/main/kotlin/dev/nextftc/control/filters/KalmanFilter.kt b/control/src/main/kotlin/dev/nextftc/control/filters/KalmanFilter.kt new file mode 100644 index 0000000..f639ab7 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/filters/KalmanFilter.kt @@ -0,0 +1,141 @@ +/* + * 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.filters + +import dev.nextftc.control.model.LinearModel +import dev.nextftc.control.util.discretizeAQ +import dev.nextftc.control.util.discretizeR +import dev.nextftc.control.util.makeCovarianceMatrix +import dev.nextftc.control.util.solveDARE +import dev.nextftc.linalg.Matrix +import dev.nextftc.linalg.Nat +import dev.nextftc.linalg.Vector + +/** + * A Kalman filter for linear state estimation. + * + * The Kalman filter is an optimal state estimator for linear systems with Gaussian noise. + * It combines predictions from a system model with noisy measurements to produce an + * optimal estimate of the system state. + * + * The filter operates on a linear system of the form: + * ``` + * x(k+1) = A * x(k) + B * u(k) + w(k) (state equation) + * y(k) = C * x(k) + D * u(k) + v(k) (measurement equation) + * ``` + * where: + * - `x` is the state vector + * - `u` is the input vector + * - `y` is the measurement vector + * - `w` is process noise with covariance Q + * - `v` is measurement noise with covariance R + * + * @param States The number of states in the system + * @param Inputs The number of inputs to the system + * @param Outputs The number of outputs (measurements) from the system + * @param plant The linear system model describing the plant dynamics + * @param stateStdDevs Standard deviations of the state model (process noise) + * @param measurementStdDevs Standard deviations of the measurements (measurement noise) + * @param dt The discretization timestep in seconds (default 0.05s / 50ms) + */ +class KalmanFilter @JvmOverloads constructor( + val plant: LinearModel, + val stateStdDevs: Vector, + val measurementStdDevs: Vector, + dt: Double = 0.05, +) { + private val A: Matrix + private val Q: Matrix + private val R: Matrix + private val initialP: Matrix + + private var P: Matrix + + private var Xhat = Vector.zero(plant.A.natRows) + + init { + require(dt > 0) { "Time step (dt) must be positive" } + + val Qcont = makeCovarianceMatrix(stateStdDevs) + val Rcont = makeCovarianceMatrix(measurementStdDevs) + + val AQ = discretizeAQ(plant.A, Qcont, dt) + A = AQ.first + Q = AQ.second + + R = discretizeR(Rcont, dt) + + initialP = solveDARE(A.transpose, plant.C.transpose, Q, R) + P = initialP + } + + /** + * Resets the filter to its initial state. + * + * Sets the state estimate to zero and resets the error covariance matrix + * to its initial value computed from the DARE solution. + */ + fun reset() { + Xhat = Vector.zero(plant.A.natRows) + P = initialP + } + + /** + * Performs the prediction step of the Kalman filter. + * + * This propagates the state estimate forward in time using the system model: + * ``` + * x̂(k+1|k) = A * x̂(k|k) + B * u(k) + * P(k+1|k) = A * P(k|k) * Aᵀ + Q + * ``` + * + * @param inputs The current input vector u(k) + * @return The predicted state estimate x̂(k+1|k) + */ + fun predict(inputs: Vector): Vector { + Xhat = plant.derivative(Xhat, inputs) + + P = A * P * A.transpose + Q + + return Xhat + } + + /** + * Performs the correction (update) step of the Kalman filter. + * + * This incorporates a new measurement to refine the state estimate using + * the Joseph form of the covariance update for numerical stability: + * ``` + * S = C * P * Cᵀ + R (innovation covariance) + * K = P * Cᵀ * S⁻¹ (Kalman gain) + * x̂(k|k) = x̂(k|k-1) + K * (y - D * u) (state update) + * P(k|k) = (I - K*C) * P * (I - K*C)ᵀ + K*R*Kᵀ (covariance update) + * ``` + * + * @param inputs The current input vector u(k) + * @param outputs The measurement vector y(k) + * @return The corrected state estimate x̂(k|k) + */ + fun correct(inputs: Vector, outputs: Vector): Vector { + val S = plant.C * P * plant.C.transpose + R + + val K = S.solve(plant.C * P).transpose + + Xhat += Vector(K * (outputs - (plant.D * inputs))) + + // (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ + P = (Matrix.identity(K.natRows) - K * plant.C) * P * + (Matrix.identity(K.natRows) - K * plant.C).transpose + + K * R * K.transpose + + return Xhat + } +} diff --git a/control/src/main/kotlin/dev/nextftc/control/model/Model.kt b/control/src/main/kotlin/dev/nextftc/control/model/Model.kt new file mode 100644 index 0000000..4922d5b --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/model/Model.kt @@ -0,0 +1,42 @@ +/* + * 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.model + +import dev.nextftc.control.util.discretizeAB +import dev.nextftc.linalg.Matrix +import dev.nextftc.linalg.Nat +import dev.nextftc.linalg.Vector + +interface Model { + fun derivative(state: Vector, input: Vector): Vector + + fun output(state: Vector, input: Vector): Vector +} + +@Suppress("PropertyName") +class LinearModel @JvmOverloads constructor( + val A: Matrix, + val B: Matrix, + val C: Matrix, + val D: Matrix, + val dt: Double = 0.05, +) : Model { + private val Ad: Matrix + private val Bd: Matrix + + init { + val (Ad, Bd) = discretizeAB(A, B, 0.05) + this.Ad = Ad + this.Bd = Bd + } + + override fun derivative(state: Vector, input: Vector): Vector = Vector(Ad * state + Bd * input) + + override fun output(state: Vector, input: Vector): Vector = Vector(C * state + D * input) +} diff --git a/control/src/main/kotlin/dev/nextftc/control/model/MotionState.kt b/control/src/main/kotlin/dev/nextftc/control/model/MotionState.kt new file mode 100644 index 0000000..47b572d --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/model/MotionState.kt @@ -0,0 +1,51 @@ +/* + * 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.model + +import dev.nextftc.units.measuretypes.Distance +import dev.nextftc.units.measuretypes.LinearAcceleration +import dev.nextftc.units.measuretypes.LinearVelocity +import dev.nextftc.units.unittypes.inches +import dev.nextftc.units.unittypes.inchesPerSecond +import dev.nextftc.units.unittypes.inchesPerSecondSquared + +/** + * The state of an object. + * + * @property position The position of the profile. + * @property velocity The velocity of the profile. + * @property acceleration The acceleration of the profile. + */ +data class MotionState @JvmOverloads constructor( + val position: Distance = 0.0.inches, + val velocity: LinearVelocity = 0.0.inchesPerSecond, + val acceleration: LinearAcceleration = 0.0.inchesPerSecondSquared, +) { + + /** + * Creates a MotionState with the given position, velocity, and acceleration. + * + * @param position Position, in inches + * @param velocity Velocity, in inches per second + * @param acceleration Acceleration, in inches per second squared + */ + constructor( + position: Double = 0.0, + velocity: Double = 0.0, + acceleration: Double = 0.0, + ) : this( + position.inches, + velocity.inchesPerSecond, + acceleration.inchesPerSecondSquared, + ) + + companion object { + val ZERO = MotionState(0.0, 0.0, 0.0) + } +} diff --git a/control/src/main/kotlin/dev/nextftc/control/profiles/TrapezoidProfile.kt b/control/src/main/kotlin/dev/nextftc/control/profiles/TrapezoidProfile.kt new file mode 100644 index 0000000..efb6a8d --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/profiles/TrapezoidProfile.kt @@ -0,0 +1,270 @@ +/* + * 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.profiles + +import dev.nextftc.control.model.MotionState +import dev.nextftc.units.measuretypes.Distance +import dev.nextftc.units.measuretypes.LinearAcceleration +import dev.nextftc.units.measuretypes.LinearVelocity +import dev.nextftc.units.unittypes.inchesPerSecond +import dev.nextftc.units.unittypes.inchesPerSecondSquared +import kotlin.math.abs +import kotlin.math.max +import kotlin.math.sqrt +import kotlin.math.withSign +import kotlin.time.Duration +import kotlin.time.DurationUnit + +/** + * Constraints for a trapezoidal motion profile. + * + * @property maxVelocity The maximum velocity of the profile. + * @property maxAcceleration The maximum acceleration of the profile. + */ +data class TrapezoidProfileConstraints(val maxVelocity: LinearVelocity, val maxAcceleration: LinearAcceleration) { + init { + require(maxVelocity.magnitude >= 0.0) { "Constraints must be non-negative" } + require(maxAcceleration.magnitude >= 0.0) { "Constraints must be non-negative" } + } + + constructor( + maxVelocity: Double, + maxAcceleration: Double, + ) : this( + maxVelocity.inchesPerSecond, + maxAcceleration.inchesPerSecondSquared, + ) +} + +/** + * A trapezoidal motion profile generator. + * + * A trapezoidal motion profile is a velocity profile that accelerates at a constant rate, + * maintains a constant velocity, then decelerates at a constant rate. This creates a + * trapezoid shape when velocity is plotted over time. + * + * The profile handles truncated motion profiles (with nonzero initial or final velocity) + * and profiles that never reach maximum velocity (triangular profiles). + * + * @param constraints The [TrapezoidProfileConstraints] that define the maximum velocity and + * acceleration for the profile. + */ +class TrapezoidProfile(private val constraints: TrapezoidProfileConstraints) { + private val maxAccel = constraints.maxAcceleration.magnitude + + private var direction = 0 + + private var currentState = MotionState.ZERO + + private var endAccel = 0.0 + private var endVel = 0.0 + private var endDecel = 0.0 + + /** + * The total time required to complete the motion profile. + */ + val totalTime: Double + get() = endDecel + + /** + * Calculates the state of the profile at a given time. + * + * @param t The time since the beginning of the profile + * @param current The current state of the system. + * @param goal The desired goal state. + * + * @return The state of the profile at time [t]. + */ + fun calculate(t: Duration, current: MotionState, goal: MotionState): MotionState { + direction = if (shouldFlipAcceleration(current, goal)) -1 else 1 + currentState = direct(current) + val directGoal = direct(goal) + + val timeSeconds = t.toDouble(DurationUnit.SECONDS) + + if (currentState.velocity.absoluteValue > constraints.maxVelocity) { + currentState = + currentState.copy( + velocity = constraints.maxVelocity.withSign( + currentState.velocity, + ) as LinearVelocity, + ) + } + + // Deal with a possibly truncated motion profile (with nonzero initial or + // final velocity) by calculating the parameters as if the profile began and + // ended at zero velocity + val cutoffBegin = currentState.velocity.magnitude / maxAccel + val cutoffDistBegin = cutoffBegin * cutoffBegin * maxAccel / 2.0 + + val cutoffEnd = directGoal.velocity.magnitude / maxAccel + val cutoffDistEnd = cutoffEnd * cutoffEnd * maxAccel / 2.0 + + // Now we can calculate the parameters as if it was a full trapezoid instead + // of a truncated one + val fullTrapezoidDist = + cutoffDistBegin + (directGoal.position.magnitude - currentState.position.magnitude) + + cutoffDistEnd + var accelerationTime = constraints.maxVelocity.magnitude / maxAccel + + var fullSpeedDist = + fullTrapezoidDist - accelerationTime * accelerationTime * maxAccel + + // Handle the case where the profile never reaches full speed + if (fullSpeedDist < 0) { + accelerationTime = sqrt(fullTrapezoidDist / maxAccel) + fullSpeedDist = 0.0 + } + + endAccel = accelerationTime - cutoffBegin + endVel = endAccel + fullSpeedDist / constraints.maxVelocity.magnitude + endDecel = endVel + accelerationTime - cutoffEnd + + val position: Double + val velocity: Double + val accel: Double + + if (timeSeconds < endAccel) { + velocity = currentState.velocity.magnitude + timeSeconds * maxAccel + position = + currentState.position.magnitude + + (currentState.velocity.magnitude + timeSeconds * maxAccel / 2.0) * + timeSeconds + accel = maxAccel + } else if (timeSeconds < endVel) { + velocity = constraints.maxVelocity.magnitude + position = currentState.position.magnitude + + ( + (currentState.velocity.magnitude + endAccel * maxAccel / 2.0) * + endAccel + + constraints.maxVelocity.magnitude * (timeSeconds - endAccel) + ) + accel = 0.0 + } else if (timeSeconds <= endDecel) { + velocity = directGoal.velocity.magnitude + (endDecel - timeSeconds) * maxAccel + val timeLeft = endDecel - timeSeconds + position = + directGoal.position.magnitude - + (directGoal.velocity.magnitude + timeLeft * maxAccel / 2.0) * timeLeft + accel = -maxAccel + } else { + velocity = directGoal.velocity.magnitude + position = directGoal.position.magnitude + accel = 0.0 + } + + return direct(MotionState(position, velocity, accel)) + } + + /** + * Calculates the time remaining until the profile reaches a target position. + * + * @param target The target position to reach. + * + * @return The time remaining until the target is reached, in seconds. + */ + fun timeLeftUntil(target: Double): Double { + val position = currentState.position.magnitude * direction + var velocity = currentState.velocity.magnitude * direction + + var endAccel = endAccel * direction + var endFullSpeed = endVel * direction - endAccel + + if (target < position) { + endAccel = -endAccel + endFullSpeed = -endFullSpeed + velocity = -velocity + } + + endAccel = max(endAccel, 0.0) + endFullSpeed = max(endFullSpeed, 0.0) + + val acceleration = maxAccel + val deceleration = -maxAccel + + val distToTarget = abs(target - position) + if (distToTarget < 1e-6) { + return 0.0 + } + + var accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel + + val decelVelocity: Double = if (endAccel > 0) { + sqrt(abs(velocity * velocity + 2 * acceleration * accelDist)) + } else { + velocity + } + + var fullSpeedDist = constraints.maxVelocity.magnitude * endFullSpeed + val decelDist: Double + + if (accelDist > distToTarget) { + accelDist = distToTarget + fullSpeedDist = 0.0 + decelDist = 0.0 + } else if (accelDist + fullSpeedDist > distToTarget) { + fullSpeedDist = distToTarget - accelDist + decelDist = 0.0 + } else { + decelDist = distToTarget - fullSpeedDist - accelDist + } + + val accelTime = + (-velocity + sqrt(abs(velocity * velocity + 2 * acceleration * accelDist))) / + acceleration + + val decelTime = + ( + -decelVelocity + + sqrt(abs(decelVelocity * decelVelocity + 2 * deceleration * decelDist)) + ) / + deceleration + + val fullSpeedTime = fullSpeedDist / constraints.maxVelocity.magnitude + + return accelTime + fullSpeedTime + decelTime + } + + /** + * Checks if the profile has finished at the given time. + * + * @param t The time since the beginning of the profile, in seconds. + * + * @return true if the profile has finished, false otherwise. + */ + fun isFinished(t: Double): Boolean = t >= totalTime + + /** + * Flips the sign of the velocity and position if the profile is inverted. + * Used internally to handle backward motion. + * + * @param state The state to transform. + * + * @return The transformed state with signs adjusted based on [direction]. + */ + private fun direct(state: MotionState): MotionState { + val position = state.position * direction.toDouble() + val velocity = state.velocity * direction.toDouble() + val acceleration = state.acceleration * direction.toDouble() + return MotionState(position, velocity, acceleration) + } + + companion object { + /** + * Determines if the acceleration should be flipped based on the initial and goal states. + * + * @param initial The initial state. + * @param goal The goal state. + * + * @return true if the goal position is less than the initial position, false otherwise. + */ + private fun shouldFlipAcceleration(initial: MotionState, goal: MotionState): Boolean = + initial.position > goal.position + } +} diff --git a/control/src/main/kotlin/dev/nextftc/control/util/Discretization.kt b/control/src/main/kotlin/dev/nextftc/control/util/Discretization.kt new file mode 100644 index 0000000..9501138 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/util/Discretization.kt @@ -0,0 +1,129 @@ +/* + * 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.util + +import dev.nextftc.linalg.DynamicMatrix +import dev.nextftc.linalg.Matrix +import dev.nextftc.linalg.Nat +import dev.nextftc.linalg.exp + +/** + * Discretizes a continuous-time system (A, B) to a discrete-time system (Ad, Bd). + * Uses the matrix exponential (approximated with a Taylor series). + * Ad = e^(A*dt) + * Bd = (∫ e^(Aτ) dτ from 0 to dt) * B ≈ (I*dt + A*dt²/2! + ...) * B + */ +internal fun discretizeAB( + A: Matrix, + B: Matrix, + dt: Double, + taylorTerms: Int = 10, +): Pair, Matrix> { + val n = A.numRows + var Ad = Matrix.Companion.identity(A.natRows) + var Bd_integral = Matrix.Companion.identity(B.natRows).times(dt) // Start with I*dt + + var APowerDt = A.times(dt) + var dtPower = dt + var factorial = 1.0 + + // Taylor series for e^(A*dt) and its integral + for (i in 1..taylorTerms) { + // Ad term: (A*dt)^i / i! + Ad = Ad.plus(APowerDt.times(1.0 / factorial)) + + // Bd integral term: A^(i-1) * dt^(i+1) / (i+1)! + dtPower *= dt + factorial *= (i + 1) + Bd_integral = Bd_integral.plus(APowerDt.times(dt / (i + 1))) + + // Prepare for next iteration + APowerDt = APowerDt.times(A.times(dt)) + factorial *= (i + 1) + } + + val Bd = Bd_integral.times(B) + return Pair(Ad, Bd) +} + +/** + * Discretizes the given continuous A and Q matrices. + * + * @param States Nat representing the number of states. + * @param A Continuous system matrix. + * @param Q Continuous process noise covariance matrix. + * @param dt Discretization timestep in seconds. + * @return a pair representing the discrete system matrix and process noise covariance matrix. +*/ +internal fun discretizeAQ( + A: Matrix, + Q: Matrix, + dt: Double, +): Pair, Matrix> { + val states = A.numRows + + // Make continuous Q symmetric if it isn't already + val Qsym = (Q + Q.transpose) * 0.5 + + // M = [−A Q ] + // [ 0 Aᵀ] + val M = DynamicMatrix.zero(2 * states, 2 * states) + + // Assign block (0, 0): -A + for (i in 0 until states) { + for (j in 0 until states) { + M[i, j] = -A[i, j] + } + } + + // Assign block (0, states): Q + for (i in 0 until states) { + for (j in 0 until states) { + M[i, j + states] = Qsym[i, j] + } + } + + // Block (states, 0) is already zero + + // Assign block (states, states): Aᵀ + for (i in 0 until states) { + for (j in 0 until states) { + M[i + states, j + states] = A[j, i] // Transpose + } + } + + // ϕ = e^(M*dt) + val phi = (M * dt).exp() + + // ϕ₁₂ = A_d⁻¹Q_d (block at row 0, col states) + val phi12 = phi.slice(0, states, states, 2 * states) + + // ϕ₂₂ = A_dᵀ (block at row states, col states) + val phi22 = phi.slice(states, 2 * states, states, 2 * states) + + // A_d = ϕ₂₂ᵀ + val discA = phi22.transpose + + // Q_d = A_d * ϕ₁₂ + var discQ = discA * phi12 + + // Make discrete Q symmetric if it isn't already + discQ = (discQ + discQ.transpose) * 0.5 + + // Convert back to sized matrices + @Suppress("UNCHECKED_CAST") + return Pair( + discA.toSizedMatrix(A.natRows, A.natColumns), + discQ.toSizedMatrix(Q.natRows, Q.natColumns), + ) +} + +internal fun discretizeR(R: Matrix, dt: Double): Matrix = R / dt diff --git a/control/src/main/kotlin/dev/nextftc/control/util/StateSpaceUtil.kt b/control/src/main/kotlin/dev/nextftc/control/util/StateSpaceUtil.kt new file mode 100644 index 0000000..2feb9f4 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/util/StateSpaceUtil.kt @@ -0,0 +1,85 @@ +/* + * 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.util + +import dev.nextftc.linalg.Matrix +import dev.nextftc.linalg.Nat +import dev.nextftc.linalg.Vector +import kotlin.math.pow + +internal fun makeBrysonMatrix(tolerances: Vector): Matrix { + val matrix = Matrix.zero(tolerances.natRows, tolerances.natRows) + + for (i in 0 until tolerances.numRows) { + if (tolerances[i].isInfinite()) { + matrix[i, i] = 0.0 + } else { + matrix[i, i] = 1.0 / (tolerances[i].pow(2)) + } + } + + return matrix +} + +internal fun makeCovarianceMatrix(tolerances: Vector): Matrix { + val matrix = Matrix.zero(tolerances.natRows, tolerances.natRows) + + for (i in 0 until tolerances.numRows) { + matrix[i, i] = tolerances[i].pow(2) + } + + return matrix +} + +/** + * Solves the Discrete-Time Algebraic Riccati Equation (DARE) using iterative method. + * P = A'PA - (A'PB)(R + B'PB)⁻¹(B'PA) + Q + * + * @author Tyler Veness (C++ implementation) + * @author Zach Harel (Kotlin implementation) + */ +internal fun solveDARE( + Ad: Matrix, + Bd: Matrix, + Q: Matrix, + R: Matrix, + maxIter: Int = -1, + epsilon: Double = 1e-6, +): Matrix { + // Initialize matrices + var A_K = Ad.copy() + + // Calculate G_k = B * R^-1 * B^T using Cholesky decomposition + // Equivalent to B * R.llt().solve(B.transpose()) + var G_K = Bd * R.solve(Bd.transpose) + + var H_K1 = Q.copy() + var H_K: Matrix + + var i = 0 + + do { + H_K = H_K1.copy() + + val W = Matrix.Companion.identity(H_K.natRows) + G_K * H_K + + val v1 = W.solve(A_K) + val v2 = W.solve(G_K.transpose).transpose + + G_K = (G_K + A_K * v2 * A_K.transpose) + + H_K1 += v1.transpose * H_K * A_K + + A_K *= v1 + } while ((H_K1 - H_K).norm > epsilon * H_K1.norm && (maxIter < 0 || i++ < maxIter)) + + return H_K1 +} diff --git a/control/src/test/kotlin/dev/nextftc/control/feedback/PIDControllerTest.kt b/control/src/test/kotlin/dev/nextftc/control/feedback/PIDControllerTest.kt new file mode 100644 index 0000000..0b4a0b8 --- /dev/null +++ b/control/src/test/kotlin/dev/nextftc/control/feedback/PIDControllerTest.kt @@ -0,0 +1,160 @@ +/* + * 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 io.kotest.core.spec.style.FunSpec +import io.kotest.matchers.doubles.plusOrMinus +import io.kotest.matchers.shouldBe +import kotlin.time.Duration.Companion.milliseconds +import kotlin.time.TestTimeSource + +class PIDControllerTest : + FunSpec({ + context("PIDCoefficients") { + test("default values for kI and kD are zero") { + val coefficients = PIDCoefficients(1.0) + coefficients.kP shouldBe 1.0 + coefficients.kI shouldBe 0.0 + coefficients.kD shouldBe 0.0 + } + + test("all values can be set") { + val coefficients = PIDCoefficients(1.0, 2.0, 3.0) + coefficients.kP shouldBe 1.0 + coefficients.kI shouldBe 2.0 + coefficients.kD shouldBe 3.0 + } + + test("values are mutable") { + val coefficients = PIDCoefficients(1.0) + coefficients.kP = 5.0 + coefficients.kI = 6.0 + coefficients.kD = 7.0 + coefficients.kP shouldBe 5.0 + coefficients.kI shouldBe 6.0 + coefficients.kD shouldBe 7.0 + } + } + + context("PIDController construction") { + test("can be constructed with coefficients object") { + val coefficients = PIDCoefficients(1.0, 2.0, 3.0) + val controller = PIDController(coefficients) + controller.coefficients shouldBe coefficients + } + + test("can be constructed with individual values") { + val controller = PIDController(1.0, 2.0, 3.0) + controller.coefficients.kP shouldBe 1.0 + controller.coefficients.kI shouldBe 2.0 + controller.coefficients.kD shouldBe 3.0 + } + + test("default values work") { + val controller = PIDController(1.0) + controller.coefficients.kP shouldBe 1.0 + controller.coefficients.kI shouldBe 0.0 + controller.coefficients.kD shouldBe 0.0 + controller.resetIntegralOnZeroCrossover shouldBe true + } + + test("resetIntegralOnZeroCrossover can be disabled") { + val controller = PIDController(1.0, resetIntegralOnZeroCrossover = false) + controller.resetIntegralOnZeroCrossover shouldBe false + } + } + + context("PIDController P term") { + test("proportional output is kP * error") { + val timeSource = TestTimeSource() + val controller = PIDController(2.0, 0.0, 0.0) + + val output = controller.calculate(timeSource.markNow(), 5.0, 0.0) + output shouldBe (2.0 * 5.0 plusOrMinus 0.001) + } + + test("negative error produces negative output") { + val timeSource = TestTimeSource() + val controller = PIDController(2.0, 0.0, 0.0) + + val output = controller.calculate(timeSource.markNow(), -5.0, 0.0) + output shouldBe (-10.0 plusOrMinus 0.001) + } + } + + context("PIDController D term") { + test("derivative output uses provided errorDerivative") { + val timeSource = TestTimeSource() + val controller = PIDController(0.0, 0.0, 3.0) + + val output = controller.calculate(timeSource.markNow(), 0.0, 2.0) + output shouldBe (6.0 plusOrMinus 0.001) + } + + test("derivative output is kD * errorDerivative") { + val timeSource = TestTimeSource() + val controller = PIDController(0.0, 0.0, 5.0) + + val output = controller.calculate(timeSource.markNow(), 0.0, -3.0) + output shouldBe (-15.0 plusOrMinus 0.001) + } + } + + context("PIDController combined terms") { + test("P and D terms combine correctly") { + val timeSource = TestTimeSource() + val controller = PIDController(2.0, 0.0, 3.0) + + // P: 2 * 5 = 10, D: 3 * 2 = 6, Total: 16 + val output = controller.calculate(timeSource.markNow(), 5.0, 2.0) + output shouldBe (16.0 plusOrMinus 0.001) + } + } + + context("PIDController reset") { + test("reset clears internal state") { + val timeSource = TestTimeSource() + val controller = PIDController(1.0, 1.0, 1.0) + + // Make a calculation to set internal state + controller.calculate(timeSource.markNow(), 5.0, 1.0) + timeSource += 10.milliseconds + + // Reset + controller.reset() + + // After reset, should behave like a fresh controller + val output = controller.calculate(timeSource.markNow(), 5.0, 1.0) + // First call after reset: P = 5, I = 0 (no time passed since reset), D = 1 + output shouldBe (6.0 plusOrMinus 0.001) + } + } + + context("PIDController reference/measured overloads") { + test("calculate with reference and measured computes error correctly") { + val timeSource = TestTimeSource() + val controller = PIDController(2.0, 0.0, 0.0) + + // reference - measured = 10 - 3 = 7, P = 2 * 7 = 14 + // Pass 0.0 for measuredDerivative since we're only testing P term + val output = controller.calculate(timeSource.markNow(), 10.0, 3.0, 0.0) + output shouldBe (14.0 plusOrMinus 0.001) + } + + test("calculate with reference and measured derivatives") { + val timeSource = TestTimeSource() + val controller = PIDController(0.0, 0.0, 3.0) + + // errorDerivative = refDeriv - measDeriv = 5 - 2 = 3 + // D = 3 * 3 = 9 + val output = controller.calculate(timeSource.markNow(), 0.0, 0.0, 5.0, 2.0) + output shouldBe (9.0 plusOrMinus 0.001) + } + } + }) diff --git a/control/src/test/kotlin/dev/nextftc/control/feedback/SquIDControllerTest.kt b/control/src/test/kotlin/dev/nextftc/control/feedback/SquIDControllerTest.kt new file mode 100644 index 0000000..52485ac --- /dev/null +++ b/control/src/test/kotlin/dev/nextftc/control/feedback/SquIDControllerTest.kt @@ -0,0 +1,188 @@ +/* + * 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 io.kotest.core.spec.style.FunSpec +import io.kotest.matchers.doubles.plusOrMinus +import io.kotest.matchers.shouldBe +import kotlin.time.Duration.Companion.milliseconds +import kotlin.time.TestTimeSource + +class SquIDControllerTest : + FunSpec({ + context("SquIDController construction") { + test("can be constructed with coefficients object") { + val coefficients = PIDCoefficients(1.0, 2.0, 3.0) + val controller = SquIDController(coefficients) + controller.coefficients shouldBe coefficients + } + + test("can be constructed with individual values") { + val controller = SquIDController(1.0, 2.0, 3.0) + controller.coefficients.kP shouldBe 1.0 + controller.coefficients.kI shouldBe 2.0 + controller.coefficients.kD shouldBe 3.0 + } + + test("default values work") { + val controller = SquIDController(1.0) + controller.coefficients.kP shouldBe 1.0 + controller.coefficients.kI shouldBe 0.0 + controller.coefficients.kD shouldBe 0.0 + controller.resetIntegralOnZeroCrossover shouldBe true + } + + test("resetIntegralOnZeroCrossover can be disabled") { + val controller = SquIDController(1.0, resetIntegralOnZeroCrossover = false) + controller.resetIntegralOnZeroCrossover shouldBe false + } + } + + context("SquIDController P term (square root)") { + test("proportional output is kP * sqrt(|error|) * sign(error) for positive error") { + val timeSource = TestTimeSource() + val controller = SquIDController(2.0, 0.0, 0.0) + + // P = 2.0 * sqrt(4.0) * 1 = 2.0 * 2.0 = 4.0 + val output = controller.calculate( + timeSource.markNow(), + 4.0, + 0.0, + ) + output shouldBe (4.0 plusOrMinus 0.001) + } + + test("proportional output for negative error") { + val timeSource = TestTimeSource() + val controller = SquIDController(2.0, 0.0, 0.0) + + // P = 2.0 * sqrt(|-4.0|) * (-1) = 2.0 * 2.0 * (-1) = -4.0 + val output = controller.calculate(timeSource.markNow(), -4.0, 0.0) + output shouldBe (-4.0 plusOrMinus 0.001) + } + + test("sqrt behavior differs from linear PID") { + val timeSource = TestTimeSource() + val squidController = SquIDController(1.0, 0.0, 0.0) + val pidController = PIDController(1.0, 0.0, 0.0) + + val error = 9.0 + val squidOutput = squidController.calculate(timeSource.markNow(), error, 0.0) + val pidOutput = pidController.calculate(timeSource.markNow(), error, 0.0) + + // SquID: 1.0 * sqrt(9) * 1 = 3.0 + // PID: 1.0 * 9 = 9.0 + squidOutput shouldBe (3.0 plusOrMinus 0.001) + pidOutput shouldBe (9.0 plusOrMinus 0.001) + } + + test("zero error produces zero output") { + val timeSource = TestTimeSource() + val controller = SquIDController(2.0, 0.0, 0.0) + + val output = controller.calculate(timeSource.markNow(), 0.0, 0.0) + output shouldBe (0.0 plusOrMinus 0.001) + } + + test("small error has proportionally larger response than linear PID") { + val timeSource = TestTimeSource() + val squidController = SquIDController(1.0, 0.0, 0.0) + val pidController = PIDController(1.0, 0.0, 0.0) + + val smallError = 0.01 + val squidOutput = squidController.calculate(timeSource.markNow(), smallError, 0.0) + val pidOutput = pidController.calculate(timeSource.markNow(), smallError, 0.0) + + // SquID: sqrt(0.01) = 0.1 + // PID: 0.01 + // SquID response is 10x larger for small errors + squidOutput shouldBe (0.1 plusOrMinus 0.001) + pidOutput shouldBe (0.01 plusOrMinus 0.001) + } + } + + context("SquIDController D term") { + test("derivative output uses provided errorDerivative") { + val timeSource = TestTimeSource() + val controller = SquIDController(0.0, 0.0, 3.0) + + val output = controller.calculate(timeSource.markNow(), 0.0, 2.0) + output shouldBe (6.0 plusOrMinus 0.001) + } + + test("derivative term is linear (not sqrt)") { + val timeSource = TestTimeSource() + val controller = SquIDController(0.0, 0.0, 5.0) + + val output = controller.calculate(timeSource.markNow(), 0.0, -3.0) + output shouldBe (-15.0 plusOrMinus 0.001) + } + } + + context("SquIDController combined terms") { + test("P (sqrt) and D (linear) terms combine correctly") { + val timeSource = TestTimeSource() + val controller = SquIDController(2.0, 0.0, 3.0) + + // P: 2 * sqrt(4) * 1 = 4, D: 3 * 2 = 6, Total: 10 + val output = controller.calculate(timeSource.markNow(), 4.0, 2.0) + output shouldBe (10.0 plusOrMinus 0.001) + } + } + + context("SquIDController reset") { + test("reset clears internal state") { + val timeSource = TestTimeSource() + val controller = SquIDController(1.0, 1.0, 1.0) + + controller.calculate(timeSource.markNow(), 4.0, 1.0) + timeSource += 10.milliseconds + controller.reset() + + // After reset, should behave like fresh controller + val output = controller.calculate(timeSource.markNow(), 4.0, 1.0) + // First call after reset: P = sqrt(4) = 2, I = 0, D = 1 + output shouldBe (3.0 plusOrMinus 0.001) + } + } + + context("SquIDController reference/measured overloads") { + test("calculate with reference and measured computes error correctly") { + val timeSource = TestTimeSource() + val controller = SquIDController(1.0, 0.0, 0.0) + + // reference - measured = 16 - 7 = 9 + // P = 1 * sqrt(9) * 1 = 3 + // Pass 0.0 for measuredDerivative since we're only testing P term + val output = controller.calculate( + timeSource.markNow(), + 16.0, + 7.0, + 0.0, + ) + output shouldBe (3.0 plusOrMinus 0.001) + } + + test("calculate with reference and measured derivatives") { + val timeSource = TestTimeSource() + val controller = SquIDController(0.0, 0.0, 3.0) + + // errorDerivative = refDeriv - measDeriv = 5 - 2 = 3 + // D = 3 * 3 = 9 + val output = controller.calculate( + timeSource.markNow(), + 0.0, + 0.0, + 5.0, + 2.0, + ) + output shouldBe (9.0 plusOrMinus 0.001) + } + } + }) diff --git a/control/src/test/kotlin/dev/nextftc/control/feedforward/GravityFeedforwardTest.kt b/control/src/test/kotlin/dev/nextftc/control/feedforward/GravityFeedforwardTest.kt new file mode 100644 index 0000000..f9b75ae --- /dev/null +++ b/control/src/test/kotlin/dev/nextftc/control/feedforward/GravityFeedforwardTest.kt @@ -0,0 +1,162 @@ +/* + * 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.feedforward + +import io.kotest.core.spec.style.FunSpec +import io.kotest.matchers.doubles.plusOrMinus +import io.kotest.matchers.shouldBe +import kotlin.math.PI +import kotlin.math.sqrt + +class GravityFeedforwardTest : + FunSpec({ + context("GravityFeedforwardParameters") { + test("default values are zero") { + val params = GravityFeedforwardParameters() + params.kG shouldBe 0.0 + params.kS shouldBe 0.0 + params.kV shouldBe 0.0 + params.kA shouldBe 0.0 + } + + test("all values can be set via constructor") { + val params = GravityFeedforwardParameters(1.0, 2.0, 3.0, 4.0) + params.kG shouldBe 1.0 + params.kS shouldBe 2.0 + params.kV shouldBe 3.0 + params.kA shouldBe 4.0 + } + + test("values are mutable") { + val params = GravityFeedforwardParameters() + params.kG = 5.0 + params.kS = 6.0 + params.kV = 7.0 + params.kA = 8.0 + params.kG shouldBe 5.0 + params.kS shouldBe 6.0 + params.kV shouldBe 7.0 + params.kA shouldBe 8.0 + } + } + + context("ElevatorFeedforward construction") { + test("can be constructed with coefficients object") { + val coefficients = GravityFeedforwardParameters(1.0, 2.0, 3.0, 4.0) + val feedforward = ElevatorFeedforward(coefficients) + feedforward.coefficients shouldBe coefficients + } + } + + context("ElevatorFeedforward calculate") { + test("calculates correctly with positive velocity") { + val feedforward = + ElevatorFeedforward(GravityFeedforwardParameters(0.5, 0.2, 2.0, 0.1)) + // output = kG + kS * sign(velocity) + kV * velocity + kA * acceleration + // output = 0.5 + 0.2 * 1 + 2.0 * 3.0 + 0.1 * 0.5 = 0.5 + 0.2 + 6.0 + 0.05 = 6.75 + feedforward.calculate(3.0, 0.5) shouldBe (6.75 plusOrMinus 0.001) + } + + test("calculates correctly with negative velocity") { + val feedforward = + ElevatorFeedforward(GravityFeedforwardParameters(0.5, 0.2, 2.0, 0.1)) + // output = kG + kS * sign(velocity) + kV * velocity + kA * acceleration + // output = 0.5 + 0.2 * (-1) + 2.0 * (-3.0) + 0.1 * 0.5 = 0.5 - 0.2 - 6.0 + 0.05 = -5.65 + feedforward.calculate(-3.0, 0.5) shouldBe (-5.65 plusOrMinus 0.001) + } + + test("calculates correctly with zero velocity") { + val feedforward = + ElevatorFeedforward(GravityFeedforwardParameters(0.5, 0.2, 2.0, 0.1)) + // output = kG + kS * sign(0) + kV * 0 + kA * acceleration + // output = 0.5 + 0 + 0 + 0.1 * 2.0 = 0.7 + feedforward.calculate(0.0, 2.0) shouldBe (0.7 plusOrMinus 0.001) + } + + test("gravity term is constant regardless of velocity") { + val feedforward = + ElevatorFeedforward(GravityFeedforwardParameters(1.5, 0.0, 0.0, 0.0)) + feedforward.calculate(0.0, 0.0) shouldBe 1.5 + feedforward.calculate(10.0, 0.0) shouldBe 1.5 + feedforward.calculate(-10.0, 0.0) shouldBe 1.5 + } + } + + context("ArmFeedforward construction") { + test("can be constructed with coefficients object") { + val coefficients = GravityFeedforwardParameters(1.0, 2.0, 3.0, 4.0) + val feedforward = ArmFeedforward(coefficients) + feedforward.coefficients shouldBe coefficients + } + } + + context("ArmFeedforward calculate") { + test("calculates correctly at horizontal position (0 radians)") { + val feedforward = ArmFeedforward(GravityFeedforwardParameters(1.0, 0.2, 2.0, 0.1)) + // position = 0, cos(0) = 1 + // output = kG * cos(position) + kS * sign(velocity) + kV * velocity + kA * acceleration + // output = 1.0 * 1 + 0.2 * 1 + 2.0 * 3.0 + 0.1 * 0.5 = 1.0 + 0.2 + 6.0 + 0.05 = 7.25 + feedforward.calculate(0.0, 3.0, 0.5) shouldBe (7.25 plusOrMinus 0.001) + } + + test("calculates correctly at vertical position (π/2 radians)") { + val feedforward = ArmFeedforward(GravityFeedforwardParameters(1.0, 0.2, 2.0, 0.1)) + // position = π/2, cos(π/2) ≈ 0 + // output = kG * cos(position) + kS * sign(velocity) + kV * velocity + kA * acceleration + // output = 1.0 * 0 + 0.2 * 1 + 2.0 * 3.0 + 0.1 * 0.5 = 0 + 0.2 + 6.0 + 0.05 = 6.25 + feedforward.calculate(PI / 2, 3.0, 0.5) shouldBe (6.25 plusOrMinus 0.001) + } + + test("calculates correctly at 45 degrees (π/4 radians)") { + val feedforward = ArmFeedforward(GravityFeedforwardParameters(1.0, 0.0, 0.0, 0.0)) + // position = π/4, cos(π/4) = √2/2 + // output = kG * cos(position) = 1.0 * √2/2 ≈ 0.7071 + feedforward.calculate(PI / 4, 0.0, 0.0) shouldBe (sqrt(2.0) / 2 plusOrMinus 0.001) + } + + test("calculates correctly at 180 degrees (π radians) - pointing down") { + val feedforward = ArmFeedforward(GravityFeedforwardParameters(1.0, 0.0, 0.0, 0.0)) + // position = π, cos(π) = -1 + // output = kG * cos(position) = 1.0 * (-1) = -1.0 + feedforward.calculate(PI, 0.0, 0.0) shouldBe (-1.0 plusOrMinus 0.001) + } + + test("calculates correctly with negative velocity") { + val feedforward = ArmFeedforward(GravityFeedforwardParameters(0.5, 0.2, 2.0, 0.1)) + // position = 0, cos(0) = 1 + // output = kG * cos(position) + kS * sign(velocity) + kV * velocity + kA * acceleration + // output = 0.5 * 1 + 0.2 * (-1) + 2.0 * (-3.0) + 0.1 * 0.5 = 0.5 - 0.2 - 6.0 + 0.05 = -5.65 + feedforward.calculate(0.0, -3.0, 0.5) shouldBe (-5.65 plusOrMinus 0.001) + } + + test("calculates correctly with zero velocity") { + val feedforward = ArmFeedforward(GravityFeedforwardParameters(0.5, 0.2, 2.0, 0.1)) + // position = 0, cos(0) = 1 + // output = kG * cos(position) + kS * sign(0) + kV * 0 + kA * acceleration + // output = 0.5 * 1 + 0 + 0 + 0.1 * 2.0 = 0.7 + feedforward.calculate(0.0, 0.0, 2.0) shouldBe (0.7 plusOrMinus 0.001) + } + + test("gravity compensation varies with angle") { + val feedforward = ArmFeedforward(GravityFeedforwardParameters(kG = 2.0)) + + // At horizontal, full gravity compensation + feedforward.calculate(0.0, 0.0, 0.0) shouldBe (2.0 plusOrMinus 0.001) + + // At 60 degrees, half gravity compensation + feedforward.calculate(PI / 3, 0.0, 0.0) shouldBe (1.0 plusOrMinus 0.001) + + // At vertical, no gravity compensation needed + feedforward.calculate(PI / 2, 0.0, 0.0) shouldBe (0.0 plusOrMinus 0.001) + + // Past vertical, negative gravity compensation (arm wants to fall the other way) + feedforward.calculate(2 * PI / 3, 0.0, 0.0) shouldBe (-1.0 plusOrMinus 0.001) + } + } + }) diff --git a/control/src/test/kotlin/dev/nextftc/control/feedforward/SimpleFeedforwardTest.kt b/control/src/test/kotlin/dev/nextftc/control/feedforward/SimpleFeedforwardTest.kt new file mode 100644 index 0000000..4bebbea --- /dev/null +++ b/control/src/test/kotlin/dev/nextftc/control/feedforward/SimpleFeedforwardTest.kt @@ -0,0 +1,119 @@ +/* + * 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.feedforward + +import io.kotest.core.spec.style.FunSpec +import io.kotest.matchers.doubles.plusOrMinus +import io.kotest.matchers.shouldBe + +class SimpleFeedforwardTest : + FunSpec({ + context("SimpleFFCoefficients") { + test("default kA is zero") { + val coefficients = SimpleFFCoefficients(1.0, 2.0) + coefficients.kS shouldBe 1.0 + coefficients.kV shouldBe 2.0 + coefficients.kA shouldBe 0.0 + } + + test("all values can be set") { + val coefficients = SimpleFFCoefficients(1.0, 2.0, 3.0) + coefficients.kS shouldBe 1.0 + coefficients.kV shouldBe 2.0 + coefficients.kA shouldBe 3.0 + } + + test("values are mutable") { + val coefficients = SimpleFFCoefficients(1.0, 2.0, 3.0) + coefficients.kS = 5.0 + coefficients.kV = 6.0 + coefficients.kA = 7.0 + coefficients.kS shouldBe 5.0 + coefficients.kV shouldBe 6.0 + coefficients.kA shouldBe 7.0 + } + } + + context("SimpleFeedforward construction") { + test("can be constructed with coefficients object") { + val coefficients = SimpleFFCoefficients(1.0, 2.0, 3.0) + val feedforward = SimpleFeedforward(coefficients) + feedforward.coefficients shouldBe coefficients + } + } + + context("SimpleFeedforward calculate") { + test("calculates correctly with positive velocity") { + val feedforward = SimpleFeedforward(SimpleFFCoefficients(0.5, 2.0, 0.1)) + // output = kS * sign(velocity) + kV * velocity + kA * acceleration + // output = 0.5 * 1 + 2.0 * 3.0 + 0.1 * 0.5 = 0.5 + 6.0 + 0.05 = 6.55 + feedforward.calculate(3.0, 0.5) shouldBe (6.55 plusOrMinus 0.001) + } + + test("calculates correctly with negative velocity") { + val feedforward = SimpleFeedforward(SimpleFFCoefficients(0.5, 2.0, 0.1)) + // output = kS * sign(velocity) + kV * velocity + kA * acceleration + // output = 0.5 * (-1) + 2.0 * (-3.0) + 0.1 * 0.5 = -0.5 - 6.0 + 0.05 = -6.45 + feedforward.calculate(-3.0, 0.5) shouldBe (-6.45 plusOrMinus 0.001) + } + + test("calculates correctly with zero velocity") { + val feedforward = SimpleFeedforward(SimpleFFCoefficients(0.5, 2.0, 0.1)) + // output = kS * sign(0) + kV * 0 + kA * acceleration + // output = 0 + 0 + 0.1 * 2.0 = 0.2 + feedforward.calculate(0.0, 2.0) shouldBe (0.2 plusOrMinus 0.001) + } + + test("calculates correctly with default acceleration") { + val feedforward = SimpleFeedforward(SimpleFFCoefficients(0.5, 2.0, 0.1)) + // output = kS * sign(velocity) + kV * velocity + kA * 0 + // output = 0.5 * 1 + 2.0 * 3.0 + 0 = 6.5 + feedforward.calculate(3.0) shouldBe (6.5 plusOrMinus 0.001) + } + } + + context("SimpleFeedforward velocity constraints") { + test("maxAchievableVelocity calculates correctly") { + val feedforward = SimpleFeedforward(SimpleFFCoefficients(0.5, 2.0, 0.1)) + // maxVel = (maxVoltage - kS - acceleration * kA) / kV + // maxVel = (12.0 - 0.5 - 1.0 * 0.1) / 2.0 = (12.0 - 0.5 - 0.1) / 2.0 = 11.4 / 2.0 = 5.7 + feedforward.maxAchievableVelocity(12.0, 1.0) shouldBe (5.7 plusOrMinus 0.001) + } + + test("minAchievableVelocity calculates correctly") { + val feedforward = SimpleFeedforward(SimpleFFCoefficients(0.5, 2.0, 0.1)) + // minVel = (-maxVoltage + kS - acceleration * kA) / kV + // minVel = (-12.0 + 0.5 - 1.0 * 0.1) / 2.0 = (-12.0 + 0.5 - 0.1) / 2.0 = -11.6 / 2.0 = -5.8 + feedforward.minAchievableVelocity(12.0, 1.0) shouldBe (-5.8 plusOrMinus 0.001) + } + } + + context("SimpleFeedforward acceleration constraints") { + test("maxAchievableAcceleration calculates correctly with positive velocity") { + val feedforward = SimpleFeedforward(SimpleFFCoefficients(0.5, 2.0, 0.1)) + // maxAccel = (maxVoltage - kS * sign(velocity) - velocity * kV) / kA + // maxAccel = (12.0 - 0.5 * 1 - 2.0 * 2.0) / 0.1 = (12.0 - 0.5 - 4.0) / 0.1 = 7.5 / 0.1 = 75 + feedforward.maxAchievableAcceleration(12.0, 2.0) shouldBe (75.0 plusOrMinus 0.001) + } + + test("maxAchievableAcceleration calculates correctly with negative velocity") { + val feedforward = SimpleFeedforward(SimpleFFCoefficients(0.5, 2.0, 0.1)) + // maxAccel = (maxVoltage - kS * sign(velocity) - velocity * kV) / kA + // maxAccel = (12.0 - 0.5 * (-1) - (-2.0) * 2.0) / 0.1 = (12.0 + 0.5 + 4.0) / 0.1 = 16.5 / 0.1 = 165 + feedforward.maxAchievableAcceleration(12.0, -2.0) shouldBe (165.0 plusOrMinus 0.001) + } + + test("minAchievableAcceleration calculates correctly") { + val feedforward = SimpleFeedforward(SimpleFFCoefficients(0.5, 2.0, 0.1)) + // minAccel = maxAchievableAcceleration(-maxVoltage, velocity) + // minAccel = (-12.0 - 0.5 * 1 - 2.0 * 2.0) / 0.1 = (-12.0 - 0.5 - 4.0) / 0.1 = -16.5 / 0.1 = -165 + feedforward.minAchievableAcceleration(12.0, 2.0) shouldBe (-165.0 plusOrMinus 0.001) + } + } + }) diff --git a/control/src/test/kotlin/dev/nextftc/control/profiles/TrapezoidProfileTest.kt b/control/src/test/kotlin/dev/nextftc/control/profiles/TrapezoidProfileTest.kt new file mode 100644 index 0000000..f748304 --- /dev/null +++ b/control/src/test/kotlin/dev/nextftc/control/profiles/TrapezoidProfileTest.kt @@ -0,0 +1,414 @@ +/* + * 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.profiles + +import dev.nextftc.control.model.MotionState +import dev.nextftc.units.Measure +import io.kotest.assertions.throwables.shouldThrow +import io.kotest.core.spec.style.FunSpec +import io.kotest.matchers.doubles.ToleranceMatcher +import io.kotest.matchers.doubles.plusOrMinus +import io.kotest.matchers.doubles.shouldBeGreaterThan +import io.kotest.matchers.doubles.shouldBeGreaterThanOrEqual +import io.kotest.matchers.doubles.shouldBeLessThan +import io.kotest.matchers.doubles.shouldBeLessThanOrEqual +import io.kotest.matchers.shouldBe +import kotlin.math.abs +import kotlin.time.Duration.Companion.seconds + +class TrapezoidProfileTest : + FunSpec({ + val tolerance = 1e-6 + + context("TrapezoidProfileConstraints") { + test("should create valid constraints with positive values") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + constraints.maxVelocity.magnitude shouldBe 5.0 + constraints.maxAcceleration.magnitude shouldBe 2.0 + } + + test("should accept zero values") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 0.0, maxAcceleration = 0.0) + constraints.maxVelocity.magnitude shouldBe 0.0 + constraints.maxAcceleration.magnitude shouldBe 0.0 + } + + test("should throw exception for negative maxVelocity") { + shouldThrow { + TrapezoidProfileConstraints(maxVelocity = -1.0, maxAcceleration = 2.0) + } + } + + test("should throw exception for negative maxAcceleration") { + shouldThrow { + TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = -2.0) + } + } + } + + context("TrapezoidProfile basic functionality") { + test("should start at initial state") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + val result = profile.calculate(0.seconds, initial, goal) + + result.position.magnitude shouldBe (initial.position.magnitude plusOrMinus tolerance) + result.velocity.magnitude shouldBe (initial.velocity.magnitude plusOrMinus tolerance) + } + + test("should reach goal state at end of profile") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + val result = profile.calculate(totalTime.seconds, initial, goal) + + result.position.magnitude shouldBe (goal.position.magnitude plusOrMinus tolerance) + result.velocity.magnitude shouldBe (goal.velocity.magnitude plusOrMinus tolerance) + } + + test("should respect maximum velocity constraint") { + val maxVelocity = 5.0 + val constraints = TrapezoidProfileConstraints(maxVelocity = maxVelocity, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 50.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + + // Sample the profile at multiple points + for (i in 0..100) { + val t = (totalTime * i / 100.0).seconds + val state = profile.calculate(t, initial, goal) + abs(state.velocity.magnitude) shouldBeLessThanOrEqual (maxVelocity + tolerance) + } + } + + test("should respect maximum acceleration constraint") { + val maxAcceleration = 2.0 + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = maxAcceleration) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 50.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + + // Sample the profile at multiple points + for (i in 0..100) { + val t = (totalTime * i / 100.0).seconds + val state = profile.calculate(t, initial, goal) + abs(state.acceleration.magnitude) shouldBeLessThanOrEqual (maxAcceleration + tolerance) + } + } + } + + context("TrapezoidProfile motion types") { + test("should generate full trapezoidal profile for long distance") { + val constraints = TrapezoidProfileConstraints(5.0, 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 50.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + + // Check that we reach max velocity somewhere in the middle + var reachedMaxVelocity = false + for (i in 0..100) { + val t = (totalTime * i / 100.0).seconds + val state = profile.calculate(t, initial, goal) + if (abs(state.velocity.magnitude - constraints.maxVelocity.magnitude) < tolerance) { + reachedMaxVelocity = true + break + } + } + reachedMaxVelocity shouldBe true + } + + test("should generate triangular profile for short distance") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 10.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 5.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + + // Check that we never reach max velocity + var reachedMaxVelocity = false + for (i in 0..100) { + val t = (totalTime * i / 100.0).seconds + val state = profile.calculate(t, initial, goal) + if (abs(state.velocity.magnitude - constraints.maxVelocity.magnitude) < tolerance) { + reachedMaxVelocity = true + break + } + } + reachedMaxVelocity shouldBe false + } + + test("should handle backward motion") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 10.0, velocity = 0.0) + val goal = MotionState(position = 0.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + val result = profile.calculate(totalTime.seconds, initial, goal) + + result.position.magnitude shouldBe (goal.position.magnitude plusOrMinus tolerance) + result.velocity.magnitude shouldBe (goal.velocity.magnitude plusOrMinus tolerance) + } + } + + context("TrapezoidProfile with non-zero initial velocity") { + test("should handle positive initial velocity in forward direction") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 2.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + val result = profile.calculate(totalTime.seconds, initial, goal) + + result.position.magnitude shouldBe (goal.position.magnitude plusOrMinus tolerance) + result.velocity.magnitude shouldBe (goal.velocity.magnitude plusOrMinus tolerance) + } + + test("should handle positive initial velocity in backward direction") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 10.0, velocity = 2.0) + val goal = MotionState(position = 0.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + val result = profile.calculate(totalTime.seconds, initial, goal) + + result.position.magnitude shouldBe (goal.position.magnitude plusOrMinus tolerance) + } + } + + context("TrapezoidProfile with non-zero goal velocity") { + test("should reach non-zero goal velocity") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 3.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + val result = profile.calculate(totalTime.seconds, initial, goal) + + result.position.magnitude shouldBe (goal.position.magnitude plusOrMinus tolerance) + result.velocity.magnitude shouldBe (goal.velocity.magnitude plusOrMinus tolerance) + } + } + + context("TrapezoidProfile edge cases") { + test("should handle zero distance movement") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 5.0, velocity = 0.0) + val goal = MotionState(position = 5.0, velocity = 0.0) + + val result = profile.calculate(0.seconds, initial, goal) + + result.position.magnitude shouldBe (initial.position.magnitude plusOrMinus tolerance) + result.velocity.magnitude shouldBe (initial.velocity.magnitude plusOrMinus tolerance) + } + + test("should handle already at goal") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 10.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + + totalTime shouldBe (0.0 plusOrMinus tolerance) + } + + test("should clamp initial velocity exceeding max velocity") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 10.0) + val goal = MotionState(position = 20.0, velocity = 0.0) + + val result = profile.calculate(0.seconds, initial, goal) + + // Should clamp to max velocity + abs(result.velocity.magnitude) shouldBeLessThanOrEqual (constraints.maxVelocity.magnitude + tolerance) + } + } + + context("TrapezoidProfile isFinished") { + test("should not be finished at start") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + + profile.isFinished(0.0) shouldBe false + } + + test("should be finished at total time") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + + profile.isFinished(totalTime) shouldBe true + } + + test("should be finished after total time") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + + profile.isFinished(totalTime + 1.0) shouldBe true + } + } + + context("TrapezoidProfile timeLeftUntil") { + test("should return zero time for current position") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val timeLeft = profile.timeLeftUntil(0.0) + + timeLeft shouldBe (0.0 plusOrMinus tolerance) + } + + test("should return positive time for target ahead") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val timeLeft = profile.timeLeftUntil(5.0) + + timeLeft shouldBeGreaterThan 0.0 + } + + test("should return negative time for target behind") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 5.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val timeLeft = profile.timeLeftUntil(3.0) + + timeLeft shouldBeLessThan 0.0 + } + } + + context("TrapezoidProfile continuity") { + test("position should be continuous throughout profile") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 20.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + val dt = 0.01 + + var previousState = profile.calculate(0.seconds, initial, goal) + for (i in 1..((totalTime / dt).toInt())) { + val t = (i * dt).seconds + val currentState = profile.calculate(t, initial, goal) + + // Position should always increase (or stay same) + currentState.position.magnitude shouldBeGreaterThanOrEqual + (previousState.position.magnitude - tolerance) + + previousState = currentState + } + } + + test("velocity should be continuous throughout profile") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 20.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime + val dt = 0.01 + + var previousState = profile.calculate(0.seconds, initial, goal) + for (i in 1..((totalTime / dt).toInt())) { + val t = (i * dt).seconds + val currentState = profile.calculate(t, initial, goal) + + // Velocity change should be bounded by acceleration * dt + val velocityChange = abs(currentState.velocity.magnitude - previousState.velocity.magnitude) + velocityChange shouldBeLessThanOrEqual (constraints.maxAcceleration.magnitude * dt + tolerance) + + previousState = currentState + } + } + } + + context("TrapezoidProfile totalTime property") { + test("should have positive total time for non-zero movement") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + + profile.totalTime shouldBeGreaterThan 0.0 + } + + test("should have consistent total time across multiple calls") { + val constraints = TrapezoidProfileConstraints(maxVelocity = 5.0, maxAcceleration = 2.0) + val profile = TrapezoidProfile(constraints) + val initial = MotionState(position = 0.0, velocity = 0.0) + val goal = MotionState(position = 10.0, velocity = 0.0) + + profile.calculate(0.seconds, initial, goal) + val totalTime1 = profile.totalTime + + profile.calculate(1.seconds, initial, goal) + val totalTime2 = profile.totalTime + + totalTime1 shouldBe (totalTime2 plusOrMinus tolerance) + } + } + }) + +infix fun Measure<*>.plusOrMinus(tolerance: Double): ToleranceMatcher = ToleranceMatcher(this.magnitude, tolerance) diff --git a/control/src/test/kotlin/dev/nextftc/control/util/DiscretizationTest.kt b/control/src/test/kotlin/dev/nextftc/control/util/DiscretizationTest.kt new file mode 100644 index 0000000..9821497 --- /dev/null +++ b/control/src/test/kotlin/dev/nextftc/control/util/DiscretizationTest.kt @@ -0,0 +1,194 @@ +/* + * 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.util + +import dev.nextftc.linalg.Matrix +import dev.nextftc.linalg.N2 +import dev.nextftc.linalg.Nat +import dev.nextftc.linalg.Vector +import io.kotest.core.spec.style.FunSpec +import io.kotest.matchers.doubles.shouldBeLessThan +import io.kotest.matchers.shouldBe + +/** + * Computes the matrix exponential e^A using a Taylor series approximation. + * e^A = I + A + A²/2! + A³/3! + ... + */ +private fun matrixExp(A: Matrix, terms: Int = 20): Matrix { + var result = Matrix.identity(A.natRows) + var term = Matrix.identity(A.natRows) + + for (k in 1..terms) { + term = term * A * (1.0 / k) + result += term + } + return result +} + +/** + * Simple RK4 integration for matrix-valued functions. + * Integrates dx/dt = f(t, x) from t0 to t0 + dt. + */ +private fun rk4Matrix( + f: (Double, Matrix) -> Matrix, + t0: Double, + x0: Matrix, + dt: Double, + steps: Int = 100, +): Matrix { + var t = t0 + var x = x0 + val h = dt / steps + + for (i in 0 until steps) { + val k1 = f(t, x) + val k2 = f(t + h / 2, x + k1 * (h / 2)) + val k3 = f(t + h / 2, x + k2 * (h / 2)) + val k4 = f(t + h, x + k3 * h) + + x += (k1 + k2 * 2.0 + k3 * 2.0 + k4) * (h / 6.0) + t += h + } + + return x +} + +class DiscretizationTest : + FunSpec({ + context("discretizeAB") { + test("discretizes double integrator correctly") { + // contA represents: dx/dt = [[0, 1], [0, 0]] * x + // This is a double integrator: position derivative = velocity, velocity derivative = 0 + val contA = Matrix.from( + N2, + N2, + arrayOf( + doubleArrayOf(0.0, 1.0), + doubleArrayOf(0.0, 0.0), + ), + ) + // contB represents: dx/dt += [[0], [1]] * u + // Input affects acceleration (velocity derivative) + val contB = Matrix.from( + N2, + dev.nextftc.linalg.N1, + arrayOf( + doubleArrayOf(0.0), + doubleArrayOf(1.0), + ), + ) + + val x0 = Vector.of(N2, 1.0, 1.0) + val u = Vector.of(dev.nextftc.linalg.N1, 1.0) + + val (discA, discB) = discretizeAB(contA, contB, 1.0) + + val x1Discrete = discA * x0 + discB * u + + // We now have pos = vel = accel = 1, which should give us: + // pos(1) = pos(0) + vel(0)*dt + 0.5*accel*dt² = 1 + 1*1 + 0.5*1*1 = 2.5 + // vel(1) = vel(0) + accel*dt = 1 + 1*1 = 2 + val x1Truth = Vector.of( + N2, + 1.0 * x0[0] + 1.0 * x0[1] + 0.5 * u[0], + 0.0 * x0[0] + 1.0 * x0[1] + 1.0 * u[0], + ) + + x1Discrete shouldBe x1Truth + } + } + + // T + // Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 + context("discretizeAQ") { + test("discretizes slow model correctly") { + val contA = Matrix.from( + N2, + N2, + arrayOf( + doubleArrayOf(0.0, 1.0), + doubleArrayOf(0.0, 0.0), + ), + ) + val contQ = Matrix.from( + N2, + N2, + arrayOf( + doubleArrayOf(1.0, 0.0), + doubleArrayOf(0.0, 1.0), + ), + ) + + val dt = 1.0 + + // T + // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 + val discQIntegrated = rk4Matrix( + { t, _ -> + val expAt = matrixExp(contA * t) + val expAtT = matrixExp(contA.transpose * t) + expAt * contQ * expAtT + }, + 0.0, + Matrix.zero(N2, N2), + dt, + ) + + val (_, discQ) = discretizeAQ(contA, contQ, dt) + + val diff = (discQIntegrated - discQ).norm + diff shouldBeLessThan 1e-10 + } + + // T + // Test that the discrete approximation of Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 + test("discretizes fast model correctly") { + val contA = Matrix.from( + N2, + N2, + arrayOf( + doubleArrayOf(0.0, 1.0), + doubleArrayOf(0.0, -1406.29), + ), + ) + val contQ = Matrix.from( + N2, + N2, + arrayOf( + doubleArrayOf(0.0025, 0.0), + doubleArrayOf(0.0, 1.0), + ), + ) + + val dt = 0.005 + + // T + // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ + // 0 + val discQIntegrated = rk4Matrix( + { t, _ -> + val expAt = matrixExp(contA * t) + val expAtT = matrixExp(contA.transpose * t) + expAt * contQ * expAtT + }, + 0.0, + Matrix.zero(N2, N2), + dt, + ) + + val (_, discQ) = discretizeAQ(contA, contQ, dt) + + val diff = (discQIntegrated - discQ).norm + diff shouldBeLessThan 1e-3 + } + } + }) diff --git a/gradle.properties b/gradle.properties index c52a471..d9cee49 100644 --- a/gradle.properties +++ b/gradle.properties @@ -5,4 +5,4 @@ org.jetbrains.dokka.experimental.gradle.pluginMode.noWarn=true dev.nextftc.publishing.automaticMavenCentralSync=true -version = 1.0.0-SNAPSHOT \ No newline at end of file +version = 0.0.1-alpha.1 \ No newline at end of file diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt index 8287814..15310b8 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -14,24 +14,23 @@ package dev.nextftc.linalg import org.ejml.simple.SimpleMatrix /** Creates a 1-dimensional vector. */ -fun makeVector(x: Double): SizedVector = - SizedVector(SimpleMatrix(1, 1, false, doubleArrayOf(x)), N1) +fun makeVector(x: Double): Vector = Vector(SimpleMatrix(1, 1, false, doubleArrayOf(x)), N1) /** Creates a 2-dimensional vector. */ -fun makeVector(x: Double, y: Double): SizedVector = - SizedVector(SimpleMatrix(2, 1, false, doubleArrayOf(x, y)), N2) +fun makeVector(x: Double, y: Double): Vector = + Vector(SimpleMatrix(2, 1, false, doubleArrayOf(x, y)), N2) /** Creates a 3-dimensional vector. */ -fun makeVector(x: Double, y: Double, z: Double): SizedVector = - SizedVector(SimpleMatrix(3, 1, false, doubleArrayOf(x, y, z)), N3) +fun makeVector(x: Double, y: Double, z: Double): Vector = + Vector(SimpleMatrix(3, 1, false, doubleArrayOf(x, y, z)), N3) /** Creates a 4-dimensional vector. */ -fun makeVector(x1: Double, x2: Double, x3: Double, x4: Double): SizedVector = - SizedVector(SimpleMatrix(4, 1, false, doubleArrayOf(x1, x2, x3, x4)), N4) +fun makeVector(x1: Double, x2: Double, x3: Double, x4: Double): Vector = + Vector(SimpleMatrix(4, 1, false, doubleArrayOf(x1, x2, x3, x4)), N4) /** Creates a 5-dimensional vector. */ -fun makeVector(x1: Double, x2: Double, x3: Double, x4: Double, x5: Double): SizedVector = - SizedVector(SimpleMatrix(5, 1, false, doubleArrayOf(x1, x2, x3, x4, x5)), N5) +fun makeVector(x1: Double, x2: Double, x3: Double, x4: Double, x5: Double): Vector = + Vector(SimpleMatrix(5, 1, false, doubleArrayOf(x1, x2, x3, x4, x5)), N5) /** Creates a 6-dimensional vector. */ fun makeVector( @@ -41,8 +40,7 @@ fun makeVector( x4: Double, x5: Double, x6: Double, -): SizedVector = - SizedVector(SimpleMatrix(6, 1, false, doubleArrayOf(x1, x2, x3, x4, x5, x6)), N6) +): Vector = Vector(SimpleMatrix(6, 1, false, doubleArrayOf(x1, x2, x3, x4, x5, x6)), N6) /** Creates a 7-dimensional vector. */ fun makeVector( @@ -53,8 +51,7 @@ fun makeVector( x5: Double, x6: Double, x7: Double, -): SizedVector = - SizedVector(SimpleMatrix(7, 1, false, doubleArrayOf(x1, x2, x3, x4, x5, x6, x7)), N7) +): Vector = Vector(SimpleMatrix(7, 1, false, doubleArrayOf(x1, x2, x3, x4, x5, x6, x7)), N7) /** Creates an 8-dimensional vector. */ fun makeVector( @@ -66,8 +63,7 @@ fun makeVector( x6: Double, x7: Double, x8: Double, -): SizedVector = - SizedVector(SimpleMatrix(8, 1, false, doubleArrayOf(x1, x2, x3, x4, x5, x6, x7, x8)), N8) +): Vector = Vector(SimpleMatrix(8, 1, false, doubleArrayOf(x1, x2, x3, x4, x5, x6, x7, x8)), N8) /** Creates a 9-dimensional vector. */ fun makeVector( @@ -80,8 +76,8 @@ fun makeVector( x7: Double, x8: Double, x9: Double, -): SizedVector = - SizedVector(SimpleMatrix(9, 1, false, doubleArrayOf(x1, x2, x3, x4, x5, x6, x7, x8, x9)), N9) +): Vector = + Vector(SimpleMatrix(9, 1, false, doubleArrayOf(x1, x2, x3, x4, x5, x6, x7, x8, x9)), N9) /** Creates a 10-dimensional vector. */ fun makeVector( @@ -95,7 +91,7 @@ fun makeVector( x8: Double, x9: Double, x10: Double, -): SizedVector = SizedVector( +): Vector = Vector( SimpleMatrix(10, 1, false, doubleArrayOf(x1, x2, x3, x4, x5, x6, x7, x8, x9, x10)), N10, ) diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/Decompositions.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Decompositions.kt index 5285d0a..7855bec 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Decompositions.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Decompositions.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt index f147de9..459f098 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -175,7 +175,13 @@ open class DynamicMatrix(internal val simple: SimpleMatrix) { /** * Multiplies this matrix by a scalar. */ - open operator fun times(scalar: Int) = DynamicMatrix(simple * scalar.toDouble()) + open operator fun times(scalar: Number) = DynamicMatrix(simple * scalar.toDouble()) + + /** Divides this matrix by a scalar. */ + open operator fun div(scalar: Double) = times(1.0 / scalar) + + /** Divides this matrix by a scalar. */ + open operator fun div(scalar: Number) = times(1.0 / scalar.toDouble()) /** * @usesMathJax @@ -223,6 +229,35 @@ open class DynamicMatrix(internal val simple: SimpleMatrix) { simple.extractMatrix(startRow, endRow, startCol, endCol), ) + /** + * Computes the matrix exponential of this matrix, + * using the Padé approximant. + * + * Uses the formula: + * e^A ≈ (1 + A/2 + A²/9 + A³/72 + A⁴/1008 + A⁵/30240) / (1 - A/2 + A²/9 - A³/72 + A⁴/1008 - A⁵/30240) + * + * @return The matrix exponential of this matrix. + */ + @Suppress("ktlint:standard:property-naming") + fun exp(): DynamicMatrix { + require(numRows == numColumns) { "Matrix must be square" } + + val I = identity(numRows) + val A2 = this * this + val A3 = A2 * this + val A4 = A3 * this + val A5 = A4 * this + + val numerator = + I + this * 0.5 + A2 * (1.0 / 9.0) + A3 * (1.0 / 72.0) + A4 * (1.0 / 1008.0) + + A5 * (1.0 / 30240.0) + val denominator = + I - this * 0.5 + A2 * (1.0 / 9.0) - A3 * (1.0 / 72.0) + A4 * (1.0 / 1008.0) - + A5 * (1.0 / 30240.0) + + return denominator.solve(numerator) + } + /** * Returns the LLT (Cholesky) decomposition of this matrix. * Only works for symmetric, positive-definite matrices. @@ -286,12 +321,12 @@ open class DynamicMatrix(internal val simple: SimpleMatrix) { override fun hashCode(): Int = simple.hashCode() /** - * Returns a [SizedMatrix] with the same dimensions as this matrix. + * Returns a [Matrix] with the same dimensions as this matrix. * The dimensions are checked at runtime. */ - fun toSizedMatrix(rows: R, cols: C): SizedMatrix { + fun toSizedMatrix(rows: R, cols: C): Matrix { require(numRows == rows.num) { "Matrix has $numRows rows, expected ${rows.num}" } require(numColumns == cols.num) { "Matrix has $numColumns columns, expected ${cols.num}" } - return SizedMatrix(simple, rows, cols) + return Matrix(simple, rows, cols) } } diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt index 1faecf1..4be386b 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -79,7 +79,7 @@ class DynamicVector internal constructor(simple: SimpleMatrix) : DynamicMatrix(s override operator fun times(scalar: Double): DynamicVector = DynamicVector(simple.scale(scalar)) /** Multiplies this vector by a scalar. */ - override operator fun times(scalar: Int): DynamicVector = times(scalar.toDouble()) + override operator fun times(scalar: Number): DynamicVector = times(scalar.toDouble()) /** Computes the dot product of this vector with another vector. */ fun dot(other: DynamicVector): Double = simple.transpose().mult(other.simple)[0, 0] diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/Matrices.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrices.kt index 8168ae5..ac4bed5 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Matrices.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrices.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/SizedMatrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt similarity index 56% rename from linalg/src/main/kotlin/dev/nextftc/linalg/SizedMatrix.kt rename to linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt index 8459def..fbcd4a8 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/SizedMatrix.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -11,6 +11,7 @@ package dev.nextftc.linalg +import dev.nextftc.linalg.Matrix.Companion.identity import org.ejml.simple.SimpleMatrix /** @@ -30,7 +31,7 @@ import org.ejml.simple.SimpleMatrix * @param R The row dimension type * @param C The column dimension type */ -open class SizedMatrix internal constructor( +open class Matrix internal constructor( internal val simple: SimpleMatrix, internal val rowNat: R, internal val colNat: C, @@ -44,66 +45,62 @@ open class SizedMatrix internal constructor( * Creates a zero matrix with dimensions specified by the [Nat] type parameters. */ @JvmStatic - fun zero(rows: R, cols: C): SizedMatrix = - SizedMatrix(SimpleMatrix(rows.num, cols.num), rows, cols) + fun zero(rows: R, cols: C): Matrix = + Matrix(SimpleMatrix(rows.num, cols.num), rows, cols) /** * Creates a zero matrix with dimensions [rows] x [cols]. */ @JvmStatic @Suppress("UNCHECKED_CAST") - fun zero(rows: Int, cols: Int): SizedMatrix { + fun zero(rows: Int, cols: Int): Matrix { val rNat = natOf(rows) val cNat = natOf(cols) - return zero(rNat, cNat) as SizedMatrix + return zero(rNat, cNat) as Matrix } /** * Creates an identity matrix with dimensions [size] x [size]. */ @JvmStatic - fun identity(size: N): SizedMatrix = - SizedMatrix(SimpleMatrix.identity(size.num), size, size) + fun identity(size: N): Matrix = + Matrix(SimpleMatrix.identity(size.num), size, size) /** * Creates a matrix with [data] along the diagonal. */ @JvmStatic - fun diagonal(size: N, vararg data: Double): SizedMatrix { + fun diagonal(size: N, vararg data: Double): Matrix { require(data.size == size.num) { "Data size must match dimension" } - return SizedMatrix(SimpleMatrix.diag(*data), size, size) + return Matrix(SimpleMatrix.diag(*data), size, size) } /** * Creates a row vector (1 x C matrix). */ @JvmStatic - fun row(cols: C, vararg data: Double): SizedMatrix { + fun row(cols: C, vararg data: Double): Matrix { require(data.size == cols.num) { "Data size must match column dimension" } - return SizedMatrix(SimpleMatrix(1, data.size, true, data), N1, cols) + return Matrix(SimpleMatrix(1, data.size, true, data), N1, cols) } /** * Creates a column vector (R x 1 matrix). */ @JvmStatic - fun column(rows: R, vararg data: Double): SizedMatrix { + fun column(rows: R, vararg data: Double): Matrix { require(data.size == rows.num) { "Data size must match row dimension" } - return SizedMatrix(SimpleMatrix(data.size, 1, false, data), rows, N1) + return Matrix(SimpleMatrix(data.size, 1, false, data), rows, N1) } /** * Creates a matrix from a 2D array with specified dimensions. */ @JvmStatic - fun from( - rows: R, - cols: C, - data: Array, - ): SizedMatrix { + fun from(rows: R, cols: C, data: Array): Matrix { require(data.size == rows.num) { "Row count must match row dimension" } require(data.all { it.size == cols.num }) { "All rows must have column dimension size" } - return SizedMatrix(SimpleMatrix(data), rows, cols) + return Matrix(SimpleMatrix(data), rows, cols) } } @@ -111,31 +108,41 @@ open class SizedMatrix internal constructor( @JvmField val numRows: Int = simple.numRows() + /** Natural number representing the number of rows. */ + @JvmField + val natRows: R = rowNat + /** The number of columns in the matrix. */ @JvmField val numColumns: Int = simple.numCols() + /** + * Natural number representing the number of columns. + */ + @JvmField + val natColumns: C = colNat + /** The size of the matrix as (rows, columns). */ @JvmField val size: Pair = numRows to numColumns /** The transpose of this matrix, with swapped dimension types. */ @get:JvmName("transpose") - val transpose: SizedMatrix - get() = SizedMatrix(simple.transpose(), colNat, rowNat) + val transpose: Matrix + get() = Matrix(simple.transpose(), colNat, rowNat) /** Returns a copy of this matrix. */ - open fun copy(): SizedMatrix = SizedMatrix(simple.copy(), rowNat, colNat) + open fun copy(): Matrix = Matrix(simple.copy(), rowNat, colNat) /** The inverse of this matrix. Only valid for square matrices. */ @get:JvmName("inverse") - val inverse: SizedMatrix - get() = SizedMatrix(simple.invert(), rowNat, colNat) + val inverse: Matrix + get() = Matrix(simple.invert(), rowNat, colNat) /** The pseudo-inverse of this matrix. */ @get:JvmName("pseudoInverse") - val pseudoInverse: SizedMatrix - get() = SizedMatrix(simple.pseudoInverse(), colNat, rowNat) + val pseudoInverse: Matrix + get() = Matrix(simple.pseudoInverse(), colNat, rowNat) /** The Frobenius norm of this matrix. */ @get:JvmName("norm") @@ -143,37 +150,42 @@ open class SizedMatrix internal constructor( get() = simple.normF() /** Negates all elements of this matrix. */ - open operator fun unaryMinus(): SizedMatrix = - SizedMatrix(simple.negative(), rowNat, colNat) + open operator fun unaryMinus(): Matrix = Matrix(simple.negative(), rowNat, colNat) /** Adds another matrix with the same dimensions. */ - operator fun plus(other: SizedMatrix): SizedMatrix = - SizedMatrix(simple + other.simple, rowNat, colNat) + operator fun plus(other: Matrix): Matrix = + Matrix(simple + other.simple, rowNat, colNat) /** Subtracts another matrix with the same dimensions. */ - operator fun minus(other: SizedMatrix): SizedMatrix = - SizedMatrix(simple - other.simple, rowNat, colNat) + operator fun minus(other: Matrix): Matrix = + Matrix(simple - other.simple, rowNat, colNat) /** * Multiplies this matrix by another matrix. * The inner dimensions must match: (R x C) * (C x K) = (R x K) */ - operator fun times(other: SizedMatrix): SizedMatrix = - SizedMatrix(simple.mult(other.simple), rowNat, other.colNat) + operator fun times(other: Matrix): Matrix = + Matrix(simple.mult(other.simple), rowNat, other.colNat) /** Multiplies this matrix by a scalar. */ - open operator fun times(scalar: Double): SizedMatrix = - SizedMatrix(simple.scale(scalar), rowNat, colNat) + open operator fun times(scalar: Double): Matrix = + Matrix(simple.scale(scalar), rowNat, colNat) /** Multiplies this matrix by a scalar. */ - open operator fun times(scalar: Int): SizedMatrix = times(scalar.toDouble()) + open operator fun times(scalar: Number): Matrix = times(scalar.toDouble()) + + /** Divides this matrix by a scalar. */ + open operator fun div(scalar: Double) = times(1.0 / scalar) + + /** Divides this matrix by a scalar. */ + open operator fun div(scalar: Number) = times(1.0 / scalar.toDouble()) /** * Solves for X in the equation AX = B, * where A is this matrix and B is [other]. */ - fun solve(other: SizedMatrix): SizedMatrix = - SizedMatrix(simple.solve(other.simple), colNat, other.colNat) + fun solve(other: Matrix): Matrix = + Matrix(simple.solve(other.simple), colNat, other.colNat) /** Returns the element at the given indices. */ operator fun get(i: Int, j: Int): Double = simple[i, j] @@ -200,16 +212,41 @@ open class SizedMatrix internal constructor( override fun equals(other: Any?): Boolean { if (this === other) return true - return other is SizedMatrix<*, *> && this.simple.isIdentical(other.simple, 1e-6) + return other is Matrix<*, *> && this.simple.isIdentical(other.simple, 1e-6) } override fun hashCode(): Int = simple.hashCode() } /** Scalar multiplication from the left. */ -operator fun Double.times(matrix: SizedMatrix): SizedMatrix = - matrix * this +operator fun Double.times(matrix: Matrix): Matrix = matrix * this /** Scalar multiplication from the left. */ -operator fun Int.times(matrix: SizedMatrix): SizedMatrix = - matrix * this +operator fun Int.times(matrix: Matrix): Matrix = matrix * this + +/** + * Computes the matrix exponential of this matrix, + * using the Padé approximant. + * + * Uses the formula: + * e^A ≈ (1 + A/2 + A²/9 + A³/72 + A⁴/1008 + A⁵/30240) / (1 - A/2 + A²/9 - A³/72 + A⁴/1008 - A⁵/30240) + * + * @return The matrix exponential of this matrix. + */ +@Suppress("ktlint:standard:property-naming") +fun Matrix.exp(): Matrix { + val I = identity(natRows) + val A2 = this * this + val A3 = A2 * this + val A4 = A3 * this + val A5 = A4 * this + + val numerator = + I + this * 0.5 + A2 * (1.0 / 9.0) + A3 * (1.0 / 72.0) + A4 * (1.0 / 1008.0) + + A5 * (1.0 / 30240.0) + val denominator = + I - this * 0.5 + A2 * (1.0 / 9.0) - A3 * (1.0 / 72.0) + A4 * (1.0 / 1008.0) - + A5 * (1.0 / 30240.0) + + return denominator.solve(numerator) +} diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/Nat.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Nat.kt index 7f53749..3b13563 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Nat.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Nat.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -12,7 +12,7 @@ package dev.nextftc.linalg /** * Type-level natural numbers for compile-time matrix dimension checking. - * Use these as generic bounds on [SizedMatrix] to ensure dimensional correctness at compile time. + * Use these as generic bounds on [Matrix] to ensure dimensional correctness at compile time. */ sealed interface Nat { val num: Int diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt similarity index 66% rename from linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt rename to linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt index 4fa5866..7e0fca3 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -29,34 +29,36 @@ import org.ejml.simple.SimpleMatrix * * @param N The dimension type */ -class SizedVector internal constructor(simple: SimpleMatrix, internal val dimNat: N) : - SizedMatrix(simple, dimNat, N1) { +class Vector internal constructor(simple: SimpleMatrix, internal val dimNat: N) : + Matrix(simple, dimNat, N1) { init { require(simple.numCols() == 1) { "Vector must have exactly one column" } } + constructor(matrix: Matrix) : this(matrix.simple, matrix.rowNat) + @Suppress("ktlint") companion object { /** * Creates a zero vector with dimension specified by the [Nat] type parameter. */ @JvmStatic - fun zero(dim: N): SizedVector = SizedVector(SimpleMatrix(dim.num, 1), dim) + fun zero(dim: N): Vector = Vector(SimpleMatrix(dim.num, 1), dim) /** * Creates a vector from the given values. */ @JvmStatic - fun of(dim: N, vararg data: Double): SizedVector { + fun of(dim: N, vararg data: Double): Vector { require(data.size == dim.num) { "Data size must match dimension" } - return SizedVector(SimpleMatrix(data.size, 1, false, data), dim) + return Vector(SimpleMatrix(data.size, 1, false, data), dim) } /** * Creates a vector from a collection. */ @JvmStatic - fun of(dim: N, data: Collection): SizedVector = + fun of(dim: N, data: Collection): Vector = of(dim, *data.toDoubleArray()) } @@ -73,28 +75,25 @@ class SizedVector internal constructor(simple: SimpleMatrix, internal v } /** Returns a copy of this vector. */ - override fun copy(): SizedVector = SizedVector(simple.copy(), dimNat) + override fun copy(): Vector = Vector(simple.copy(), dimNat) /** Negates all elements of this vector. */ - override operator fun unaryMinus(): SizedVector = SizedVector(simple.negative(), dimNat) + override operator fun unaryMinus(): Vector = Vector(simple.negative(), dimNat) /** Adds another vector with the same dimension. */ - operator fun plus(other: SizedVector): SizedVector = - SizedVector(simple + other.simple, dimNat) + operator fun plus(other: Vector): Vector = Vector(simple + other.simple, dimNat) /** Subtracts another vector with the same dimension. */ - operator fun minus(other: SizedVector): SizedVector = - SizedVector(simple - other.simple, dimNat) + operator fun minus(other: Vector): Vector = Vector(simple - other.simple, dimNat) /** Multiplies this vector by a scalar. */ - override operator fun times(scalar: Double): SizedVector = - SizedVector(simple.scale(scalar), dimNat) + override operator fun times(scalar: Double): Vector = Vector(simple.scale(scalar), dimNat) /** Multiplies this vector by a scalar. */ - override operator fun times(scalar: Int): SizedVector = times(scalar.toDouble()) + override operator fun times(scalar: Number): Vector = times(scalar.toDouble()) /** Computes the dot product of this vector with another vector of the same dimension. */ - infix fun dot(other: SizedVector): Double = simple.transpose().mult(other.simple)[0, 0] + infix fun dot(other: Vector): Double = simple.transpose().mult(other.simple)[0, 0] /** Returns the Euclidean norm (magnitude) of this vector. */ @get:JvmName("magnitude") @@ -102,7 +101,7 @@ class SizedVector internal constructor(simple: SimpleMatrix, internal v get() = simple.normF() /** Returns a normalized (unit) vector in the same direction. */ - fun normalized(): SizedVector = this * (1.0 / magnitude) + fun normalized(): Vector = this * (1.0 / magnitude) /** Converts to a [DynamicVector]. */ fun toDynamicVector(): DynamicVector = DynamicVector(simple) @@ -118,7 +117,7 @@ class SizedVector internal constructor(simple: SimpleMatrix, internal v } /** Scalar multiplication from the left. */ -operator fun Double.times(vector: SizedVector): SizedVector = vector * this +operator fun Double.times(vector: Vector): Vector = vector * this /** Scalar multiplication from the left. */ -operator fun Int.times(vector: SizedVector): SizedVector = vector * this +operator fun Int.times(vector: Vector): Vector = vector * this diff --git a/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt index d7dafd4..802b214 100644 --- a/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt +++ b/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -419,4 +419,31 @@ class DynamicMatrixTest : } } } + + context("Matrix Exponential") { + test("exp of identity matrix equals e*I") { + val matrix = DynamicMatrix.identity(2) + val result = matrix.exp() + + result[0, 0] shouldBe (Math.E plusOrMinus 1e-9) + result[0, 1] shouldBe (0.0 plusOrMinus 1e-9) + result[1, 0] shouldBe (0.0 plusOrMinus 1e-9) + result[1, 1] shouldBe (Math.E plusOrMinus 1e-9) + } + + test("exp of scaled matrix") { + val matrix = DynamicMatrix( + arrayOf( + doubleArrayOf(1.0, 2.0), + doubleArrayOf(3.0, 4.0), + ), + ) + val result = (matrix * 0.01).exp() + + result[0, 0] shouldBe (1.01035625 plusOrMinus 1e-8) + result[0, 1] shouldBe (0.02050912 plusOrMinus 1e-8) + result[1, 0] shouldBe (0.03076368 plusOrMinus 1e-8) + result[1, 1] shouldBe (1.04111993 plusOrMinus 1e-8) + } + } }) diff --git a/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicVectorTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicVectorTest.kt index 16b4806..1b17fb9 100644 --- a/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicVectorTest.kt +++ b/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicVectorTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt index d3ca973..888285a 100644 --- a/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt +++ b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -17,7 +17,7 @@ class SizedMatrixTest : FunSpec({ context("Construction") { test("zero matrix has all zeros") { - val m = SizedMatrix.zero(N2, N3) + val m = Matrix.zero(N2, N3) m.numRows shouldBe 2 m.numColumns shouldBe 3 for (i in 0 until 2) { @@ -28,7 +28,7 @@ class SizedMatrixTest : } test("identity creates identity matrix") { - val m = SizedMatrix.identity(N3) + val m = Matrix.identity(N3) m.numRows shouldBe 3 m.numColumns shouldBe 3 m[0, 0] shouldBe 1.0 @@ -39,7 +39,7 @@ class SizedMatrixTest : } test("diagonal creates diagonal matrix") { - val m = SizedMatrix.diagonal(N3, 1.0, 2.0, 3.0) + val m = Matrix.diagonal(N3, 1.0, 2.0, 3.0) m.numRows shouldBe 3 m.numColumns shouldBe 3 m[0, 0] shouldBe 1.0 @@ -50,12 +50,12 @@ class SizedMatrixTest : test("diagonal throws on size mismatch") { shouldThrow { - SizedMatrix.diagonal(N3, 1.0, 2.0) + Matrix.diagonal(N3, 1.0, 2.0) } } test("row creates row vector") { - val m = SizedMatrix.row(N3, 1.0, 2.0, 3.0) + val m = Matrix.row(N3, 1.0, 2.0, 3.0) m.numRows shouldBe 1 m.numColumns shouldBe 3 m[0, 0] shouldBe 1.0 @@ -65,12 +65,12 @@ class SizedMatrixTest : test("row throws on size mismatch") { shouldThrow { - SizedMatrix.row(N3, 1.0, 2.0) + Matrix.row(N3, 1.0, 2.0) } } test("column creates column vector") { - val m = SizedMatrix.column(N3, 1.0, 2.0, 3.0) + val m = Matrix.column(N3, 1.0, 2.0, 3.0) m.numRows shouldBe 3 m.numColumns shouldBe 1 m[0, 0] shouldBe 1.0 @@ -80,12 +80,12 @@ class SizedMatrixTest : test("column throws on size mismatch") { shouldThrow { - SizedMatrix.column(N3, 1.0, 2.0) + Matrix.column(N3, 1.0, 2.0) } } test("from creates matrix from 2D array") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N2, arrayOf( @@ -103,7 +103,7 @@ class SizedMatrixTest : test("from throws on row count mismatch") { shouldThrow { - SizedMatrix.from( + Matrix.from( N3, N2, arrayOf( @@ -116,7 +116,7 @@ class SizedMatrixTest : test("from throws on column count mismatch") { shouldThrow { - SizedMatrix.from( + Matrix.from( N2, N3, arrayOf( @@ -128,7 +128,7 @@ class SizedMatrixTest : } test("2D array constructor") { - val m = SizedMatrix( + val m = Matrix( arrayOf( doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 4.0), @@ -143,12 +143,12 @@ class SizedMatrixTest : context("Properties") { test("size returns correct dimensions") { - val m = SizedMatrix.zero(N2, N3) + val m = Matrix.zero(N2, N3) m.size shouldBe (2 to 3) } test("transpose swaps dimensions and types") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N3, arrayOf( @@ -156,7 +156,7 @@ class SizedMatrixTest : doubleArrayOf(4.0, 5.0, 6.0), ), ) - val t: SizedMatrix = m.transpose + val t: Matrix = m.transpose t.numRows shouldBe 3 t.numColumns shouldBe 2 t[0, 0] shouldBe 1.0 @@ -166,7 +166,7 @@ class SizedMatrixTest : } test("inverse computes correctly") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N2, arrayOf( @@ -174,7 +174,7 @@ class SizedMatrixTest : doubleArrayOf(2.0, 6.0), ), ) - val inv: SizedMatrix = m.inverse + val inv: Matrix = m.inverse val product = m * inv product[0, 0] shouldBe (1.0 plusOrMinus 1e-9) product[1, 1] shouldBe (1.0 plusOrMinus 1e-9) @@ -183,7 +183,7 @@ class SizedMatrixTest : } test("pseudoInverse has correct dimensions") { - val m = SizedMatrix.from( + val m = Matrix.from( N3, N2, arrayOf( @@ -192,13 +192,13 @@ class SizedMatrixTest : doubleArrayOf(5.0, 6.0), ), ) - val pinv: SizedMatrix = m.pseudoInverse + val pinv: Matrix = m.pseudoInverse pinv.numRows shouldBe 2 pinv.numColumns shouldBe 3 } test("norm computes Frobenius norm") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N2, arrayOf( @@ -213,7 +213,7 @@ class SizedMatrixTest : context("Element access") { test("get returns correct element") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N2, arrayOf( @@ -228,7 +228,7 @@ class SizedMatrixTest : } test("set modifies element") { - val m = SizedMatrix.zero(N2, N2) + val m = Matrix.zero(N2, N2) m[0, 1] = 5.0 m[0, 1] shouldBe 5.0 } @@ -236,7 +236,7 @@ class SizedMatrixTest : context("Arithmetic operations") { test("unary minus negates all elements") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N2, arrayOf( @@ -244,7 +244,7 @@ class SizedMatrixTest : doubleArrayOf(-3.0, 4.0), ), ) - val neg: SizedMatrix = -m + val neg: Matrix = -m neg[0, 0] shouldBe -1.0 neg[0, 1] shouldBe 2.0 neg[1, 0] shouldBe 3.0 @@ -252,17 +252,17 @@ class SizedMatrixTest : } test("plus adds matrices element-wise") { - val a = SizedMatrix.from( + val a = Matrix.from( N2, N2, arrayOf(doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 4.0)), ) - val b = SizedMatrix.from( + val b = Matrix.from( N2, N2, arrayOf(doubleArrayOf(5.0, 6.0), doubleArrayOf(7.0, 8.0)), ) - val c: SizedMatrix = a + b + val c: Matrix = a + b c[0, 0] shouldBe 6.0 c[0, 1] shouldBe 8.0 c[1, 0] shouldBe 10.0 @@ -270,17 +270,17 @@ class SizedMatrixTest : } test("minus subtracts matrices element-wise") { - val a = SizedMatrix.from( + val a = Matrix.from( N2, N2, arrayOf(doubleArrayOf(5.0, 6.0), doubleArrayOf(7.0, 8.0)), ) - val b = SizedMatrix.from( + val b = Matrix.from( N2, N2, arrayOf(doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 4.0)), ) - val c: SizedMatrix = a - b + val c: Matrix = a - b c[0, 0] shouldBe 4.0 c[0, 1] shouldBe 4.0 c[1, 0] shouldBe 4.0 @@ -288,7 +288,7 @@ class SizedMatrixTest : } test("times multiplies matrices with type-safe dimensions") { - val a: SizedMatrix = SizedMatrix.from( + val a: Matrix = Matrix.from( N2, N3, arrayOf( @@ -296,7 +296,7 @@ class SizedMatrixTest : doubleArrayOf(4.0, 5.0, 6.0), ), ) - val b: SizedMatrix = SizedMatrix.from( + val b: Matrix = Matrix.from( N3, N2, arrayOf( @@ -305,7 +305,7 @@ class SizedMatrixTest : doubleArrayOf(11.0, 12.0), ), ) - val c: SizedMatrix = a * b + val c: Matrix = a * b c.numRows shouldBe 2 c.numColumns shouldBe 2 c[0, 0] shouldBe (1.0 * 7.0 + 2.0 * 9.0 + 3.0 * 11.0) @@ -313,12 +313,12 @@ class SizedMatrixTest : } test("times scalar multiplies all elements") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N2, arrayOf(doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 4.0)), ) - val result: SizedMatrix = m * 2.0 + val result: Matrix = m * 2.0 result[0, 0] shouldBe 2.0 result[0, 1] shouldBe 4.0 result[1, 0] shouldBe 6.0 @@ -326,34 +326,34 @@ class SizedMatrixTest : } test("times int scalar multiplies all elements") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N2, arrayOf(doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 4.0)), ) - val result: SizedMatrix = m * 2 + val result: Matrix = m * 2 result[0, 0] shouldBe 2.0 result[1, 1] shouldBe 8.0 } test("scalar times matrix from left") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N2, arrayOf(doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 4.0)), ) - val result: SizedMatrix = 2.0 * m + val result: Matrix = 2.0 * m result[0, 0] shouldBe 2.0 result[1, 1] shouldBe 8.0 } test("int scalar times matrix from left") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N2, arrayOf(doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 4.0)), ) - val result: SizedMatrix = 2 * m + val result: Matrix = 2 * m result[0, 0] shouldBe 2.0 result[1, 1] shouldBe 8.0 } @@ -361,7 +361,7 @@ class SizedMatrixTest : context("Solve") { test("solve computes solution with type-safe dimensions") { - val a: SizedMatrix = SizedMatrix.from( + val a: Matrix = Matrix.from( N2, N2, arrayOf( @@ -369,8 +369,8 @@ class SizedMatrixTest : doubleArrayOf(1.0, 3.0), ), ) - val b: SizedMatrix = SizedMatrix.column(N2, 5.0, 10.0) - val x: SizedMatrix = a.solve(b) + val b: Matrix = Matrix.column(N2, 5.0, 10.0) + val x: Matrix = a.solve(b) // Verify: A * x ≈ b val result = a * x result[0, 0] shouldBe (5.0 plusOrMinus 1e-9) @@ -380,16 +380,16 @@ class SizedMatrixTest : context("Type safety") { test("matrix multiplication produces correct result type") { - val m2x3: SizedMatrix = SizedMatrix.zero(N2, N3) - val m3x4: SizedMatrix = SizedMatrix.zero(N3, N4) - val m2x4: SizedMatrix = m2x3 * m3x4 + val m2x3: Matrix = Matrix.zero(N2, N3) + val m3x4: Matrix = Matrix.zero(N3, N4) + val m2x4: Matrix = m2x3 * m3x4 m2x4.numRows shouldBe 2 m2x4.numColumns shouldBe 4 } test("transpose produces correct result type") { - val m2x3: SizedMatrix = SizedMatrix.zero(N2, N3) - val m3x2: SizedMatrix = m2x3.transpose + val m2x3: Matrix = Matrix.zero(N2, N3) + val m3x2: Matrix = m2x3.transpose m3x2.numRows shouldBe 3 m3x2.numColumns shouldBe 2 } @@ -397,7 +397,7 @@ class SizedMatrixTest : context("Conversion") { test("toDynamicMatrix converts to dynamic matrix") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N2, arrayOf(doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 4.0)), @@ -412,7 +412,7 @@ class SizedMatrixTest : context("Utility") { test("copy creates independent copy") { - val m = SizedMatrix.from( + val m = Matrix.from( N2, N2, arrayOf(doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 4.0)), @@ -424,17 +424,17 @@ class SizedMatrixTest : } test("equals compares matrices") { - val a = SizedMatrix.from( + val a = Matrix.from( N2, N2, arrayOf(doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 4.0)), ) - val b = SizedMatrix.from( + val b = Matrix.from( N2, N2, arrayOf(doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 4.0)), ) - val c = SizedMatrix.from( + val c = Matrix.from( N2, N2, arrayOf(doubleArrayOf(1.0, 2.0), doubleArrayOf(3.0, 5.0)), @@ -444,8 +444,37 @@ class SizedMatrixTest : } test("toString formats correctly") { - val m = SizedMatrix.identity(N2) + val m = Matrix.identity(N2) m.toString().contains("SizedMatrix<2, 2>") shouldBe true } } + + context("Matrix Exponential") { + test("exp of identity matrix equals e*I") { + val matrix = Matrix.identity(N2) + val result = matrix.exp() + + result[0, 0] shouldBe (Math.E plusOrMinus 1e-9) + result[0, 1] shouldBe (0.0 plusOrMinus 1e-9) + result[1, 0] shouldBe (0.0 plusOrMinus 1e-9) + result[1, 1] shouldBe (Math.E plusOrMinus 1e-9) + } + + test("exp of scaled matrix") { + val matrix = Matrix.from( + N2, + N2, + arrayOf( + doubleArrayOf(1.0, 2.0), + doubleArrayOf(3.0, 4.0), + ), + ) + val result = (matrix * 0.01).exp() + + result[0, 0] shouldBe (1.01035625 plusOrMinus 1e-8) + result[0, 1] shouldBe (0.02050912 plusOrMinus 1e-8) + result[1, 0] shouldBe (0.03076368 plusOrMinus 1e-8) + result[1, 1] shouldBe (1.04111993 plusOrMinus 1e-8) + } + } }) diff --git a/linalg/src/test/kotlin/dev/nextftc/linalg/SizedVectorTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedVectorTest.kt index 408bff8..5efad70 100644 --- a/linalg/src/test/kotlin/dev/nextftc/linalg/SizedVectorTest.kt +++ b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedVectorTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -17,7 +17,7 @@ class SizedVectorTest : FunSpec({ context("Construction") { test("zero vector has all zeros") { - val v = SizedVector.zero(N3) + val v = Vector.zero(N3) v.dimension shouldBe 3 v[0] shouldBe 0.0 v[1] shouldBe 0.0 @@ -25,7 +25,7 @@ class SizedVectorTest : } test("of vararg creates vector with given values") { - val v = SizedVector.of(N3, 1.0, 2.0, 3.0) + val v = Vector.of(N3, 1.0, 2.0, 3.0) v.dimension shouldBe 3 v[0] shouldBe 1.0 v[1] shouldBe 2.0 @@ -33,7 +33,7 @@ class SizedVectorTest : } test("of collection creates vector") { - val v = SizedVector.of(N3, listOf(1.0, 2.0, 3.0)) + val v = Vector.of(N3, listOf(1.0, 2.0, 3.0)) v.dimension shouldBe 3 v[0] shouldBe 1.0 v[1] shouldBe 2.0 @@ -42,7 +42,7 @@ class SizedVectorTest : test("of throws on size mismatch") { shouldThrow { - SizedVector.of(N3, 1.0, 2.0) + Vector.of(N3, 1.0, 2.0) } } @@ -106,7 +106,7 @@ class SizedVectorTest : test("plus adds vectors element-wise") { val a = makeVector(1.0, 2.0, 3.0) val b = makeVector(4.0, 5.0, 6.0) - val c: SizedVector = a + b + val c: Vector = a + b c[0] shouldBe 5.0 c[1] shouldBe 7.0 c[2] shouldBe 9.0 @@ -115,7 +115,7 @@ class SizedVectorTest : test("minus subtracts vectors element-wise") { val a = makeVector(4.0, 5.0, 6.0) val b = makeVector(1.0, 2.0, 3.0) - val c: SizedVector = a - b + val c: Vector = a - b c[0] shouldBe 3.0 c[1] shouldBe 3.0 c[2] shouldBe 3.0 @@ -139,7 +139,7 @@ class SizedVectorTest : test("scalar times vector from left") { val v = makeVector(1.0, 2.0, 3.0) - val result: SizedVector = 2.0 * v + val result: Vector = 2.0 * v result[0] shouldBe 2.0 result[1] shouldBe 4.0 result[2] shouldBe 6.0 @@ -147,7 +147,7 @@ class SizedVectorTest : test("int scalar times vector from left") { val v = makeVector(1.0, 2.0, 3.0) - val result: SizedVector = 2 * v + val result: Vector = 2 * v result[0] shouldBe 2.0 result[1] shouldBe 4.0 result[2] shouldBe 6.0 @@ -177,8 +177,8 @@ class SizedVectorTest : context("Type safety") { test("operations preserve dimension type") { - val v3: SizedVector = makeVector(1.0, 2.0, 3.0) - val result: SizedVector = v3 + v3 + val v3: Vector = makeVector(1.0, 2.0, 3.0) + val result: Vector = v3 + v3 result.dimension shouldBe 3 } } diff --git a/units/src/main/kotlin/dev/nextftc/units/Measure.kt b/units/src/main/kotlin/dev/nextftc/units/Measure.kt index da7eeb0..91299b8 100644 --- a/units/src/main/kotlin/dev/nextftc/units/Measure.kt +++ b/units/src/main/kotlin/dev/nextftc/units/Measure.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -8,7 +8,13 @@ package dev.nextftc.units +import dev.nextftc.units.measuretypes.Mul +import dev.nextftc.units.measuretypes.Per +import dev.nextftc.units.unittypes.MulUnit +import dev.nextftc.units.unittypes.PerUnit import kotlin.math.abs +import kotlin.math.absoluteValue +import kotlin.math.sign import kotlin.math.withSign /** @@ -119,6 +125,14 @@ interface Measure> : Comparable> { val baseUnit get() = unit.baseUnit + /** + * Absolute value of measure. + * + * @return the absolute value of this measure in the same unit as this measure. + */ + val absoluteValue: Measure + get() = unit.of(magnitude.absoluteValue) + /** * Absolute value of measure. * @@ -134,7 +148,21 @@ interface Measure> : Comparable> { * @param unit unit to use * @return the value of the measure in the given unit with the sign of the provided measure */ - fun copySign(other: Measure, unit: U): Double = this.into(unit).withSign(other.into(unit)) + fun withSign(other: Measure, unit: U): Double = this.into(unit).withSign(other.into(unit)) + + /** + * Take the sign of another measure. This measure's and the provided measure's signs are + * considered in this measure's unit. + * + * @param other measure from which to take sign + * @return the value of the measure with the sign of the provided measure + */ + fun withSign(other: Measure) = unit.of(this.into(unit).withSign(other.into(unit))) + + /** + * Returns the sign of this measure. + */ + val sign get() = magnitude.sign /** * Returns a measure equivalent to this one equal to zero minus its current value. For non-linear @@ -183,6 +211,14 @@ interface Measure> : Comparable> { */ operator fun times(multiplier: Double): Measure + /** + * Multiplies this measure by a scalar unitless multiplier. + * + * @param multiplier the scalar multiplication factor + * @return the scaled result + */ + operator fun times(multiplier: Number): Measure = times(multiplier.toDouble()) + /** * Divides this measure by a scalar and returns the result. * @@ -191,6 +227,14 @@ interface Measure> : Comparable> { */ operator fun div(divisor: Double): Measure + /** + * Divides this measure by a scalar and returns the result. + * + * @param divisor the value to divide by + * @return the division result + */ + operator fun div(divisor: Number): Measure = div(divisor.toDouble()) + /** * Divides this measure by another measure and returns the ratio as a dimensionless value. * @@ -210,12 +254,10 @@ interface Measure> : Comparable> { * @param other the other measure to multiply by * @return a Mul measurement representing the product of the two measures */ - operator fun > times(other: Measure): dev.nextftc.units.measuretypes.Mul { + operator fun > times(other: Measure): Mul { val mulUnit = - dev.nextftc.units.unittypes - .MulUnit(this.unit, other.unit) - return dev.nextftc.units.measuretypes - .Mul(this.magnitude * other.magnitude, mulUnit) + MulUnit(this.unit, other.unit) + return Mul(this.magnitude * other.magnitude, mulUnit) } /** @@ -230,12 +272,10 @@ interface Measure> : Comparable> { * @param other the other measure to divide by * @return a Per measurement representing the ratio of the two measures */ - operator fun > div(other: Measure): dev.nextftc.units.measuretypes.Per { + operator fun > div(other: Measure): Per { val perUnit = - dev.nextftc.units.unittypes - .PerUnit(this.unit, other.unit) - return dev.nextftc.units.measuretypes - .Per(this.magnitude / other.magnitude, perUnit) + PerUnit(this.unit, other.unit) + return Per(this.magnitude / other.magnitude, perUnit) } /** @@ -295,7 +335,7 @@ interface Measure> : Comparable> { abs(baseUnitMagnitude - other.baseUnitMagnitude) <= EQUIVALENCE_THRESHOLD } - override fun compareTo(other: Measure): Int = + override operator fun compareTo(other: Measure): Int = this.baseUnitMagnitude.compareTo(other.baseUnitMagnitude) /** diff --git a/units/src/main/kotlin/dev/nextftc/units/Unit.kt b/units/src/main/kotlin/dev/nextftc/units/Unit.kt index 4e4036e..ed485e7 100644 --- a/units/src/main/kotlin/dev/nextftc/units/Unit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/Unit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Angle.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Angle.kt index 1e3cdef..3a3b0b2 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Angle.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Angle.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -81,4 +81,6 @@ internal constructor(override val magnitude: Double, override val unit: AngleUni .AngularVelocityUnit(unit, time.unit) return AngularVelocity(magnitude / time.magnitude, velocityUnit) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularAcceleration.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularAcceleration.kt index 9d9832d..2fc3fc6 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularAcceleration.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularAcceleration.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -59,4 +59,6 @@ class AngularAcceleration(magnitude: Double, unit: AngularAccelerationUnit) : val timeInCorrectUnit = time.into(unit.denominator) return AngularVelocity(magnitude * timeInCorrectUnit, velocityUnit) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularVelocity.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularVelocity.kt index 5fbfc1e..f40a41f 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularVelocity.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularVelocity.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -79,4 +79,6 @@ class AngularVelocity(magnitude: Double, unit: AngularVelocityUnit) : val torqueInNm = torque.baseUnitMagnitude return Power(angularVelocityInRadPerSec * torqueInNm, dev.nextftc.units.unittypes.Watts) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Current.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Current.kt index f92f4eb..382a335 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Current.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Current.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -56,4 +56,6 @@ internal constructor( val voltageInVolts = voltage.baseUnitMagnitude return Power(currentInAmperes * voltageInVolts, Watts) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Distance.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Distance.kt index 5af40e4..ffd2057 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Distance.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Distance.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -97,4 +97,6 @@ internal constructor( val forceInNewtons = force.baseUnitMagnitude return Energy(distanceInMeters * forceInNewtons, dev.nextftc.units.unittypes.Joules) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Energy.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Energy.kt index 23b5ba1..6898340 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Energy.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Energy.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -52,4 +52,6 @@ internal constructor(override val magnitude: Double, override val unit: EnergyUn val timeInSeconds = time.baseUnitMagnitude return Power(energyInJoules / timeInSeconds, Watts) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Force.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Force.kt index d1ad678..9397e46 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Force.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Force.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -69,4 +69,6 @@ internal constructor(override val magnitude: Double, override val unit: ForceUni val distanceInMeters = momentArm.baseUnitMagnitude return Torque(forceInNewtons * distanceInMeters, NewtonMeters) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearAcceleration.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearAcceleration.kt index db47864..bbdb412 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearAcceleration.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearAcceleration.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -72,4 +72,6 @@ class LinearAcceleration(magnitude: Double, unit: LinearAccelerationUnit) : val massInKg = mass.baseUnitMagnitude return Force(accelerationInMps2 * massInKg, dev.nextftc.units.unittypes.Newtons) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearVelocity.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearVelocity.kt index bae3e07..e39800c 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearVelocity.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearVelocity.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -12,7 +12,9 @@ import dev.nextftc.units.Measure import dev.nextftc.units.unittypes.DistanceUnit import dev.nextftc.units.unittypes.LinearVelocityUnit import dev.nextftc.units.unittypes.PerUnit +import dev.nextftc.units.unittypes.Seconds import dev.nextftc.units.unittypes.TimeUnit +import kotlin.time.Duration.Companion.seconds /** * Immutable measurement of linear velocity (distance per time). @@ -65,4 +67,6 @@ class LinearVelocity(magnitude: Double, unit: LinearVelocityUnit) : ) return LinearAcceleration(magnitude / time.magnitude, accelerationUnit) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mass.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mass.kt index a356257..59aefe4 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mass.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mass.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -54,4 +54,6 @@ internal constructor(override val magnitude: Double, override val unit: MassUnit val accelerationInMps2 = acceleration.baseUnitMagnitude return Force(massInKg * accelerationInMps2, Newtons) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mul.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mul.kt index 469f657..320dcf1 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mul.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mul.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -65,4 +65,6 @@ open class Mul, D : Unit>( * @return the division result */ override fun div(divisor: Double): Mul = Mul(magnitude / divisor, unit) + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Per.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Per.kt index e717b8a..b88adbb 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Per.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Per.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -62,4 +62,6 @@ open class Per, D : Unit>( * @return the division result */ override fun div(divisor: Double): Per = Per(magnitude / divisor, unit) + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Power.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Power.kt index 74dc655..3329078 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Power.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Power.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -54,4 +54,6 @@ internal constructor(override val magnitude: Double, override val unit: PowerUni val timeInSeconds = time.baseUnitMagnitude return Energy(powerInWatts * timeInSeconds, Joules) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Temperature.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Temperature.kt index 2b0ae29..ad24940 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Temperature.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Temperature.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -41,4 +41,6 @@ internal constructor( override fun times(multiplier: Double): Temperature = Temperature(magnitude * multiplier, unit) override fun div(divisor: Double): Temperature = Temperature(magnitude / divisor, unit) + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Time.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Time.kt index 3bef61d..edb7a7d 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Time.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Time.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -106,4 +106,6 @@ internal constructor(private val duration: Duration, override val unit: TimeUnit * @return the angular velocity achieved */ operator fun times(acceleration: AngularAcceleration): AngularVelocity = acceleration * this + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Torque.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Torque.kt index d577e16..2c5d482 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Torque.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Torque.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -66,4 +66,6 @@ internal constructor(override val magnitude: Double, override val unit: TorqueUn val angularVelocityInRadPerSec = angularVelocity.baseUnitMagnitude return Power(torqueInNm * angularVelocityInRadPerSec, dev.nextftc.units.unittypes.Watts) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Voltage.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Voltage.kt index 13c44d5..9e69a96 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Voltage.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Voltage.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -85,4 +85,6 @@ internal constructor( val currentInAmperes = current.baseUnitMagnitude return Power(voltageInVolts * currentInAmperes, Watts) } + + override fun toString() = toShortString() } diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/AngleUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/AngleUnit.kt index 01592aa..67279ea 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/AngleUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/AngleUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/AngularAccelerationUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/AngularAccelerationUnit.kt index 3b23723..abd08f2 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/AngularAccelerationUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/AngularAccelerationUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/AngularVelocityUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/AngularVelocityUnit.kt index 471dea9..a7673f7 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/AngularVelocityUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/AngularVelocityUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/CurrentUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/CurrentUnit.kt index 0706a40..e214bcf 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/CurrentUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/CurrentUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/DistanceUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/DistanceUnit.kt index 536c72b..630f01e 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/DistanceUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/DistanceUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/EnergyUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/EnergyUnit.kt index 7b3971d..81df1ad 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/EnergyUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/EnergyUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/ForceUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/ForceUnit.kt index 167ef05..0da6b08 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/ForceUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/ForceUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearAccelerationUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearAccelerationUnit.kt index 3262579..e66a9d4 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearAccelerationUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearAccelerationUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -34,6 +34,7 @@ class LinearAccelerationUnit(velocity: LinearVelocityUnit, time: TimeUnit) : // Common linear acceleration units val MetersPerSecondSquared = LinearAccelerationUnit(MetersPerSecond, Seconds) val FeetPerSecondSquared = LinearAccelerationUnit(FeetPerSecond, Seconds) +val InchesPerSecondSquared = LinearAccelerationUnit(InchesPerSecond, Seconds) // Standard gravity constant (approximately 9.80665 m/s²) const val STANDARD_GRAVITY_MPS2 = 9.80665 @@ -41,3 +42,4 @@ const val STANDARD_GRAVITY_MPS2 = 9.80665 // Extension properties for Double inline val Double.metersPerSecondSquared get() = MetersPerSecondSquared.of(this) inline val Double.feetPerSecondSquared get() = FeetPerSecondSquared.of(this) +inline val Double.inchesPerSecondSquared get() = InchesPerSecondSquared.of(this) diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearVelocityUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearVelocityUnit.kt index e3397be..e471f48 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearVelocityUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearVelocityUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 @@ -37,9 +37,11 @@ val MetersPerSecond = LinearVelocityUnit(Meters, Seconds) val KilometersPerHour = LinearVelocityUnit(Kilometers, Hours) val MilesPerHour = LinearVelocityUnit(Miles, Hours) val FeetPerSecond = LinearVelocityUnit(Feet, Seconds) +val InchesPerSecond = LinearVelocityUnit(Inches, Seconds) // Extension properties for Double inline val Double.metersPerSecond get() = MetersPerSecond.of(this) inline val Double.kilometersPerHour get() = KilometersPerHour.of(this) inline val Double.milesPerHour get() = MilesPerHour.of(this) inline val Double.feetPerSecond get() = FeetPerSecond.of(this) +inline val Double.inchesPerSecond get() = InchesPerSecond.of(this) diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/MassUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/MassUnit.kt index 789d4bc..2d7123e 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/MassUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/MassUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/MulUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/MulUnit.kt index 1036910..e2476b3 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/MulUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/MulUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/PerUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/PerUnit.kt index 72db5bc..59fafbe 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/PerUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/PerUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/PowerUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/PowerUnit.kt index 733dedc..2ded02b 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/PowerUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/PowerUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/TemperatureUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/TemperatureUnit.kt index d7751b4..190ac22 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/TemperatureUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/TemperatureUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/TimeUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/TimeUnit.kt index 49d26a1..efed86a 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/TimeUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/TimeUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/TorqueUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/TorqueUnit.kt index ee3830f..193127c 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/TorqueUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/TorqueUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/unittypes/VoltageUnit.kt b/units/src/main/kotlin/dev/nextftc/units/unittypes/VoltageUnit.kt index c4f1d21..c0b8527 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/VoltageUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/VoltageUnit.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/FunsiesTest.kt b/units/src/test/kotlin/dev/nextftc/units/FunsiesTest.kt deleted file mode 100644 index 0bc64be..0000000 --- a/units/src/test/kotlin/dev/nextftc/units/FunsiesTest.kt +++ /dev/null @@ -1,26 +0,0 @@ -/* - * Copyright (c) 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.units - -import dev.nextftc.units.unittypes.meters -import dev.nextftc.units.unittypes.seconds -import io.kotest.core.spec.style.FunSpec -import io.kotest.matchers.shouldBe - -class FunsiesTest : - FunSpec({ - test("funsies") { - val d = 10.0.meters - val t = 2.0.seconds - - val v = d / t - - v.magnitude shouldBe 5.0 - } - }) diff --git a/units/src/test/kotlin/dev/nextftc/units/MeasureArithmeticTest.kt b/units/src/test/kotlin/dev/nextftc/units/MeasureArithmeticTest.kt index 81ca02f..7764c11 100644 --- a/units/src/test/kotlin/dev/nextftc/units/MeasureArithmeticTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/MeasureArithmeticTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/AngleTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/AngleTest.kt index 145c761..568984d 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/AngleTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/AngleTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/CurrentTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/CurrentTest.kt index a73af5e..87680ea 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/CurrentTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/CurrentTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/DistanceTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/DistanceTest.kt index 3b4217d..e60181b 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/DistanceTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/DistanceTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/EnergyTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/EnergyTest.kt index e64bdee..0b60a2c 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/EnergyTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/EnergyTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/ForceTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/ForceTest.kt index 5fcdbaa..6347a56 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/ForceTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/ForceTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/MassTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/MassTest.kt index 2b52d9e..6995e43 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/MassTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/MassTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/MulTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/MulTest.kt index a298ae8..4094884 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/MulTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/MulTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/PerTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/PerTest.kt index 19f4a7c..6121e1f 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/PerTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/PerTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/PhysicalRelationshipsTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/PhysicalRelationshipsTest.kt index d3c19f8..a3ee509 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/PhysicalRelationshipsTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/PhysicalRelationshipsTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/PowerTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/PowerTest.kt index 9a7077f..b1df5c0 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/PowerTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/PowerTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/TemperatureTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/TemperatureTest.kt index 6c0fe59..7f8ec4b 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/TemperatureTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/TemperatureTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/TimeTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/TimeTest.kt index ce1c669..29586be 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/TimeTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/TimeTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/TorqueTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/TorqueTest.kt index c0877e3..d24cf09 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/TorqueTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/TorqueTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/measuretypes/VoltageTest.kt b/units/src/test/kotlin/dev/nextftc/units/measuretypes/VoltageTest.kt index 4198f3e..92197da 100644 --- a/units/src/test/kotlin/dev/nextftc/units/measuretypes/VoltageTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/measuretypes/VoltageTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/AngleUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/AngleUnitTest.kt index 34adc98..73db0d4 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/AngleUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/AngleUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/CurrentUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/CurrentUnitTest.kt index d16133e..28bd7d6 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/CurrentUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/CurrentUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/DistanceUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/DistanceUnitTest.kt index daf052b..d43d731 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/DistanceUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/DistanceUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/EnergyUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/EnergyUnitTest.kt index 3fb273f..f166aff 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/EnergyUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/EnergyUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/ForceUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/ForceUnitTest.kt index e1ffd29..f47d34e 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/ForceUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/ForceUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/MassUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/MassUnitTest.kt index 0ed672d..daf31c1 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/MassUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/MassUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/MulUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/MulUnitTest.kt index 858b546..804d80a 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/MulUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/MulUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/PerUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/PerUnitTest.kt index bf60070..f1cc6ba 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/PerUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/PerUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/PowerUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/PowerUnitTest.kt index dcd8bf8..fb29454 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/PowerUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/PowerUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/TemperatureUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/TemperatureUnitTest.kt index 3ecd630..1353ab9 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/TemperatureUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/TemperatureUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/TimeUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/TimeUnitTest.kt index 1364e3d..5aec2f1 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/TimeUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/TimeUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/TorqueUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/TorqueUnitTest.kt index e8baab8..2f321e4 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/TorqueUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/TorqueUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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 diff --git a/units/src/test/kotlin/dev/nextftc/units/unittypes/VoltageUnitTest.kt b/units/src/test/kotlin/dev/nextftc/units/unittypes/VoltageUnitTest.kt index c88fb67..ae12370 100644 --- a/units/src/test/kotlin/dev/nextftc/units/unittypes/VoltageUnitTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/unittypes/VoltageUnitTest.kt @@ -1,5 +1,5 @@ /* - * Copyright (c) NextFTC Team + * 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