From d2b75e98893ceca54b17a5f425f49d3c05ea31dc Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 16 Dec 2025 11:31:38 -0500 Subject: [PATCH 01/28] add: configure testing dependencies for control module and XML file updates Signed-off-by: Zach Harel --- .idea/misc.xml | 1 + .idea/vcs.xml | 2 +- control/build.gradle.kts | 4 ++++ 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/.idea/misc.xml b/.idea/misc.xml index 9e0e519..04d8e09 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,3 +1,4 @@ + diff --git a/.idea/vcs.xml b/.idea/vcs.xml index 94a25f7..35eb1dd 100644 --- a/.idea/vcs.xml +++ b/.idea/vcs.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/control/build.gradle.kts b/control/build.gradle.kts index 189b634..86d2e9c 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( From 80a00207926507bc59d1e552687521bf8b7b700d Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 16 Dec 2025 13:22:51 -0500 Subject: [PATCH 02/28] add: implement AngularState and LinearState classes for kinematic motion representation Signed-off-by: Zach Harel --- .../nextftc/control/states/AngularState.kt | 110 ++++++++++++++++++ .../dev/nextftc/control/states/LinearState.kt | 106 +++++++++++++++++ .../dev/nextftc/control/states/State.kt | 79 +++++++++++++ 3 files changed, 295 insertions(+) create mode 100644 control/src/main/kotlin/dev/nextftc/control/states/AngularState.kt create mode 100644 control/src/main/kotlin/dev/nextftc/control/states/LinearState.kt create mode 100644 control/src/main/kotlin/dev/nextftc/control/states/State.kt diff --git a/control/src/main/kotlin/dev/nextftc/control/states/AngularState.kt b/control/src/main/kotlin/dev/nextftc/control/states/AngularState.kt new file mode 100644 index 0000000..2998729 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/states/AngularState.kt @@ -0,0 +1,110 @@ +/* + * 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.control.states + +import dev.nextftc.units.measuretypes.Angle +import dev.nextftc.units.measuretypes.AngularAcceleration +import dev.nextftc.units.measuretypes.AngularVelocity +import dev.nextftc.units.unittypes.AngleUnit +import dev.nextftc.units.unittypes.radians +import dev.nextftc.units.unittypes.radiansPerSecond +import dev.nextftc.units.unittypes.radiansPerSecondSquared + +/** + * Represents the kinematic state of an object in angular (rotational) motion. + * + * This data class encapsulates angle, angular velocity, and angular acceleration for rotational motion, + * providing a convenient way to track and manipulate rotational state in control systems. + * + * @property angle The angular position/orientation of the object. + * @property velocity The angular velocity of the object. + * @property acceleration The angular acceleration of the object. + * @constructor Creates an AngularState with the specified angle, velocity, and acceleration. + * All parameters default to zero. + */ +data class AngularState @JvmOverloads constructor( + val angle: Angle = 0.0.radians, + override val velocity: AngularVelocity = 0.0.radiansPerSecond, + override val acceleration: AngularAcceleration = 0.0.radiansPerSecondSquared, +) : State { + /** + * The angular position of this state. Alias for [angle]. + */ + override val position: Angle get() = angle + /** + * Creates an AngularState from raw double values. + * + * **All values are interpreted as radians-based units:** + * - [angle] in **radians** + * - [velocity] in **radians per second** + * - [acceleration] in **radians per second squared** + * + * @param angle The angle in radians. + * @param velocity The angular velocity in radians per second. + * @param acceleration The angular acceleration in radians per second squared. + */ + constructor( + angle: Double, + velocity: Double, + acceleration: Double, + ) : this( + angle.radians, + velocity.radiansPerSecond, + acceleration.radiansPerSecondSquared, + ) + + /** + * Adds two angular states component-wise. + * @param other The state to add to this state. + * @return A new [AngularState] with summed components. + */ + override operator fun plus(other: AngularState): AngularState = AngularState( + angle + other.angle, + velocity + other.velocity, + acceleration + other.acceleration, + ) + + /** + * Subtracts another angular state from this one component-wise. + * @param other The state to subtract from this state. + * @return A new [AngularState] with the difference of components. + */ + override operator fun minus(other: AngularState): AngularState = AngularState( + angle - other.angle, + velocity - other.velocity, + acceleration - other.acceleration, + ) + + /** + * Multiplies all components of this state by a scalar value. + * @param scalar The value to multiply each component by. + * @return A new [AngularState] with scaled components. + */ + override operator fun times(scalar: Double): AngularState = AngularState( + angle * scalar, + velocity * scalar, + acceleration * scalar, + ) + + /** + * Divides all components of this state by a scalar value. + * @param scalar The value to divide each component by. + * @return A new [AngularState] with divided components. + */ + override operator fun div(scalar: Double): AngularState = AngularState( + angle / scalar, + velocity / scalar, + acceleration / scalar, + ) + + companion object { + /** An [AngularState] with all components set to zero. */ + @JvmField val ZERO = AngularState() + } +} \ No newline at end of file diff --git a/control/src/main/kotlin/dev/nextftc/control/states/LinearState.kt b/control/src/main/kotlin/dev/nextftc/control/states/LinearState.kt new file mode 100644 index 0000000..f097531 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/states/LinearState.kt @@ -0,0 +1,106 @@ +/* + * 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.control.states + +import dev.nextftc.units.measuretypes.Distance +import dev.nextftc.units.measuretypes.LinearAcceleration +import dev.nextftc.units.measuretypes.LinearVelocity +import dev.nextftc.units.unittypes.DistanceUnit +import dev.nextftc.units.unittypes.inches +import dev.nextftc.units.unittypes.inchesPerSecond +import dev.nextftc.units.unittypes.inchesPerSecondSquared + +/** + * Represents the kinematic state of an object in linear (translational) motion. + * + * This data class encapsulates position, velocity, and acceleration for linear motion, + * providing a convenient way to track and manipulate motion state in control systems. + * + * @property position The linear position/displacement of the object. + * @property velocity The linear velocity of the object. + * @property acceleration The linear acceleration of the object. + * @constructor Creates a LinearState with the specified position, velocity, and acceleration. + * All parameters default to zero. + */ +data class LinearState @JvmOverloads constructor( + override val position: Distance = 0.0.inches, + override val velocity: LinearVelocity = 0.0.inchesPerSecond, + override val acceleration: LinearAcceleration = 0.0.inchesPerSecondSquared, +) : State { + /** + * Creates a LinearState from raw double values. + * + * **All values are interpreted as inches-based units:** + * - [position] in **inches** + * - [velocity] in **inches per second** + * - [acceleration] in **inches per second squared** + * + * @param position The position in inches. + * @param velocity The velocity in inches per second. + * @param acceleration The acceleration in inches per second squared. + */ + constructor( + position: Double, + velocity: Double, + acceleration: Double, + ) : this( + position.inches, + velocity.inchesPerSecond, + acceleration.inchesPerSecondSquared, + ) + + /** + * Adds two linear states component-wise. + * @param other The state to add to this state. + * @return A new [LinearState] with summed components. + */ + override operator fun plus(other: LinearState): LinearState = LinearState( + position + other.position, + velocity + other.velocity, + acceleration + other.acceleration, + ) + + /** + * Subtracts another linear state from this one component-wise. + * @param other The state to subtract from this state. + * @return A new [LinearState] with the difference of components. + */ + override operator fun minus(other: LinearState): LinearState = LinearState( + position - other.position, + velocity - other.velocity, + acceleration - other.acceleration, + ) + + /** + * Multiplies all components of this state by a scalar value. + * @param scalar The value to multiply each component by. + * @return A new [LinearState] with scaled components. + */ + override operator fun times(scalar: Double): LinearState = LinearState( + position * scalar, + velocity * scalar, + acceleration * scalar, + ) + + /** + * Divides all components of this state by a scalar value. + * @param scalar The value to divide each component by. + * @return A new [LinearState] with divided components. + */ + override operator fun div(scalar: Double): LinearState = LinearState( + position / scalar, + velocity / scalar, + acceleration / scalar, + ) + + companion object { + /** A [LinearState] with all components set to zero. */ + @JvmField val ZERO = LinearState() + } +} \ No newline at end of file diff --git a/control/src/main/kotlin/dev/nextftc/control/states/State.kt b/control/src/main/kotlin/dev/nextftc/control/states/State.kt new file mode 100644 index 0000000..95c28a4 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/states/State.kt @@ -0,0 +1,79 @@ +/* + * 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.control.states + +import dev.nextftc.units.Measure +import dev.nextftc.units.Unit +import dev.nextftc.units.measuretypes.Per +import dev.nextftc.units.unittypes.PerUnit +import dev.nextftc.units.unittypes.TimeUnit + +/** + * Represents a kinematic state with position, velocity, and acceleration. + * + * This generic interface defines the contract for kinematic state representations, + * parameterized by the unit type [U]. It provides position, velocity (first derivative), + * and acceleration (second derivative) properties, along with arithmetic operations + * for combining and scaling states. + * + * Implementations include: + * - [LinearState]: For linear (translational) motion using distance units + * - [AngularState]: For angular (rotational) motion using angle units + * + * @param U The unit type for the position measurement (e.g., [dev.nextftc.units.unittypes.DistanceUnit], + * [dev.nextftc.units.unittypes.AngleUnit]) + * @param S The self type of the implementing class, used for covariant return types + * + * @see LinearState + * @see AngularState + */ +interface State, S : State> { + /** + * The position component of this state. + */ + val position: Measure + + /** + * The velocity component of this state (first time derivative of position). + */ + val velocity: Per + + /** + * The acceleration component of this state (second time derivative of position). + */ + val acceleration: Per, TimeUnit> + + /** + * Adds another state to this one component-wise. + * @param other The state to add. + * @return A new state with summed components. + */ + operator fun plus(other: S): S + + /** + * Subtracts another state from this one component-wise. + * @param other The state to subtract. + * @return A new state with the difference of components. + */ + operator fun minus(other: S): S + + /** + * Multiplies all components of this state by a scalar value. + * @param scalar The value to multiply each component by. + * @return A new state with scaled components. + */ + operator fun times(scalar: Double): S + + /** + * Divides all components of this state by a scalar value. + * @param scalar The value to divide each component by. + * @return A new state with divided components. + */ + operator fun div(scalar: Double): S +} \ No newline at end of file From 47696ee08b4b8125719fac0d3adf8e32a7ed284d Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 16 Dec 2025 13:23:03 -0500 Subject: [PATCH 03/28] add: introduce InchesPerSecond and inchesPerSecond extension for linear units Signed-off-by: Zach Harel --- units/src/main/kotlin/dev/nextftc/units/Measure.kt | 6 ++++++ .../dev/nextftc/units/unittypes/LinearAccelerationUnit.kt | 2 ++ .../dev/nextftc/units/unittypes/LinearVelocityUnit.kt | 2 ++ 3 files changed, 10 insertions(+) diff --git a/units/src/main/kotlin/dev/nextftc/units/Measure.kt b/units/src/main/kotlin/dev/nextftc/units/Measure.kt index da7eeb0..0369140 100644 --- a/units/src/main/kotlin/dev/nextftc/units/Measure.kt +++ b/units/src/main/kotlin/dev/nextftc/units/Measure.kt @@ -9,6 +9,7 @@ package dev.nextftc.units import kotlin.math.abs +import kotlin.math.sign import kotlin.math.withSign /** @@ -136,6 +137,11 @@ interface Measure> : Comparable> { */ fun copySign(other: Measure, unit: U): Double = 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 * unit types like temperature, the zero point is treated as the zero value of the base unit (eg 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..3c08577 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearAccelerationUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearAccelerationUnit.kt @@ -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..844a64d 100644 --- a/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearVelocityUnit.kt +++ b/units/src/main/kotlin/dev/nextftc/units/unittypes/LinearVelocityUnit.kt @@ -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) From ae55d987b62364b2a0bd738004bc198bc4b39331 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Wed, 17 Dec 2025 13:59:54 -0500 Subject: [PATCH 04/28] remove state classes Signed-off-by: Zach Harel --- .idea/codeStyles/codeStyleConfig.xml | 5 + .../nextftc/control/states/AngularState.kt | 110 ------------------ .../dev/nextftc/control/states/LinearState.kt | 106 ----------------- .../dev/nextftc/control/states/State.kt | 79 ------------- 4 files changed, 5 insertions(+), 295 deletions(-) create mode 100644 .idea/codeStyles/codeStyleConfig.xml delete mode 100644 control/src/main/kotlin/dev/nextftc/control/states/AngularState.kt delete mode 100644 control/src/main/kotlin/dev/nextftc/control/states/LinearState.kt delete mode 100644 control/src/main/kotlin/dev/nextftc/control/states/State.kt 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/control/src/main/kotlin/dev/nextftc/control/states/AngularState.kt b/control/src/main/kotlin/dev/nextftc/control/states/AngularState.kt deleted file mode 100644 index 2998729..0000000 --- a/control/src/main/kotlin/dev/nextftc/control/states/AngularState.kt +++ /dev/null @@ -1,110 +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.control.states - -import dev.nextftc.units.measuretypes.Angle -import dev.nextftc.units.measuretypes.AngularAcceleration -import dev.nextftc.units.measuretypes.AngularVelocity -import dev.nextftc.units.unittypes.AngleUnit -import dev.nextftc.units.unittypes.radians -import dev.nextftc.units.unittypes.radiansPerSecond -import dev.nextftc.units.unittypes.radiansPerSecondSquared - -/** - * Represents the kinematic state of an object in angular (rotational) motion. - * - * This data class encapsulates angle, angular velocity, and angular acceleration for rotational motion, - * providing a convenient way to track and manipulate rotational state in control systems. - * - * @property angle The angular position/orientation of the object. - * @property velocity The angular velocity of the object. - * @property acceleration The angular acceleration of the object. - * @constructor Creates an AngularState with the specified angle, velocity, and acceleration. - * All parameters default to zero. - */ -data class AngularState @JvmOverloads constructor( - val angle: Angle = 0.0.radians, - override val velocity: AngularVelocity = 0.0.radiansPerSecond, - override val acceleration: AngularAcceleration = 0.0.radiansPerSecondSquared, -) : State { - /** - * The angular position of this state. Alias for [angle]. - */ - override val position: Angle get() = angle - /** - * Creates an AngularState from raw double values. - * - * **All values are interpreted as radians-based units:** - * - [angle] in **radians** - * - [velocity] in **radians per second** - * - [acceleration] in **radians per second squared** - * - * @param angle The angle in radians. - * @param velocity The angular velocity in radians per second. - * @param acceleration The angular acceleration in radians per second squared. - */ - constructor( - angle: Double, - velocity: Double, - acceleration: Double, - ) : this( - angle.radians, - velocity.radiansPerSecond, - acceleration.radiansPerSecondSquared, - ) - - /** - * Adds two angular states component-wise. - * @param other The state to add to this state. - * @return A new [AngularState] with summed components. - */ - override operator fun plus(other: AngularState): AngularState = AngularState( - angle + other.angle, - velocity + other.velocity, - acceleration + other.acceleration, - ) - - /** - * Subtracts another angular state from this one component-wise. - * @param other The state to subtract from this state. - * @return A new [AngularState] with the difference of components. - */ - override operator fun minus(other: AngularState): AngularState = AngularState( - angle - other.angle, - velocity - other.velocity, - acceleration - other.acceleration, - ) - - /** - * Multiplies all components of this state by a scalar value. - * @param scalar The value to multiply each component by. - * @return A new [AngularState] with scaled components. - */ - override operator fun times(scalar: Double): AngularState = AngularState( - angle * scalar, - velocity * scalar, - acceleration * scalar, - ) - - /** - * Divides all components of this state by a scalar value. - * @param scalar The value to divide each component by. - * @return A new [AngularState] with divided components. - */ - override operator fun div(scalar: Double): AngularState = AngularState( - angle / scalar, - velocity / scalar, - acceleration / scalar, - ) - - companion object { - /** An [AngularState] with all components set to zero. */ - @JvmField val ZERO = AngularState() - } -} \ No newline at end of file diff --git a/control/src/main/kotlin/dev/nextftc/control/states/LinearState.kt b/control/src/main/kotlin/dev/nextftc/control/states/LinearState.kt deleted file mode 100644 index f097531..0000000 --- a/control/src/main/kotlin/dev/nextftc/control/states/LinearState.kt +++ /dev/null @@ -1,106 +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.control.states - -import dev.nextftc.units.measuretypes.Distance -import dev.nextftc.units.measuretypes.LinearAcceleration -import dev.nextftc.units.measuretypes.LinearVelocity -import dev.nextftc.units.unittypes.DistanceUnit -import dev.nextftc.units.unittypes.inches -import dev.nextftc.units.unittypes.inchesPerSecond -import dev.nextftc.units.unittypes.inchesPerSecondSquared - -/** - * Represents the kinematic state of an object in linear (translational) motion. - * - * This data class encapsulates position, velocity, and acceleration for linear motion, - * providing a convenient way to track and manipulate motion state in control systems. - * - * @property position The linear position/displacement of the object. - * @property velocity The linear velocity of the object. - * @property acceleration The linear acceleration of the object. - * @constructor Creates a LinearState with the specified position, velocity, and acceleration. - * All parameters default to zero. - */ -data class LinearState @JvmOverloads constructor( - override val position: Distance = 0.0.inches, - override val velocity: LinearVelocity = 0.0.inchesPerSecond, - override val acceleration: LinearAcceleration = 0.0.inchesPerSecondSquared, -) : State { - /** - * Creates a LinearState from raw double values. - * - * **All values are interpreted as inches-based units:** - * - [position] in **inches** - * - [velocity] in **inches per second** - * - [acceleration] in **inches per second squared** - * - * @param position The position in inches. - * @param velocity The velocity in inches per second. - * @param acceleration The acceleration in inches per second squared. - */ - constructor( - position: Double, - velocity: Double, - acceleration: Double, - ) : this( - position.inches, - velocity.inchesPerSecond, - acceleration.inchesPerSecondSquared, - ) - - /** - * Adds two linear states component-wise. - * @param other The state to add to this state. - * @return A new [LinearState] with summed components. - */ - override operator fun plus(other: LinearState): LinearState = LinearState( - position + other.position, - velocity + other.velocity, - acceleration + other.acceleration, - ) - - /** - * Subtracts another linear state from this one component-wise. - * @param other The state to subtract from this state. - * @return A new [LinearState] with the difference of components. - */ - override operator fun minus(other: LinearState): LinearState = LinearState( - position - other.position, - velocity - other.velocity, - acceleration - other.acceleration, - ) - - /** - * Multiplies all components of this state by a scalar value. - * @param scalar The value to multiply each component by. - * @return A new [LinearState] with scaled components. - */ - override operator fun times(scalar: Double): LinearState = LinearState( - position * scalar, - velocity * scalar, - acceleration * scalar, - ) - - /** - * Divides all components of this state by a scalar value. - * @param scalar The value to divide each component by. - * @return A new [LinearState] with divided components. - */ - override operator fun div(scalar: Double): LinearState = LinearState( - position / scalar, - velocity / scalar, - acceleration / scalar, - ) - - companion object { - /** A [LinearState] with all components set to zero. */ - @JvmField val ZERO = LinearState() - } -} \ No newline at end of file diff --git a/control/src/main/kotlin/dev/nextftc/control/states/State.kt b/control/src/main/kotlin/dev/nextftc/control/states/State.kt deleted file mode 100644 index 95c28a4..0000000 --- a/control/src/main/kotlin/dev/nextftc/control/states/State.kt +++ /dev/null @@ -1,79 +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.control.states - -import dev.nextftc.units.Measure -import dev.nextftc.units.Unit -import dev.nextftc.units.measuretypes.Per -import dev.nextftc.units.unittypes.PerUnit -import dev.nextftc.units.unittypes.TimeUnit - -/** - * Represents a kinematic state with position, velocity, and acceleration. - * - * This generic interface defines the contract for kinematic state representations, - * parameterized by the unit type [U]. It provides position, velocity (first derivative), - * and acceleration (second derivative) properties, along with arithmetic operations - * for combining and scaling states. - * - * Implementations include: - * - [LinearState]: For linear (translational) motion using distance units - * - [AngularState]: For angular (rotational) motion using angle units - * - * @param U The unit type for the position measurement (e.g., [dev.nextftc.units.unittypes.DistanceUnit], - * [dev.nextftc.units.unittypes.AngleUnit]) - * @param S The self type of the implementing class, used for covariant return types - * - * @see LinearState - * @see AngularState - */ -interface State, S : State> { - /** - * The position component of this state. - */ - val position: Measure - - /** - * The velocity component of this state (first time derivative of position). - */ - val velocity: Per - - /** - * The acceleration component of this state (second time derivative of position). - */ - val acceleration: Per, TimeUnit> - - /** - * Adds another state to this one component-wise. - * @param other The state to add. - * @return A new state with summed components. - */ - operator fun plus(other: S): S - - /** - * Subtracts another state from this one component-wise. - * @param other The state to subtract. - * @return A new state with the difference of components. - */ - operator fun minus(other: S): S - - /** - * Multiplies all components of this state by a scalar value. - * @param scalar The value to multiply each component by. - * @return A new state with scaled components. - */ - operator fun times(scalar: Double): S - - /** - * Divides all components of this state by a scalar value. - * @param scalar The value to divide each component by. - * @return A new state with divided components. - */ - operator fun div(scalar: Double): S -} \ No newline at end of file From a9b543d7c6694bf3b802b746f861a24ff6e0b971 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Wed, 17 Dec 2025 14:00:01 -0500 Subject: [PATCH 05/28] add: implement constructor for SizedVector from SizedMatrix Signed-off-by: Zach Harel --- linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt index 4fa5866..e5b257a 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt @@ -35,6 +35,8 @@ class SizedVector internal constructor(simple: SimpleMatrix, internal v require(simple.numCols() == 1) { "Vector must have exactly one column" } } + constructor(matrix: SizedMatrix) : this(matrix.simple, matrix.rowNat) + @Suppress("ktlint") companion object { /** From 25d23c4d47954314cc23533b4a95e916ad9151da Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 13:04:37 -0500 Subject: [PATCH 06/28] reformat copyright again Signed-off-by: Zach Harel --- .idea/copyright/BSD_3_License_header.xml | 2 +- .idea/copyright/profiles_settings.xml | 4 ++-- linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt | 2 +- .../src/main/kotlin/dev/nextftc/linalg/Decompositions.kt | 2 +- .../src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt | 2 +- .../src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt | 2 +- linalg/src/main/kotlin/dev/nextftc/linalg/Matrices.kt | 2 +- linalg/src/main/kotlin/dev/nextftc/linalg/Nat.kt | 2 +- linalg/src/main/kotlin/dev/nextftc/linalg/SizedMatrix.kt | 2 +- linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt | 2 +- .../test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt | 2 +- .../test/kotlin/dev/nextftc/linalg/DynamicVectorTest.kt | 2 +- .../test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt | 2 +- .../test/kotlin/dev/nextftc/linalg/SizedVectorTest.kt | 2 +- units/src/main/kotlin/dev/nextftc/units/Measure.kt | 2 +- units/src/main/kotlin/dev/nextftc/units/Unit.kt | 2 +- .../main/kotlin/dev/nextftc/units/measuretypes/Angle.kt | 2 +- .../nextftc/units/measuretypes/AngularAcceleration.kt | 2 +- .../dev/nextftc/units/measuretypes/AngularVelocity.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/Current.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/Distance.kt | 2 +- .../main/kotlin/dev/nextftc/units/measuretypes/Energy.kt | 2 +- .../main/kotlin/dev/nextftc/units/measuretypes/Force.kt | 2 +- .../dev/nextftc/units/measuretypes/LinearAcceleration.kt | 2 +- .../dev/nextftc/units/measuretypes/LinearVelocity.kt | 2 +- .../main/kotlin/dev/nextftc/units/measuretypes/Mass.kt | 2 +- .../main/kotlin/dev/nextftc/units/measuretypes/Mul.kt | 2 +- .../main/kotlin/dev/nextftc/units/measuretypes/Per.kt | 2 +- .../main/kotlin/dev/nextftc/units/measuretypes/Power.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/Temperature.kt | 2 +- .../main/kotlin/dev/nextftc/units/measuretypes/Time.kt | 2 +- .../main/kotlin/dev/nextftc/units/measuretypes/Torque.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/Voltage.kt | 2 +- .../main/kotlin/dev/nextftc/units/unittypes/AngleUnit.kt | 2 +- .../nextftc/units/unittypes/AngularAccelerationUnit.kt | 2 +- .../dev/nextftc/units/unittypes/AngularVelocityUnit.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/CurrentUnit.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/DistanceUnit.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/EnergyUnit.kt | 2 +- .../main/kotlin/dev/nextftc/units/unittypes/ForceUnit.kt | 2 +- .../nextftc/units/unittypes/LinearAccelerationUnit.kt | 2 +- .../dev/nextftc/units/unittypes/LinearVelocityUnit.kt | 2 +- .../main/kotlin/dev/nextftc/units/unittypes/MassUnit.kt | 2 +- .../main/kotlin/dev/nextftc/units/unittypes/MulUnit.kt | 2 +- .../main/kotlin/dev/nextftc/units/unittypes/PerUnit.kt | 2 +- .../main/kotlin/dev/nextftc/units/unittypes/PowerUnit.kt | 2 +- .../dev/nextftc/units/unittypes/TemperatureUnit.kt | 2 +- .../main/kotlin/dev/nextftc/units/unittypes/TimeUnit.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/TorqueUnit.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/VoltageUnit.kt | 2 +- units/src/test/kotlin/dev/nextftc/units/FunsiesTest.kt | 9 ++------- .../kotlin/dev/nextftc/units/MeasureArithmeticTest.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/AngleTest.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/CurrentTest.kt | 2 +- .../dev/nextftc/units/measuretypes/DistanceTest.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/EnergyTest.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/ForceTest.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/MassTest.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/MulTest.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/PerTest.kt | 2 +- .../units/measuretypes/PhysicalRelationshipsTest.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/PowerTest.kt | 2 +- .../dev/nextftc/units/measuretypes/TemperatureTest.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/TimeTest.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/TorqueTest.kt | 2 +- .../kotlin/dev/nextftc/units/measuretypes/VoltageTest.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/AngleUnitTest.kt | 2 +- .../dev/nextftc/units/unittypes/CurrentUnitTest.kt | 2 +- .../dev/nextftc/units/unittypes/DistanceUnitTest.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/EnergyUnitTest.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/ForceUnitTest.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/MassUnitTest.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/MulUnitTest.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/PerUnitTest.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/PowerUnitTest.kt | 2 +- .../dev/nextftc/units/unittypes/TemperatureUnitTest.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/TimeUnitTest.kt | 2 +- .../kotlin/dev/nextftc/units/unittypes/TorqueUnitTest.kt | 2 +- .../dev/nextftc/units/unittypes/VoltageUnitTest.kt | 2 +- 79 files changed, 81 insertions(+), 86 deletions(-) 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/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt index 8287814..b43e380 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 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..61d063d 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 diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt index 1faecf1..8d22164 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 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/Nat.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Nat.kt index 7f53749..348e981 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 diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/SizedMatrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/SizedMatrix.kt index 8459def..4067251 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/SizedMatrix.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/SizedMatrix.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/SizedVector.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt index e5b257a..12f0cfa 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.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/DynamicMatrixTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt index d7dafd4..2a34319 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 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..31661e3 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 diff --git a/linalg/src/test/kotlin/dev/nextftc/linalg/SizedVectorTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedVectorTest.kt index 408bff8..7612b43 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 diff --git a/units/src/main/kotlin/dev/nextftc/units/Measure.kt b/units/src/main/kotlin/dev/nextftc/units/Measure.kt index 0369140..2bb3898 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 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..9ae5f47 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 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..fbb6c27 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 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..05475c9 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 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..3073376 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 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..ade6ec6 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 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..6de0fba 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 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..d7b74c1 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 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..2a9ea89 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 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..64e1ce4 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 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..e43109d 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 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..9edd63d 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 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..719e16e 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 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..e72fd41 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 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..dfd5256 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 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..98084e6 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 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..4ea0be4 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 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..30f57ef 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 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 3c08577..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 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 844a64d..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 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 index 0bc64be..db6e7db 100644 --- a/units/src/test/kotlin/dev/nextftc/units/FunsiesTest.kt +++ b/units/src/test/kotlin/dev/nextftc/units/FunsiesTest.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 @@ -16,11 +16,6 @@ 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 + println("Elphaba Two Drivers".toByteArray().size) } }) 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 From 4605e5c8fc93ebb89e36baadd60e71151d18575e Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 20:41:46 -0500 Subject: [PATCH 07/28] add: override toString() method to return short string representation for various unit classes Signed-off-by: Zach Harel --- .../main/kotlin/dev/nextftc/units/Measure.kt | 58 +++++++++++++++---- .../dev/nextftc/units/measuretypes/Angle.kt | 2 + .../units/measuretypes/AngularAcceleration.kt | 2 + .../units/measuretypes/AngularVelocity.kt | 2 + .../dev/nextftc/units/measuretypes/Current.kt | 2 + .../nextftc/units/measuretypes/Distance.kt | 2 + .../dev/nextftc/units/measuretypes/Energy.kt | 2 + .../dev/nextftc/units/measuretypes/Force.kt | 2 + .../units/measuretypes/LinearAcceleration.kt | 2 + .../units/measuretypes/LinearVelocity.kt | 4 ++ .../dev/nextftc/units/measuretypes/Mass.kt | 2 + .../dev/nextftc/units/measuretypes/Mul.kt | 2 + .../dev/nextftc/units/measuretypes/Per.kt | 2 + .../dev/nextftc/units/measuretypes/Power.kt | 2 + .../nextftc/units/measuretypes/Temperature.kt | 2 + .../dev/nextftc/units/measuretypes/Time.kt | 2 + .../dev/nextftc/units/measuretypes/Torque.kt | 2 + .../dev/nextftc/units/measuretypes/Voltage.kt | 2 + .../kotlin/dev/nextftc/units/FunsiesTest.kt | 21 ------- 19 files changed, 82 insertions(+), 33 deletions(-) delete mode 100644 units/src/test/kotlin/dev/nextftc/units/FunsiesTest.kt diff --git a/units/src/main/kotlin/dev/nextftc/units/Measure.kt b/units/src/main/kotlin/dev/nextftc/units/Measure.kt index 2bb3898..91299b8 100644 --- a/units/src/main/kotlin/dev/nextftc/units/Measure.kt +++ b/units/src/main/kotlin/dev/nextftc/units/Measure.kt @@ -8,7 +8,12 @@ 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 @@ -120,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. * @@ -135,7 +148,16 @@ 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. @@ -189,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. * @@ -197,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. * @@ -216,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) } /** @@ -236,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) } /** @@ -301,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/measuretypes/Angle.kt b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Angle.kt index 9ae5f47..3a3b0b2 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Angle.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Angle.kt @@ -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 fbb6c27..2fc3fc6 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularAcceleration.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularAcceleration.kt @@ -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 05475c9..f40a41f 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularVelocity.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/AngularVelocity.kt @@ -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 3073376..382a335 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Current.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Current.kt @@ -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 ade6ec6..ffd2057 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Distance.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Distance.kt @@ -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 6de0fba..6898340 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Energy.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Energy.kt @@ -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 d7b74c1..9397e46 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Force.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Force.kt @@ -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 2a9ea89..bbdb412 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearAcceleration.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearAcceleration.kt @@ -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 64e1ce4..e39800c 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearVelocity.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/LinearVelocity.kt @@ -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 e43109d..59aefe4 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mass.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mass.kt @@ -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 9edd63d..320dcf1 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mul.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Mul.kt @@ -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 719e16e..b88adbb 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Per.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Per.kt @@ -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 e72fd41..3329078 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Power.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Power.kt @@ -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 dfd5256..ad24940 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Temperature.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Temperature.kt @@ -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 98084e6..edb7a7d 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Time.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Time.kt @@ -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 4ea0be4..2c5d482 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Torque.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Torque.kt @@ -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 30f57ef..9e69a96 100644 --- a/units/src/main/kotlin/dev/nextftc/units/measuretypes/Voltage.kt +++ b/units/src/main/kotlin/dev/nextftc/units/measuretypes/Voltage.kt @@ -85,4 +85,6 @@ internal constructor( val currentInAmperes = current.baseUnitMagnitude return Power(voltageInVolts * currentInAmperes, Watts) } + + override fun toString() = toShortString() } 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 db6e7db..0000000 --- a/units/src/test/kotlin/dev/nextftc/units/FunsiesTest.kt +++ /dev/null @@ -1,21 +0,0 @@ -/* - * 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.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") { - println("Elphaba Two Drivers".toByteArray().size) - } - }) From faf8b9a7ab3495dc67cf43b83c1fdb6129131c11 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 20:42:03 -0500 Subject: [PATCH 08/28] add: implement PID and SquID controller classes Signed-off-by: Zach Harel --- .../nextftc/control/feedback/PIDController.kt | 153 ++++++++++++++ .../control/feedback/SquIDController.kt | 146 ++++++++++++++ .../control/feedback/PIDControllerTest.kt | 160 +++++++++++++++ .../control/feedback/SquIDControllerTest.kt | 188 ++++++++++++++++++ 4 files changed, 647 insertions(+) create mode 100644 control/src/main/kotlin/dev/nextftc/control/feedback/PIDController.kt create mode 100644 control/src/main/kotlin/dev/nextftc/control/feedback/SquIDController.kt create mode 100644 control/src/test/kotlin/dev/nextftc/control/feedback/PIDControllerTest.kt create mode 100644 control/src/test/kotlin/dev/nextftc/control/feedback/SquIDControllerTest.kt 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/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) + } + } + }) From e899509568fd3cec2a9737d216d5ed7a5325c77e Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 20:42:06 -0500 Subject: [PATCH 09/28] add: implement MotionState data class for tracking position, velocity, and acceleration Signed-off-by: Zach Harel --- .../dev/nextftc/control/model/MotionState.kt | 51 +++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 control/src/main/kotlin/dev/nextftc/control/model/MotionState.kt 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) + } +} From 43ebbc070997b6370b2e9ca0f4bca76b9c95c9d9 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 20:58:27 -0500 Subject: [PATCH 10/28] refactor: rename SizedMatrix to Matrix and SizedVector to Vector to make it more clear that they are the default Signed-off-by: Zach Harel --- .../main/kotlin/dev/nextftc/linalg/{SizedMatrix.kt => Matrix.kt} | 0 .../main/kotlin/dev/nextftc/linalg/{SizedVector.kt => Vector.kt} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename linalg/src/main/kotlin/dev/nextftc/linalg/{SizedMatrix.kt => Matrix.kt} (100%) rename linalg/src/main/kotlin/dev/nextftc/linalg/{SizedVector.kt => Vector.kt} (100%) diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/SizedMatrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt similarity index 100% rename from linalg/src/main/kotlin/dev/nextftc/linalg/SizedMatrix.kt rename to linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt similarity index 100% rename from linalg/src/main/kotlin/dev/nextftc/linalg/SizedVector.kt rename to linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt From 43260e328dadd67859a71035f0f86aaf995e94c5 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 21:01:11 -0500 Subject: [PATCH 11/28] add: implement GravityFeedforward and SimpleFeedforward controllers with associated tests Signed-off-by: Zach Harel --- .../control/feedforward/GravityFeedforward.kt | 102 +++++++++++ .../control/feedforward/SimpleFeedforward.kt | 113 +++++++++++++ .../feedforward/GravityFeedforwardTest.kt | 159 ++++++++++++++++++ .../feedforward/SimpleFeedforwardTest.kt | 120 +++++++++++++ 4 files changed, 494 insertions(+) create mode 100644 control/src/main/kotlin/dev/nextftc/control/feedforward/GravityFeedforward.kt create mode 100644 control/src/main/kotlin/dev/nextftc/control/feedforward/SimpleFeedforward.kt create mode 100644 control/src/test/kotlin/dev/nextftc/control/feedforward/GravityFeedforwardTest.kt create mode 100644 control/src/test/kotlin/dev/nextftc/control/feedforward/SimpleFeedforwardTest.kt 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..0d1c94d --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/feedforward/GravityFeedforward.kt @@ -0,0 +1,102 @@ +/* + * 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/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..c115002 --- /dev/null +++ b/control/src/test/kotlin/dev/nextftc/control/feedforward/GravityFeedforwardTest.kt @@ -0,0 +1,159 @@ +/* + * 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..5d04c7f --- /dev/null +++ b/control/src/test/kotlin/dev/nextftc/control/feedforward/SimpleFeedforwardTest.kt @@ -0,0 +1,120 @@ +/* + * 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) + } + } + }) + From 540084b7254631b32feef21a61cdc29e83e192c3 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 21:01:15 -0500 Subject: [PATCH 12/28] add: implement TrapezoidProfile class and associated tests for motion profiling Signed-off-by: Zach Harel --- .../control/profiles/TrapezoidProfile.kt | 273 ++++++++++++ .../control/profiles/TrapezoidProfileTest.kt | 415 ++++++++++++++++++ 2 files changed, 688 insertions(+) create mode 100644 control/src/main/kotlin/dev/nextftc/control/profiles/TrapezoidProfile.kt create mode 100644 control/src/test/kotlin/dev/nextftc/control/profiles/TrapezoidProfileTest.kt 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..b52eea1 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/profiles/TrapezoidProfile.kt @@ -0,0 +1,273 @@ +/* + * 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/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..c037842 --- /dev/null +++ b/control/src/test/kotlin/dev/nextftc/control/profiles/TrapezoidProfileTest.kt @@ -0,0 +1,415 @@ +/* + * 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 { + return ToleranceMatcher(this.magnitude, tolerance) +} \ No newline at end of file From 1bc72ab0b0d609c852c6106788c49215aad66c69 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 21:01:18 -0500 Subject: [PATCH 13/28] add: implement LinearModel class and Model interface for state-space representation Signed-off-by: Zach Harel --- .../kotlin/dev/nextftc/control/model/Model.kt | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 control/src/main/kotlin/dev/nextftc/control/model/Model.kt 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..83a4587 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/model/Model.kt @@ -0,0 +1,35 @@ +/* + * 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.linalg.Nat +import dev.nextftc.linalg.SizedMatrix +import dev.nextftc.linalg.SizedVector + +interface Model { + fun derivative(state: SizedVector, input: SizedVector): SizedVector + + fun output(state: SizedVector, input: SizedVector): SizedVector +} + +@Suppress("PropertyName") +class LinearModel( + val A: SizedMatrix, + val B: SizedMatrix, + val C: SizedMatrix, + val D: SizedMatrix, +) : Model { + override fun derivative( + state: SizedVector, + input: SizedVector, + ): SizedVector = SizedVector(A * state + B * input) + + override fun output(state: SizedVector, input: SizedVector): SizedVector = + SizedVector(C * state + D * input) +} From 618926ba72177003747ea6d73046ff5482170dac Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 21:07:08 -0500 Subject: [PATCH 14/28] refactor: rename SizedVector and SizedMatrix to Vector and Matrix for clarity Signed-off-by: Zach Harel --- .../kotlin/dev/nextftc/control/model/Model.kt | 26 ++--- .../kotlin/dev/nextftc/linalg/Builders.kt | 38 +++--- .../dev/nextftc/linalg/DynamicMatrix.kt | 6 +- .../main/kotlin/dev/nextftc/linalg/Matrix.kt | 76 ++++++------ .../src/main/kotlin/dev/nextftc/linalg/Nat.kt | 2 +- .../main/kotlin/dev/nextftc/linalg/Vector.kt | 40 +++---- .../dev/nextftc/linalg/SizedMatrixTest.kt | 110 +++++++++--------- .../dev/nextftc/linalg/SizedVectorTest.kt | 20 ++-- 8 files changed, 159 insertions(+), 159 deletions(-) diff --git a/control/src/main/kotlin/dev/nextftc/control/model/Model.kt b/control/src/main/kotlin/dev/nextftc/control/model/Model.kt index 83a4587..56afc07 100644 --- a/control/src/main/kotlin/dev/nextftc/control/model/Model.kt +++ b/control/src/main/kotlin/dev/nextftc/control/model/Model.kt @@ -9,27 +9,27 @@ package dev.nextftc.control.model import dev.nextftc.linalg.Nat -import dev.nextftc.linalg.SizedMatrix -import dev.nextftc.linalg.SizedVector +import dev.nextftc.linalg.Matrix +import dev.nextftc.linalg.Vector interface Model { - fun derivative(state: SizedVector, input: SizedVector): SizedVector + fun derivative(state: Vector, input: Vector): Vector - fun output(state: SizedVector, input: SizedVector): SizedVector + fun output(state: Vector, input: Vector): Vector } @Suppress("PropertyName") class LinearModel( - val A: SizedMatrix, - val B: SizedMatrix, - val C: SizedMatrix, - val D: SizedMatrix, + val A: Matrix, + val B: Matrix, + val C: Matrix, + val D: Matrix, ) : Model { override fun derivative( - state: SizedVector, - input: SizedVector, - ): SizedVector = SizedVector(A * state + B * input) + state: Vector, + input: Vector, + ): Vector = Vector(A * state + B * input) - override fun output(state: SizedVector, input: SizedVector): SizedVector = - SizedVector(C * state + D * input) + override fun output(state: Vector, input: Vector): Vector = + Vector(C * state + D * input) } diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt index b43e380..03bb248 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt @@ -14,24 +14,24 @@ 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 +41,8 @@ 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 +53,8 @@ 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 +66,8 @@ 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 +80,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 +95,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/DynamicMatrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt index 61d063d..ef436b1 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt @@ -286,12 +286,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/Matrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt index 4067251..f8e1689 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt @@ -30,7 +30,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,52 +44,52 @@ 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) } /** @@ -100,10 +100,10 @@ open class SizedMatrix internal constructor( rows: R, cols: C, data: Array, - ): SizedMatrix { + ): 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) } } @@ -121,21 +121,21 @@ open class SizedMatrix internal constructor( /** 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 +143,37 @@ 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: Int): Matrix = times(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 +200,16 @@ 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 = +operator fun Double.times(matrix: Matrix): Matrix = matrix * this /** Scalar multiplication from the left. */ -operator fun Int.times(matrix: SizedMatrix): SizedMatrix = +operator fun Int.times(matrix: Matrix): Matrix = matrix * this diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/Nat.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Nat.kt index 348e981..3b13563 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Nat.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Nat.kt @@ -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/Vector.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt index 12f0cfa..fe59ad8 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt @@ -29,13 +29,13 @@ 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: SizedMatrix) : this(matrix.simple, matrix.rowNat) + constructor(matrix: Matrix) : this(matrix.simple, matrix.rowNat) @Suppress("ktlint") companion object { @@ -43,22 +43,22 @@ class SizedVector internal constructor(simple: SimpleMatrix, internal v * 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()) } @@ -75,28 +75,28 @@ 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: Int): 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") @@ -104,7 +104,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) @@ -120,7 +120,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/SizedMatrixTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt index 31661e3..63baa9d 100644 --- a/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt +++ b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt @@ -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,7 +444,7 @@ class SizedMatrixTest : } test("toString formats correctly") { - val m = SizedMatrix.identity(N2) + val m = Matrix.identity(N2) m.toString().contains("SizedMatrix<2, 2>") shouldBe true } } diff --git a/linalg/src/test/kotlin/dev/nextftc/linalg/SizedVectorTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedVectorTest.kt index 7612b43..5efad70 100644 --- a/linalg/src/test/kotlin/dev/nextftc/linalg/SizedVectorTest.kt +++ b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedVectorTest.kt @@ -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 } } From 0ab72cf1cabc31f3945dfbf65cc4b739401fbb1d Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 21:09:20 -0500 Subject: [PATCH 15/28] add: introduce natural number representations for rows and columns in Matrix class Signed-off-by: Zach Harel --- linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt index f8e1689..e783306 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt @@ -111,10 +111,20 @@ open class Matrix 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 From ec34141c846a7e0f7f3a99b1454e4b2063d9cb8c Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 21:35:22 -0500 Subject: [PATCH 16/28] refactor: adjust line length limit to 120 and reformat Signed-off-by: Zach Harel --- control/build.gradle.kts | 2 +- .../control/feedforward/GravityFeedforward.kt | 9 +- .../kotlin/dev/nextftc/control/model/Model.kt | 10 +- .../control/profiles/TrapezoidProfile.kt | 5 +- .../feedforward/GravityFeedforwardTest.kt | 13 +- .../feedforward/SimpleFeedforwardTest.kt | 1 - .../control/profiles/TrapezoidProfileTest.kt | 603 +++++++++--------- .../kotlin/dev/nextftc/linalg/Builders.kt | 12 +- .../main/kotlin/dev/nextftc/linalg/Matrix.kt | 15 +- .../main/kotlin/dev/nextftc/linalg/Vector.kt | 9 +- 10 files changed, 329 insertions(+), 350 deletions(-) diff --git a/control/build.gradle.kts b/control/build.gradle.kts index 86d2e9c..df235e4 100644 --- a/control/build.gradle.kts +++ b/control/build.gradle.kts @@ -34,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/feedforward/GravityFeedforward.kt b/control/src/main/kotlin/dev/nextftc/control/feedforward/GravityFeedforward.kt index 0d1c94d..d7ed6f8 100644 --- a/control/src/main/kotlin/dev/nextftc/control/feedforward/GravityFeedforward.kt +++ b/control/src/main/kotlin/dev/nextftc/control/feedforward/GravityFeedforward.kt @@ -82,11 +82,10 @@ class ArmFeedforward(val coefficients: GravityFeedforwardParameters) { * @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 + 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]. diff --git a/control/src/main/kotlin/dev/nextftc/control/model/Model.kt b/control/src/main/kotlin/dev/nextftc/control/model/Model.kt index 56afc07..3ee220f 100644 --- a/control/src/main/kotlin/dev/nextftc/control/model/Model.kt +++ b/control/src/main/kotlin/dev/nextftc/control/model/Model.kt @@ -8,8 +8,8 @@ package dev.nextftc.control.model -import dev.nextftc.linalg.Nat import dev.nextftc.linalg.Matrix +import dev.nextftc.linalg.Nat import dev.nextftc.linalg.Vector interface Model { @@ -25,11 +25,7 @@ class LinearModel( val C: Matrix, val D: Matrix, ) : Model { - override fun derivative( - state: Vector, - input: Vector, - ): Vector = Vector(A * state + B * input) + override fun derivative(state: Vector, input: Vector): Vector = Vector(A * state + B * input) - override fun output(state: Vector, input: Vector): Vector = - Vector(C * state + D * input) + override fun output(state: Vector, input: Vector): Vector = Vector(C * state + D * input) } diff --git a/control/src/main/kotlin/dev/nextftc/control/profiles/TrapezoidProfile.kt b/control/src/main/kotlin/dev/nextftc/control/profiles/TrapezoidProfile.kt index b52eea1..efb6a8d 100644 --- a/control/src/main/kotlin/dev/nextftc/control/profiles/TrapezoidProfile.kt +++ b/control/src/main/kotlin/dev/nextftc/control/profiles/TrapezoidProfile.kt @@ -27,10 +27,7 @@ import kotlin.time.DurationUnit * @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, -) { +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" } diff --git a/control/src/test/kotlin/dev/nextftc/control/feedforward/GravityFeedforwardTest.kt b/control/src/test/kotlin/dev/nextftc/control/feedforward/GravityFeedforwardTest.kt index c115002..f9b75ae 100644 --- a/control/src/test/kotlin/dev/nextftc/control/feedforward/GravityFeedforwardTest.kt +++ b/control/src/test/kotlin/dev/nextftc/control/feedforward/GravityFeedforwardTest.kt @@ -56,28 +56,32 @@ class GravityFeedforwardTest : context("ElevatorFeedforward calculate") { test("calculates correctly with positive velocity") { - val feedforward = ElevatorFeedforward(GravityFeedforwardParameters(0.5, 0.2, 2.0, 0.1)) + 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)) + 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)) + 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)) + 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 @@ -156,4 +160,3 @@ class GravityFeedforwardTest : } } }) - diff --git a/control/src/test/kotlin/dev/nextftc/control/feedforward/SimpleFeedforwardTest.kt b/control/src/test/kotlin/dev/nextftc/control/feedforward/SimpleFeedforwardTest.kt index 5d04c7f..4bebbea 100644 --- a/control/src/test/kotlin/dev/nextftc/control/feedforward/SimpleFeedforwardTest.kt +++ b/control/src/test/kotlin/dev/nextftc/control/feedforward/SimpleFeedforwardTest.kt @@ -117,4 +117,3 @@ class SimpleFeedforwardTest : } } }) - diff --git a/control/src/test/kotlin/dev/nextftc/control/profiles/TrapezoidProfileTest.kt b/control/src/test/kotlin/dev/nextftc/control/profiles/TrapezoidProfileTest.kt index c037842..f748304 100644 --- a/control/src/test/kotlin/dev/nextftc/control/profiles/TrapezoidProfileTest.kt +++ b/control/src/test/kotlin/dev/nextftc/control/profiles/TrapezoidProfileTest.kt @@ -22,394 +22,393 @@ 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 - } +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 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 maxVelocity") { + shouldThrow { + TrapezoidProfileConstraints(maxVelocity = -1.0, maxAcceleration = 2.0) + } } - } - test("should throw exception for negative maxAcceleration") { - shouldThrow { - TrapezoidProfileConstraints(maxVelocity = 5.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) + 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) + 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) - } + 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) + 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) + 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) - } + 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 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) + 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 + + 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 } - 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 + 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 } - 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) + 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) + 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) + 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) + 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) + 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) - } + 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) + 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) + 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.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) + 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) + 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) + 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) + 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) + 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) - } + 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) + 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 + profile.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime - totalTime shouldBe (0.0 plusOrMinus tolerance) - } + 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) + 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) + val result = profile.calculate(0.seconds, initial, goal) - // Should clamp to max velocity - abs(result.velocity.magnitude) shouldBeLessThanOrEqual (constraints.maxVelocity.magnitude + tolerance) + // 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) + 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.calculate(0.seconds, initial, goal) - profile.isFinished(0.0) shouldBe false - } + 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) + 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.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime - profile.isFinished(totalTime) shouldBe true - } + 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) + 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.calculate(0.seconds, initial, goal) + val totalTime = profile.totalTime - profile.isFinished(totalTime + 1.0) shouldBe true + 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) + 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) + profile.calculate(0.seconds, initial, goal) + val timeLeft = profile.timeLeftUntil(0.0) - timeLeft shouldBe (0.0 plusOrMinus tolerance) - } + 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) + 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) + profile.calculate(0.seconds, initial, goal) + val timeLeft = profile.timeLeftUntil(5.0) - timeLeft shouldBeGreaterThan 0.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) + 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) + profile.calculate(0.seconds, initial, goal) + val timeLeft = profile.timeLeftUntil(3.0) - timeLeft shouldBeLessThan 0.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) + 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 + 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) + 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) + // Position should always increase (or stay same) + currentState.position.magnitude shouldBeGreaterThanOrEqual + (previousState.position.magnitude - tolerance) - previousState = currentState + 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) + 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 + 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) + 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) + // 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 + 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) + 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.calculate(0.seconds, initial, goal) - profile.totalTime shouldBeGreaterThan 0.0 - } + 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) + 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(0.seconds, initial, goal) + val totalTime1 = profile.totalTime - profile.calculate(1.seconds, initial, goal) - val totalTime2 = profile.totalTime + profile.calculate(1.seconds, initial, goal) + val totalTime2 = profile.totalTime - totalTime1 shouldBe (totalTime2 plusOrMinus tolerance) + totalTime1 shouldBe (totalTime2 plusOrMinus tolerance) + } } - } -}) - + }) -infix fun Measure<*>.plusOrMinus(tolerance: Double) : ToleranceMatcher { - return ToleranceMatcher(this.magnitude, tolerance) -} \ No newline at end of file +infix fun Measure<*>.plusOrMinus(tolerance: Double): ToleranceMatcher = ToleranceMatcher(this.magnitude, tolerance) diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt index 03bb248..15310b8 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Builders.kt @@ -14,8 +14,7 @@ package dev.nextftc.linalg import org.ejml.simple.SimpleMatrix /** Creates a 1-dimensional vector. */ -fun makeVector(x: Double): Vector = - Vector(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): Vector = @@ -41,8 +40,7 @@ fun makeVector( x4: Double, x5: Double, x6: Double, -): Vector = - Vector(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, -): Vector = - Vector(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, -): Vector = - Vector(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( diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt index e783306..06098bc 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt @@ -96,11 +96,7 @@ open class Matrix internal constructor( * Creates a matrix from a 2D array with specified dimensions. */ @JvmStatic - fun from( - rows: R, - cols: C, - data: Array, - ): Matrix { + 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 Matrix(SimpleMatrix(data), rows, cols) @@ -153,8 +149,7 @@ open class Matrix internal constructor( get() = simple.normF() /** Negates all elements of this matrix. */ - open operator fun unaryMinus(): Matrix = - Matrix(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: Matrix): Matrix = @@ -217,9 +212,7 @@ open class Matrix internal constructor( } /** Scalar multiplication from the left. */ -operator fun Double.times(matrix: Matrix): Matrix = - matrix * this +operator fun Double.times(matrix: Matrix): Matrix = matrix * this /** Scalar multiplication from the left. */ -operator fun Int.times(matrix: Matrix): Matrix = - matrix * this +operator fun Int.times(matrix: Matrix): Matrix = matrix * this diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt index fe59ad8..7c50463 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt @@ -81,16 +81,13 @@ class Vector internal constructor(simple: SimpleMatrix, internal val di override operator fun unaryMinus(): Vector = Vector(simple.negative(), dimNat) /** Adds another vector with the same dimension. */ - operator fun plus(other: Vector): Vector = - Vector(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: Vector): Vector = - Vector(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): Vector = - Vector(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): Vector = times(scalar.toDouble()) From 96ce0bab0bbf17673917778818dddb318a46c9d9 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 21:37:44 -0500 Subject: [PATCH 17/28] add: implement LQRController control using state-space representation Signed-off-by: Zach Harel --- .../nextftc/control/feedback/LQRController.kt | 209 ++++++++++++++++++ 1 file changed, 209 insertions(+) create mode 100644 control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt 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..ad38068 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt @@ -0,0 +1,209 @@ +@file:Suppress("ktlint:standard:property-naming") + +package dev.nextftc.control.feedback + +import dev.nextftc.control.model.LinearModel +import dev.nextftc.linalg.Matrix +import dev.nextftc.linalg.N1 +import dev.nextftc.linalg.Nat +import dev.nextftc.linalg.Vector +import kotlin.math.pow + +/** + * 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) = discretizeSystem(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 +} + +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 +} + +/** + * 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.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 +} + +/** + * Computes the optimal gain matrix K using [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 +} + +/** + * 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 discretizeSystem( + A: Matrix, + B: Matrix, + dt: Double, + taylorTerms: Int = 10, +): Pair, Matrix> { + val n = A.numRows + var Ad = Matrix.identity(A.natRows) + var Bd_integral = Matrix.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) +} From 305466340c729885f2adf3163c5a186c8cb99b39 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 18 Dec 2025 21:38:50 -0500 Subject: [PATCH 18/28] add: move utility functions for state-space representation to StateSpaceUtil and clean up LQRController Signed-off-by: Zach Harel --- .../nextftc/control/feedback/LQRController.kt | 111 ++--------------- .../nextftc/control/util/StateSpaceUtil.kt | 112 ++++++++++++++++++ 2 files changed, 124 insertions(+), 99 deletions(-) create mode 100644 control/src/main/kotlin/dev/nextftc/control/util/StateSpaceUtil.kt diff --git a/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt b/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt index ad38068..2afa136 100644 --- a/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt +++ b/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt @@ -1,13 +1,23 @@ +/* + * 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.discretizeSystem +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 -import kotlin.math.pow /** * A Linear Quadratic Regulator (LQR) for controlling a system modeled by state-space equations. @@ -88,67 +98,8 @@ class LQRController @JvmOverloads con fun update(error: Matrix): Matrix = -K * error } -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 -} - -/** - * 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.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 -} - /** - * Computes the optimal gain matrix K using [solveDARE]. + * Computes the optimal gain matrix K using [dev.nextftc.control.util.solveDARE]. * * @return Pair of DARE solution X and K. */ @@ -169,41 +120,3 @@ internal fun computeLQRGain( return X to K } -/** - * 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 discretizeSystem( - A: Matrix, - B: Matrix, - dt: Double, - taylorTerms: Int = 10, -): Pair, Matrix> { - val n = A.numRows - var Ad = Matrix.identity(A.natRows) - var Bd_integral = Matrix.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) -} 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..65a0595 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/util/StateSpaceUtil.kt @@ -0,0 +1,112 @@ +/* + * 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.Nat +import dev.nextftc.linalg.Vector +import kotlin.math.pow + +internal fun makeBrysonMatrix(tolerances: Vector): Matrix { + val matrix = Matrix.Companion.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 +} + +/** + * 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 +} + +/** + * 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 discretizeSystem( + 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) +} \ No newline at end of file From 1103676a14ebf768ce289a42963dfda960d24f5a Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 19 Dec 2025 13:48:58 -0500 Subject: [PATCH 19/28] =?UTF-8?q?add:=20implement=20matrix=20exponential?= =?UTF-8?q?=20function=20using=20Pad=C3=A9=20approximant=20and=20add=20tes?= =?UTF-8?q?ts?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Zach Harel --- .../dev/nextftc/linalg/DynamicMatrix.kt | 24 +++++++++++++++ .../main/kotlin/dev/nextftc/linalg/Matrix.kt | 23 +++++++++++++++ .../dev/nextftc/linalg/DynamicMatrixTest.kt | 27 +++++++++++++++++ .../dev/nextftc/linalg/SizedMatrixTest.kt | 29 +++++++++++++++++++ 4 files changed, 103 insertions(+) diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt index ef436b1..5976eb4 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt @@ -223,6 +223,30 @@ 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. + */ + 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. diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt index 06098bc..0e70387 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt @@ -11,6 +11,7 @@ package dev.nextftc.linalg +import dev.nextftc.linalg.Matrix.Companion.identity import org.ejml.simple.SimpleMatrix /** @@ -216,3 +217,25 @@ operator fun Double.times(matrix: Matrix): Matrix /** Scalar multiplication from the left. */ 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. + */ +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) +} \ No newline at end of file diff --git a/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt index 2a34319..1176c98 100644 --- a/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt +++ b/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt @@ -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/SizedMatrixTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt index 63baa9d..f323ebd 100644 --- a/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt +++ b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt @@ -448,4 +448,33 @@ class SizedMatrixTest : 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) + } + } }) From a3bad82095ca3de467a29403f3815bcd5be29b5a Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 19 Dec 2025 13:56:31 -0500 Subject: [PATCH 20/28] add: move discretizeAB to new file and add discretizeAQ + tests Signed-off-by: Zach Harel --- .../nextftc/control/feedback/LQRController.kt | 4 +- .../nextftc/control/util/Discretization.kt | 124 ++++++++++++ .../nextftc/control/util/StateSpaceUtil.kt | 38 ---- .../control/util/DiscretizationTest.kt | 189 ++++++++++++++++++ 4 files changed, 315 insertions(+), 40 deletions(-) create mode 100644 control/src/main/kotlin/dev/nextftc/control/util/Discretization.kt create mode 100644 control/src/test/kotlin/dev/nextftc/control/util/DiscretizationTest.kt diff --git a/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt b/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt index 2afa136..94b3073 100644 --- a/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt +++ b/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt @@ -11,7 +11,7 @@ package dev.nextftc.control.feedback import dev.nextftc.control.model.LinearModel -import dev.nextftc.control.util.discretizeSystem +import dev.nextftc.control.util.discretizeAB import dev.nextftc.control.util.makeBrysonMatrix import dev.nextftc.control.util.solveDARE import dev.nextftc.linalg.Matrix @@ -52,7 +52,7 @@ class LQRController @JvmOverloads con init { require(dt > 0) { "Time step (dt) must be positive" } - val (Ad, Bd) = discretizeSystem(A, B, dt) + val (Ad, Bd) = discretizeAB(A, B, dt) val (_, K) = computeLQRGain(Ad, Bd, Q, R) this.K = K } 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..e1d1cd3 --- /dev/null +++ b/control/src/main/kotlin/dev/nextftc/control/util/Discretization.kt @@ -0,0 +1,124 @@ +/* + * 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.DynamicMatrix +import dev.nextftc.linalg.Matrix +import dev.nextftc.linalg.Nat + +/** + * 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) + ) +} diff --git a/control/src/main/kotlin/dev/nextftc/control/util/StateSpaceUtil.kt b/control/src/main/kotlin/dev/nextftc/control/util/StateSpaceUtil.kt index 65a0595..ea9918c 100644 --- a/control/src/main/kotlin/dev/nextftc/control/util/StateSpaceUtil.kt +++ b/control/src/main/kotlin/dev/nextftc/control/util/StateSpaceUtil.kt @@ -72,41 +72,3 @@ internal fun solveDARE( return H_K1 } -/** - * 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 discretizeSystem( - 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) -} \ No newline at end of file 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..3d5ffd2 --- /dev/null +++ b/control/src/test/kotlin/dev/nextftc/control/util/DiscretizationTest.kt @@ -0,0 +1,189 @@ +/* + * 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 + } + } + }) + From d770adfe78026640e8ba16b25a023e5e3ef6c22c Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 19 Dec 2025 15:44:38 -0500 Subject: [PATCH 21/28] add: enhance matrix operations to support division by scalar Signed-off-by: Zach Harel --- .../dev/nextftc/linalg/DynamicMatrix.kt | 17 ++++++++++++++--- .../main/kotlin/dev/nextftc/linalg/Matrix.kt | 19 +++++++++++++++---- .../dev/nextftc/linalg/DynamicMatrixTest.kt | 4 ++-- .../dev/nextftc/linalg/SizedMatrixTest.kt | 4 ++-- 4 files changed, 33 insertions(+), 11 deletions(-) diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt index 5976eb4..459f098 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicMatrix.kt @@ -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 @@ -232,6 +238,7 @@ open class DynamicMatrix(internal val simple: SimpleMatrix) { * * @return The matrix exponential of this matrix. */ + @Suppress("ktlint:standard:property-naming") fun exp(): DynamicMatrix { require(numRows == numColumns) { "Matrix must be square" } @@ -241,8 +248,12 @@ open class DynamicMatrix(internal val simple: SimpleMatrix) { 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) + 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/Matrix.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt index 0e70387..fbcd4a8 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Matrix.kt @@ -172,7 +172,13 @@ open class Matrix internal constructor( Matrix(simple.scale(scalar), rowNat, colNat) /** Multiplies this matrix by a scalar. */ - open operator fun times(scalar: Int): Matrix = 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, @@ -227,6 +233,7 @@ operator fun Int.times(matrix: Matrix): Matrix = * * @return The matrix exponential of this matrix. */ +@Suppress("ktlint:standard:property-naming") fun Matrix.exp(): Matrix { val I = identity(natRows) val A2 = this * this @@ -234,8 +241,12 @@ fun Matrix.exp(): Matrix { 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) + 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) -} \ No newline at end of file +} diff --git a/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt index 1176c98..802b214 100644 --- a/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt +++ b/linalg/src/test/kotlin/dev/nextftc/linalg/DynamicMatrixTest.kt @@ -435,8 +435,8 @@ class DynamicMatrixTest : val matrix = DynamicMatrix( arrayOf( doubleArrayOf(1.0, 2.0), - doubleArrayOf(3.0, 4.0) - ) + doubleArrayOf(3.0, 4.0), + ), ) val result = (matrix * 0.01).exp() diff --git a/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt index f323ebd..888285a 100644 --- a/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt +++ b/linalg/src/test/kotlin/dev/nextftc/linalg/SizedMatrixTest.kt @@ -466,8 +466,8 @@ class SizedMatrixTest : N2, arrayOf( doubleArrayOf(1.0, 2.0), - doubleArrayOf(3.0, 4.0) - ) + doubleArrayOf(3.0, 4.0), + ), ) val result = (matrix * 0.01).exp() From 8843a92b87506f0fe35203c6c5d110d721a84ae0 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 19 Dec 2025 15:45:15 -0500 Subject: [PATCH 22/28] add: implement discretizeR function and improve formatting in Discretization and StateSpaceUtil Signed-off-by: Zach Harel --- .../nextftc/control/feedback/LQRController.kt | 1 - .../nextftc/control/util/Discretization.kt | 11 ++-- .../nextftc/control/util/StateSpaceUtil.kt | 15 +++++- .../control/util/DiscretizationTest.kt | 51 ++++++++++--------- 4 files changed, 49 insertions(+), 29 deletions(-) diff --git a/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt b/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt index 94b3073..8d0154a 100644 --- a/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt +++ b/control/src/main/kotlin/dev/nextftc/control/feedback/LQRController.kt @@ -119,4 +119,3 @@ internal fun computeLQRGain( return X to K } - diff --git a/control/src/main/kotlin/dev/nextftc/control/util/Discretization.kt b/control/src/main/kotlin/dev/nextftc/control/util/Discretization.kt index e1d1cd3..9501138 100644 --- a/control/src/main/kotlin/dev/nextftc/control/util/Discretization.kt +++ b/control/src/main/kotlin/dev/nextftc/control/util/Discretization.kt @@ -6,11 +6,14 @@ * 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). @@ -63,7 +66,7 @@ internal fun discretizeAB( internal fun discretizeAQ( A: Matrix, Q: Matrix, - dt: Double + dt: Double, ): Pair, Matrix> { val states = A.numRows @@ -93,7 +96,7 @@ internal fun discretizeAQ( // 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 + M[i + states, j + states] = A[j, i] // Transpose } } @@ -119,6 +122,8 @@ internal fun discretizeAQ( @Suppress("UNCHECKED_CAST") return Pair( discA.toSizedMatrix(A.natRows, A.natColumns), - discQ.toSizedMatrix(Q.natRows, Q.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 index ea9918c..2feb9f4 100644 --- a/control/src/main/kotlin/dev/nextftc/control/util/StateSpaceUtil.kt +++ b/control/src/main/kotlin/dev/nextftc/control/util/StateSpaceUtil.kt @@ -6,6 +6,8 @@ * https://opensource.org/license/bsd-3-clause. */ +@file:Suppress("ktlint:standard:property-naming") + package dev.nextftc.control.util import dev.nextftc.linalg.Matrix @@ -14,7 +16,7 @@ import dev.nextftc.linalg.Vector import kotlin.math.pow internal fun makeBrysonMatrix(tolerances: Vector): Matrix { - val matrix = Matrix.Companion.zero(tolerances.natRows, tolerances.natRows) + val matrix = Matrix.zero(tolerances.natRows, tolerances.natRows) for (i in 0 until tolerances.numRows) { if (tolerances[i].isInfinite()) { @@ -27,6 +29,16 @@ internal fun makeBrysonMatrix(tolerances: Vector): Matrix { 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 @@ -71,4 +83,3 @@ internal fun solveDARE( return H_K1 } - diff --git a/control/src/test/kotlin/dev/nextftc/control/util/DiscretizationTest.kt b/control/src/test/kotlin/dev/nextftc/control/util/DiscretizationTest.kt index 3d5ffd2..9821497 100644 --- a/control/src/test/kotlin/dev/nextftc/control/util/DiscretizationTest.kt +++ b/control/src/test/kotlin/dev/nextftc/control/util/DiscretizationTest.kt @@ -40,7 +40,7 @@ private fun rk4Matrix( t0: Double, x0: Matrix, dt: Double, - steps: Int = 100 + steps: Int = 100, ): Matrix { var t = t0 var x = x0 @@ -66,20 +66,22 @@ class DiscretizationTest : // 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, + N2, + N2, arrayOf( doubleArrayOf(0.0, 1.0), - doubleArrayOf(0.0, 0.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, + N2, + dev.nextftc.linalg.N1, arrayOf( doubleArrayOf(0.0), - doubleArrayOf(1.0) - ) + doubleArrayOf(1.0), + ), ) val x0 = Vector.of(N2, 1.0, 1.0) @@ -95,7 +97,7 @@ class DiscretizationTest : 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] + 0.0 * x0[0] + 1.0 * x0[1] + 1.0 * u[0], ) x1Discrete shouldBe x1Truth @@ -108,18 +110,20 @@ class DiscretizationTest : context("discretizeAQ") { test("discretizes slow model correctly") { val contA = Matrix.from( - N2, N2, + N2, + N2, arrayOf( doubleArrayOf(0.0, 1.0), - doubleArrayOf(0.0, 0.0) - ) + doubleArrayOf(0.0, 0.0), + ), ) val contQ = Matrix.from( - N2, N2, + N2, + N2, arrayOf( doubleArrayOf(1.0, 0.0), - doubleArrayOf(0.0, 1.0) - ) + doubleArrayOf(0.0, 1.0), + ), ) val dt = 1.0 @@ -135,7 +139,7 @@ class DiscretizationTest : }, 0.0, Matrix.zero(N2, N2), - dt + dt, ) val (_, discQ) = discretizeAQ(contA, contQ, dt) @@ -149,18 +153,20 @@ class DiscretizationTest : // 0 test("discretizes fast model correctly") { val contA = Matrix.from( - N2, N2, + N2, + N2, arrayOf( doubleArrayOf(0.0, 1.0), - doubleArrayOf(0.0, -1406.29) - ) + doubleArrayOf(0.0, -1406.29), + ), ) val contQ = Matrix.from( - N2, N2, + N2, + N2, arrayOf( doubleArrayOf(0.0025, 0.0), - doubleArrayOf(0.0, 1.0) - ) + doubleArrayOf(0.0, 1.0), + ), ) val dt = 0.005 @@ -176,7 +182,7 @@ class DiscretizationTest : }, 0.0, Matrix.zero(N2, N2), - dt + dt, ) val (_, discQ) = discretizeAQ(contA, contQ, dt) @@ -186,4 +192,3 @@ class DiscretizationTest : } } }) - From 81a08ccf9fff0f94edf4073ede45a15e4e516e7b Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 19 Dec 2025 15:45:45 -0500 Subject: [PATCH 23/28] add: update scalar multiplication to accept Number type in DynamicVector and Vector classes Signed-off-by: Zach Harel --- linalg/src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt | 2 +- linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt index 8d22164..4be386b 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/DynamicVector.kt @@ -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/Vector.kt b/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt index 7c50463..7e0fca3 100644 --- a/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt +++ b/linalg/src/main/kotlin/dev/nextftc/linalg/Vector.kt @@ -90,7 +90,7 @@ class Vector internal constructor(simple: SimpleMatrix, internal val di override operator fun times(scalar: Double): Vector = Vector(simple.scale(scalar), dimNat) /** Multiplies this vector by a scalar. */ - override operator fun times(scalar: Int): Vector = 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: Vector): Double = simple.transpose().mult(other.simple)[0, 0] From b97f458658f483f62961f09ee05ee2f7ba34549f Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 19 Dec 2025 15:59:56 -0500 Subject: [PATCH 24/28] add: integrate discretization of matrices in LinearModel using discretizeAB function Signed-off-by: Zach Harel --- .../kotlin/dev/nextftc/control/model/Model.kt | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/control/src/main/kotlin/dev/nextftc/control/model/Model.kt b/control/src/main/kotlin/dev/nextftc/control/model/Model.kt index 3ee220f..788e73d 100644 --- a/control/src/main/kotlin/dev/nextftc/control/model/Model.kt +++ b/control/src/main/kotlin/dev/nextftc/control/model/Model.kt @@ -8,6 +8,7 @@ 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 @@ -19,13 +20,23 @@ interface Model { } @Suppress("PropertyName") -class LinearModel( +class LinearModel @JvmOverloads constructor( val A: Matrix, val B: Matrix, val C: Matrix, val D: Matrix, + val dt: Double = 0.05 ) : Model { - override fun derivative(state: Vector, input: Vector): Vector = Vector(A * state + B * input) + 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) } From dbc918a9cb6e017169fb6ddeb7f30262da283eac Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 19 Dec 2025 16:15:30 -0500 Subject: [PATCH 25/28] add: implement Kalman filter for linear state estimation Signed-off-by: Zach Harel --- .../nextftc/control/filters/KalmanFilter.kt | 141 ++++++++++++++++++ 1 file changed, 141 insertions(+) create mode 100644 control/src/main/kotlin/dev/nextftc/control/filters/KalmanFilter.kt 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..c5a3937 --- /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 + } +} From 0b516aac61b71f6f8ec6fcbe368e15795adbef92 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 19 Dec 2025 21:27:46 -0500 Subject: [PATCH 26/28] add: update version to 0.0.1-alpha.1 in gradle.properties Signed-off-by: Zach Harel --- gradle.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From f517eec761520479ecf8e3996bf79da01e7800ae Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 19 Dec 2025 21:29:24 -0500 Subject: [PATCH 27/28] run spotless oopsies Signed-off-by: Zach Harel --- .../kotlin/dev/nextftc/control/filters/KalmanFilter.kt | 8 ++++---- .../src/main/kotlin/dev/nextftc/control/model/Model.kt | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/control/src/main/kotlin/dev/nextftc/control/filters/KalmanFilter.kt b/control/src/main/kotlin/dev/nextftc/control/filters/KalmanFilter.kt index c5a3937..f639ab7 100644 --- a/control/src/main/kotlin/dev/nextftc/control/filters/KalmanFilter.kt +++ b/control/src/main/kotlin/dev/nextftc/control/filters/KalmanFilter.kt @@ -46,11 +46,11 @@ import dev.nextftc.linalg.Vector * @param measurementStdDevs Standard deviations of the measurements (measurement noise) * @param dt The discretization timestep in seconds (default 0.05s / 50ms) */ -class KalmanFilter @JvmOverloads constructor( +class KalmanFilter @JvmOverloads constructor( val plant: LinearModel, val stateStdDevs: Vector, val measurementStdDevs: Vector, - dt: Double = 0.05 + dt: Double = 0.05, ) { private val A: Matrix private val Q: Matrix @@ -133,8 +133,8 @@ class KalmanFilter @JvmOverloads constru // (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 + (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 index 788e73d..4922d5b 100644 --- a/control/src/main/kotlin/dev/nextftc/control/model/Model.kt +++ b/control/src/main/kotlin/dev/nextftc/control/model/Model.kt @@ -25,7 +25,7 @@ class LinearModel @JvmOverloads construc val B: Matrix, val C: Matrix, val D: Matrix, - val dt: Double = 0.05 + val dt: Double = 0.05, ) : Model { private val Ad: Matrix private val Bd: Matrix From 0a93373cb6141fae845d08f05113bbe5d6d89ffa Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Fri, 19 Dec 2025 21:31:06 -0500 Subject: [PATCH 28/28] add: update group ID to 'dev.nextftc.control' in build.gradle.kts Signed-off-by: Zach Harel --- build.gradle.kts | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 {