From 9d046c29cc5b818db4d2a506647a2e18e985fd92 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Wed, 20 May 2026 21:53:18 -0400 Subject: [PATCH 1/2] feat: add NextMotor class for comprehensive DC motor control with multiple modes Signed-off-by: Zach Harel --- .../nextftc/hardware/actuators/NextMotor.kt | 321 ++++++++++++++++++ 1 file changed, 321 insertions(+) create mode 100644 hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt new file mode 100644 index 0000000..c3df96b --- /dev/null +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt @@ -0,0 +1,321 @@ +/* + * Copyright (c) 2026 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.hardware.actuators + +import com.qualcomm.hardware.lynx.LynxModule +import com.qualcomm.robotcore.hardware.DcMotorImplEx +import com.qualcomm.robotcore.hardware.DcMotorSimple +import dev.nextftc.control.feedback.PIDCoefficients +import dev.nextftc.control.feedback.PIDController +import dev.nextftc.control.feedforward.SimpleFFCoefficients +import dev.nextftc.control.feedforward.SimpleFeedforward +import dev.nextftc.hardware.Caching +import dev.nextftc.hardware.LazyHardware +import dev.nextftc.hardware.RobotController +import dev.nextftc.hardware.motorController +import dev.nextftc.units.inches +import dev.nextftc.units.measuretypes.Distance +import dev.nextftc.units.measuretypes.LinearVelocity +import dev.nextftc.units.measuretypes.Voltage +import dev.nextftc.units.seconds +import kotlin.math.sign + +/** + * Comprehensive wrapper around a [DcMotorImplEx] supporting multiple control modes. + * + * [NextMotor] provides a high-level interface for DC motor control including: + * - **Throttle control**: Direct power/PWM percentage. + * - **Voltage control**: Set power as a voltage relative to the input rail voltage. + * - **Position control**: PID-based regulation to reach and hold a target encoder position. + * - **Velocity control**: PID + feedforward to reach and hold a target velocity. + * + * The motor wraps a [DcMotorImplEx] obtained lazily through an initializer, supporting + * construction from a Lynx module/port, hardware map name, or a raw initializer function. + * + * Encoder readings are automatically converted to [Distance] and [LinearVelocity] using + * the configured [distancePerCount] conversion factor. + * + * Example: + * ``` + * val motor = NextMotor("driveMotor", distancePerCount = 0.05.inches) + * motor.setVelocitySetpoint(12.0.inchesPerSecond) + * ``` + * + * @param initializer A function that returns the backing [DcMotorImplEx]. Invoked lazily. + * @param distancePerCount Conversion factor from encoder counts to distance. Defaults to 1.0 inch. + * @param cacheTolerance Power caching tolerance to reduce redundant hardware writes. Defaults to 0.01. + */ +class NextMotor( + initializer: () -> DcMotorImplEx, + var distancePerCount: Distance = 1.0.inches, + cacheTolerance: Double = 0.01 +) { + /** + * Constructs a motor from a Lynx hub module and port number. + * + * Wraps the port within the given module's motor controller. + * + * @param module The Lynx module housing this motor port. + * @param port The motor port on the module (0-based). + * @param distancePerCount Encoder count to distance conversion factor. + * @param cacheTolerance Power caching tolerance. + */ + @JvmOverloads constructor(module: LynxModule, port: Int, distancePerCount: Distance = 1.0.inches, cacheTolerance: Double = 0.01) : this( + { DcMotorImplEx(module.motorController, port) }, + distancePerCount, + cacheTolerance, + ) + + /** + * Constructs a motor by name from the current hardware map. + * + * Looks up the motor in [RobotController.hardwareMap]. + * + * @param name Hardware map device name. + * @param distancePerCount Encoder count to distance conversion factor. + * @param cacheTolerance Power caching tolerance. + */ + @JvmOverloads constructor(name: String, distancePerCount: Distance = 1.0.inches, cacheTolerance: Double = 0.01) : this({ RobotController.hardwareMap[name] as DcMotorImplEx }, distancePerCount, cacheTolerance) + + private val motor by LazyHardware(initializer) + + /** + * Position control constants (PID and feedforward gains). + * + * Modify [kP], [kI], [kD], [kS], [kV], [kA] properties and gravity/cos terms + * to tune position setpoint tracking. + */ + val positionConstants: MotorPositionConstants = MotorPositionConstants() + + /** + * PID controller used for position setpoint tracking. + */ + val positionPID = PIDController(positionConstants.pidConstants) + + /** + * Velocity control constants (PID and feedforward gains). + * + * Modify [kP], [kI], [kD], [kS], [kV], [kA] properties to tune + * velocity setpoint tracking. + */ + val velocityConstants: MotorVelocityConstants = MotorVelocityConstants() + + /** + * PID controller used for velocity setpoint tracking. + */ + val velocityPID = PIDController(velocityConstants.pidConstants) + + /** + * Feedforward calculator used in conjunction with velocity PID. + */ + val velocityFF = SimpleFeedforward(velocityConstants.ffCoefficients) + + /** + * Current active control mode (THROTTLE, VOLTAGE, POSITION, or VELOCITY). + */ + var controlType: ControlType = ControlType.THROTTLE + private set + + /** + * Raw motor power (throttle) in the range [-1.0, 1.0]. + * + * This backing field is managed by the caching delegate to reduce + * redundant hardware writes. + */ + private var power by Caching(cacheTolerance) { + if (it != null) { + motor.power = it + } + } + + /** + * Motor rotation direction (FORWARD or REVERSE). + * + * When set, automatically updates the underlying motor's direction. + */ + var direction = NextMotor.Direction.FORWARD + set(value) { + field = value + motor.direction = value.sdkDirection + } + + /** + * Current encoder position in physical distance units. + * + * Computed from the raw encoder count scaled by [distancePerCount]. + */ + val encoderPosition: Distance + get() = (distancePerCount * motor.currentPosition) as Distance + + /** + * Current encoder velocity in physical distance per time units. + * + * Computed from the raw motor velocity scaled by [distancePerCount] + * and divided by 1 second to convert to the expected time unit. + */ + val encoderVelocity: LinearVelocity + get() = distancePerCount * motor.velocity / 1.0.seconds + + /** + * Set throttle control mode and power. + * + * This is the simplest control mode: power is applied directly. + * + * @param throttle Power in the range [-1.0, 1.0]. Positive is forward. + */ + fun setThrottle(throttle: Double) { + controlType = ControlType.THROTTLE + power = throttle + } + + /** + * Set voltage control mode and voltage setpoint. + * + * Adjusts power to maintain the specified voltage, accounting for the + * current input rail voltage (to remain relatively hardware-independent). + * + * @param voltage Target voltage setpoint. + */ + fun setVoltage(voltage: Voltage) { + controlType = ControlType.VOLTAGE + power = (voltage / RobotController.inputVoltage).magnitude + } + + /** + * Set position control mode and position setpoint. + * + * Uses [positionPID] to calculate closed-loop power and adds a static + * friction term ([positionConstants.kS]) scaled by the sign of error. + * + * @param setpoint Target encoder position. + */ + fun setPositionSetpoint(setpoint: Distance) { + controlType = ControlType.POSITION + power = positionPID.calculate(reference = setpoint.magnitude, measured = encoderPosition.into(setpoint.unit)) + + positionConstants.kS * (setpoint.magnitude - encoderPosition.magnitude).sign + } + + /** + * Set velocity control mode and velocity setpoint. + * + * Uses [velocityPID] and [velocityFF] to calculate closed-loop power. + * The feedforward component helps overcome friction and inertia. + * + * @param setpoint Target velocity. + */ + fun setVelocitySetpoint(setpoint: LinearVelocity) { + controlType = ControlType.VELOCITY + power = velocityPID.calculate(reference = setpoint.magnitude, measured = encoderVelocity.into(setpoint.unit)) + + velocityFF.calculate(setpoint.magnitude) + } + + + /** + * Motor control mode. + * + * Determines how the motor interprets and uses the power value. + */ + enum class ControlType { + /** + * Direct power/duty cycle control. Power is applied as-is. + */ + THROTTLE, + + /** + * Voltage-based control. Power is adjusted to account for rail voltage. + */ + VOLTAGE, + + /** + * Closed-loop position tracking using PID and feedforward. + */ + POSITION, + + /** + * Closed-loop velocity tracking using PID and feedforward. + */ + VELOCITY + } + + /** + * Motor rotation direction. + */ + enum class Direction(val sdkDirection: DcMotorSimple.Direction) { + /** + * Motor spins forward (in its default/positive direction). + */ + FORWARD(DcMotorSimple.Direction.FORWARD), + + /** + * Motor spins in reverse (negated). + */ + REVERSE(DcMotorSimple.Direction.REVERSE), + } +} + +/** + * Tuning constants for position control. + * + * Exposes PID gains (kP, kI, kD), feedforward gains (kS, kV, kA), and additional + * compensation terms (kG for gravity, kCos/kCosRatio for cosine compensation). + * + * @param pidConstants The underlying PID coefficient holder. + * @param ffCoefficients The underlying feedforward coefficient holder. + * @param kG Gravity compensation constant. Defaults to 0.0. + * @param kCos Cosine amplitude compensation. Defaults to 0.0. + * @param kCosRatio Ratio for cosine compensation (e.g., linkage ratio). Defaults to 0.0. + */ +data class MotorPositionConstants( + internal val pidConstants: PIDCoefficients = PIDCoefficients(0.0), + internal val ffCoefficients: SimpleFFCoefficients = SimpleFFCoefficients(0.0, 0.0, 0.0), + @JvmField var kG: Double = 0.0, + @JvmField var kCos: Double = 0.0, + @JvmField var kCosRatio : Double = 0.0, +) { + /** Proportional gain for position PID. */ + var kP by pidConstants::kP + /** Integral gain for position PID. */ + var kI by pidConstants::kI + /** Derivative gain for position PID. */ + var kD by pidConstants::kD + + /** Static friction compensation for position control. */ + var kS by ffCoefficients::kS + /** Velocity feedforward gain for position control. */ + var kV by ffCoefficients::kV + /** Acceleration feedforward gain for position control. */ + var kA by ffCoefficients::kA +} + +/** + * Tuning constants for velocity control. + * + * Exposes PID gains (kP, kI, kD) and feedforward gains (kS, kV, kA). + * + * @param pidConstants The underlying PID coefficient holder. + * @param ffCoefficients The underlying feedforward coefficient holder. + */ +data class MotorVelocityConstants( + internal val pidConstants: PIDCoefficients = PIDCoefficients(0.0, 0.0, 0.0), + internal val ffCoefficients: SimpleFFCoefficients = SimpleFFCoefficients(0.0, 0.0, 0.0), +) { + /** Proportional gain for velocity PID. */ + var kP by pidConstants::kP + /** Integral gain for velocity PID. */ + var kI by pidConstants::kI + /** Derivative gain for velocity PID. */ + var kD by pidConstants::kD + + /** Static friction compensation for velocity control. */ + var kS by ffCoefficients::kS + /** Velocity feedforward gain for velocity control. */ + var kV by ffCoefficients::kV + /** Acceleration feedforward gain for velocity control. */ + var kA by ffCoefficients::kA +} \ No newline at end of file From 78f4e7e59981985cf2568a3d66bb9ed0d1bb5ebd Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Thu, 21 May 2026 10:12:55 -0400 Subject: [PATCH 2/2] run spotless Signed-off-by: Zach Harel --- .../nextftc/hardware/actuators/NextMotor.kt | 438 +++++++++--------- 1 file changed, 231 insertions(+), 207 deletions(-) diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt index c3df96b..387a3d7 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt @@ -52,211 +52,227 @@ import kotlin.math.sign * @param cacheTolerance Power caching tolerance to reduce redundant hardware writes. Defaults to 0.01. */ class NextMotor( - initializer: () -> DcMotorImplEx, - var distancePerCount: Distance = 1.0.inches, - cacheTolerance: Double = 0.01 + initializer: () -> DcMotorImplEx, + var distancePerCount: Distance = 1.0.inches, + cacheTolerance: Double = 0.01, ) { - /** - * Constructs a motor from a Lynx hub module and port number. - * - * Wraps the port within the given module's motor controller. - * - * @param module The Lynx module housing this motor port. - * @param port The motor port on the module (0-based). - * @param distancePerCount Encoder count to distance conversion factor. - * @param cacheTolerance Power caching tolerance. - */ - @JvmOverloads constructor(module: LynxModule, port: Int, distancePerCount: Distance = 1.0.inches, cacheTolerance: Double = 0.01) : this( - { DcMotorImplEx(module.motorController, port) }, - distancePerCount, - cacheTolerance, - ) + /** + * Constructs a motor from a Lynx hub module and port number. + * + * Wraps the port within the given module's motor controller. + * + * @param module The Lynx module housing this motor port. + * @param port The motor port on the module (0-based). + * @param distancePerCount Encoder count to distance conversion factor. + * @param cacheTolerance Power caching tolerance. + */ + @JvmOverloads constructor( + module: LynxModule, + port: Int, + distancePerCount: Distance = 1.0.inches, + cacheTolerance: Double = 0.01, + ) : this( + { DcMotorImplEx(module.motorController, port) }, + distancePerCount, + cacheTolerance, + ) - /** - * Constructs a motor by name from the current hardware map. - * - * Looks up the motor in [RobotController.hardwareMap]. - * - * @param name Hardware map device name. - * @param distancePerCount Encoder count to distance conversion factor. - * @param cacheTolerance Power caching tolerance. - */ - @JvmOverloads constructor(name: String, distancePerCount: Distance = 1.0.inches, cacheTolerance: Double = 0.01) : this({ RobotController.hardwareMap[name] as DcMotorImplEx }, distancePerCount, cacheTolerance) + /** + * Constructs a motor by name from the current hardware map. + * + * Looks up the motor in [RobotController.hardwareMap]. + * + * @param name Hardware map device name. + * @param distancePerCount Encoder count to distance conversion factor. + * @param cacheTolerance Power caching tolerance. + */ + @JvmOverloads constructor( + name: String, + distancePerCount: Distance = 1.0.inches, + cacheTolerance: Double = 0.01, + ) : this({ RobotController.hardwareMap[name] as DcMotorImplEx }, distancePerCount, cacheTolerance) - private val motor by LazyHardware(initializer) + private val motor by LazyHardware(initializer) - /** - * Position control constants (PID and feedforward gains). - * - * Modify [kP], [kI], [kD], [kS], [kV], [kA] properties and gravity/cos terms - * to tune position setpoint tracking. - */ - val positionConstants: MotorPositionConstants = MotorPositionConstants() + /** + * Position control constants (PID and feedforward gains). + * + * Modify [kP], [kI], [kD], [kS], [kV], [kA] properties and gravity/cos terms + * to tune position setpoint tracking. + */ + val positionConstants: MotorPositionConstants = MotorPositionConstants() - /** - * PID controller used for position setpoint tracking. - */ - val positionPID = PIDController(positionConstants.pidConstants) + /** + * PID controller used for position setpoint tracking. + */ + val positionPID = PIDController(positionConstants.pidConstants) - /** - * Velocity control constants (PID and feedforward gains). - * - * Modify [kP], [kI], [kD], [kS], [kV], [kA] properties to tune - * velocity setpoint tracking. - */ - val velocityConstants: MotorVelocityConstants = MotorVelocityConstants() + /** + * Velocity control constants (PID and feedforward gains). + * + * Modify [kP], [kI], [kD], [kS], [kV], [kA] properties to tune + * velocity setpoint tracking. + */ + val velocityConstants: MotorVelocityConstants = MotorVelocityConstants() - /** - * PID controller used for velocity setpoint tracking. - */ - val velocityPID = PIDController(velocityConstants.pidConstants) + /** + * PID controller used for velocity setpoint tracking. + */ + val velocityPID = PIDController(velocityConstants.pidConstants) - /** - * Feedforward calculator used in conjunction with velocity PID. - */ - val velocityFF = SimpleFeedforward(velocityConstants.ffCoefficients) + /** + * Feedforward calculator used in conjunction with velocity PID. + */ + val velocityFF = SimpleFeedforward(velocityConstants.ffCoefficients) - /** - * Current active control mode (THROTTLE, VOLTAGE, POSITION, or VELOCITY). - */ - var controlType: ControlType = ControlType.THROTTLE - private set + /** + * Current active control mode (THROTTLE, VOLTAGE, POSITION, or VELOCITY). + */ + var controlType: ControlType = ControlType.THROTTLE + private set - /** - * Raw motor power (throttle) in the range [-1.0, 1.0]. - * - * This backing field is managed by the caching delegate to reduce - * redundant hardware writes. - */ - private var power by Caching(cacheTolerance) { - if (it != null) { - motor.power = it - } + /** + * Raw motor power (throttle) in the range [-1.0, 1.0]. + * + * This backing field is managed by the caching delegate to reduce + * redundant hardware writes. + */ + private var power by Caching(cacheTolerance) { + if (it != null) { + motor.power = it } + } - /** - * Motor rotation direction (FORWARD or REVERSE). - * - * When set, automatically updates the underlying motor's direction. - */ - var direction = NextMotor.Direction.FORWARD - set(value) { - field = value - motor.direction = value.sdkDirection - } + /** + * Motor rotation direction (FORWARD or REVERSE). + * + * When set, automatically updates the underlying motor's direction. + */ + var direction = NextMotor.Direction.FORWARD + set(value) { + field = value + motor.direction = value.sdkDirection + } - /** - * Current encoder position in physical distance units. - * - * Computed from the raw encoder count scaled by [distancePerCount]. - */ - val encoderPosition: Distance - get() = (distancePerCount * motor.currentPosition) as Distance + /** + * Current encoder position in physical distance units. + * + * Computed from the raw encoder count scaled by [distancePerCount]. + */ + val encoderPosition: Distance + get() = (distancePerCount * motor.currentPosition) as Distance - /** - * Current encoder velocity in physical distance per time units. - * - * Computed from the raw motor velocity scaled by [distancePerCount] - * and divided by 1 second to convert to the expected time unit. - */ - val encoderVelocity: LinearVelocity - get() = distancePerCount * motor.velocity / 1.0.seconds + /** + * Current encoder velocity in physical distance per time units. + * + * Computed from the raw motor velocity scaled by [distancePerCount] + * and divided by 1 second to convert to the expected time unit. + */ + val encoderVelocity: LinearVelocity + get() = distancePerCount * motor.velocity / 1.0.seconds + + /** + * Set throttle control mode and power. + * + * This is the simplest control mode: power is applied directly. + * + * @param throttle Power in the range [-1.0, 1.0]. Positive is forward. + */ + fun setThrottle(throttle: Double) { + controlType = ControlType.THROTTLE + power = throttle + } + + /** + * Set voltage control mode and voltage setpoint. + * + * Adjusts power to maintain the specified voltage, accounting for the + * current input rail voltage (to remain relatively hardware-independent). + * + * @param voltage Target voltage setpoint. + */ + fun setVoltage(voltage: Voltage) { + controlType = ControlType.VOLTAGE + power = (voltage / RobotController.inputVoltage).magnitude + } + /** + * Set position control mode and position setpoint. + * + * Uses [positionPID] to calculate closed-loop power and adds a static + * friction term ([positionConstants.kS]) scaled by the sign of error. + * + * @param setpoint Target encoder position. + */ + fun setPositionSetpoint(setpoint: Distance) { + controlType = ControlType.POSITION + power = + positionPID.calculate( + reference = setpoint.magnitude, + measured = encoderPosition.into(setpoint.unit), + ) + + positionConstants.kS * (setpoint.magnitude - encoderPosition.magnitude).sign + } + + /** + * Set velocity control mode and velocity setpoint. + * + * Uses [velocityPID] and [velocityFF] to calculate closed-loop power. + * The feedforward component helps overcome friction and inertia. + * + * @param setpoint Target velocity. + */ + fun setVelocitySetpoint(setpoint: LinearVelocity) { + controlType = ControlType.VELOCITY + power = + velocityPID.calculate( + reference = setpoint.magnitude, + measured = encoderVelocity.into(setpoint.unit), + ) + + velocityFF.calculate(setpoint.magnitude) + } + + /** + * Motor control mode. + * + * Determines how the motor interprets and uses the power value. + */ + enum class ControlType { /** - * Set throttle control mode and power. - * - * This is the simplest control mode: power is applied directly. - * - * @param throttle Power in the range [-1.0, 1.0]. Positive is forward. + * Direct power/duty cycle control. Power is applied as-is. */ - fun setThrottle(throttle: Double) { - controlType = ControlType.THROTTLE - power = throttle - } + THROTTLE, /** - * Set voltage control mode and voltage setpoint. - * - * Adjusts power to maintain the specified voltage, accounting for the - * current input rail voltage (to remain relatively hardware-independent). - * - * @param voltage Target voltage setpoint. + * Voltage-based control. Power is adjusted to account for rail voltage. */ - fun setVoltage(voltage: Voltage) { - controlType = ControlType.VOLTAGE - power = (voltage / RobotController.inputVoltage).magnitude - } + VOLTAGE, /** - * Set position control mode and position setpoint. - * - * Uses [positionPID] to calculate closed-loop power and adds a static - * friction term ([positionConstants.kS]) scaled by the sign of error. - * - * @param setpoint Target encoder position. + * Closed-loop position tracking using PID and feedforward. */ - fun setPositionSetpoint(setpoint: Distance) { - controlType = ControlType.POSITION - power = positionPID.calculate(reference = setpoint.magnitude, measured = encoderPosition.into(setpoint.unit)) + - positionConstants.kS * (setpoint.magnitude - encoderPosition.magnitude).sign - } + POSITION, /** - * Set velocity control mode and velocity setpoint. - * - * Uses [velocityPID] and [velocityFF] to calculate closed-loop power. - * The feedforward component helps overcome friction and inertia. - * - * @param setpoint Target velocity. + * Closed-loop velocity tracking using PID and feedforward. */ - fun setVelocitySetpoint(setpoint: LinearVelocity) { - controlType = ControlType.VELOCITY - power = velocityPID.calculate(reference = setpoint.magnitude, measured = encoderVelocity.into(setpoint.unit)) + - velocityFF.calculate(setpoint.magnitude) - } - + VELOCITY, + } + /** + * Motor rotation direction. + */ + enum class Direction(val sdkDirection: DcMotorSimple.Direction) { /** - * Motor control mode. - * - * Determines how the motor interprets and uses the power value. + * Motor spins forward (in its default/positive direction). */ - enum class ControlType { - /** - * Direct power/duty cycle control. Power is applied as-is. - */ - THROTTLE, - - /** - * Voltage-based control. Power is adjusted to account for rail voltage. - */ - VOLTAGE, - - /** - * Closed-loop position tracking using PID and feedforward. - */ - POSITION, - - /** - * Closed-loop velocity tracking using PID and feedforward. - */ - VELOCITY - } + FORWARD(DcMotorSimple.Direction.FORWARD), /** - * Motor rotation direction. + * Motor spins in reverse (negated). */ - enum class Direction(val sdkDirection: DcMotorSimple.Direction) { - /** - * Motor spins forward (in its default/positive direction). - */ - FORWARD(DcMotorSimple.Direction.FORWARD), - - /** - * Motor spins in reverse (negated). - */ - REVERSE(DcMotorSimple.Direction.REVERSE), - } + REVERSE(DcMotorSimple.Direction.REVERSE), + } } /** @@ -272,25 +288,29 @@ class NextMotor( * @param kCosRatio Ratio for cosine compensation (e.g., linkage ratio). Defaults to 0.0. */ data class MotorPositionConstants( - internal val pidConstants: PIDCoefficients = PIDCoefficients(0.0), - internal val ffCoefficients: SimpleFFCoefficients = SimpleFFCoefficients(0.0, 0.0, 0.0), - @JvmField var kG: Double = 0.0, - @JvmField var kCos: Double = 0.0, - @JvmField var kCosRatio : Double = 0.0, + internal val pidConstants: PIDCoefficients = PIDCoefficients(0.0), + internal val ffCoefficients: SimpleFFCoefficients = SimpleFFCoefficients(0.0, 0.0, 0.0), + @JvmField var kG: Double = 0.0, + @JvmField var kCos: Double = 0.0, + @JvmField var kCosRatio: Double = 0.0, ) { - /** Proportional gain for position PID. */ - var kP by pidConstants::kP - /** Integral gain for position PID. */ - var kI by pidConstants::kI - /** Derivative gain for position PID. */ - var kD by pidConstants::kD - - /** Static friction compensation for position control. */ - var kS by ffCoefficients::kS - /** Velocity feedforward gain for position control. */ - var kV by ffCoefficients::kV - /** Acceleration feedforward gain for position control. */ - var kA by ffCoefficients::kA + /** Proportional gain for position PID. */ + var kP by pidConstants::kP + + /** Integral gain for position PID. */ + var kI by pidConstants::kI + + /** Derivative gain for position PID. */ + var kD by pidConstants::kD + + /** Static friction compensation for position control. */ + var kS by ffCoefficients::kS + + /** Velocity feedforward gain for position control. */ + var kV by ffCoefficients::kV + + /** Acceleration feedforward gain for position control. */ + var kA by ffCoefficients::kA } /** @@ -302,20 +322,24 @@ data class MotorPositionConstants( * @param ffCoefficients The underlying feedforward coefficient holder. */ data class MotorVelocityConstants( - internal val pidConstants: PIDCoefficients = PIDCoefficients(0.0, 0.0, 0.0), - internal val ffCoefficients: SimpleFFCoefficients = SimpleFFCoefficients(0.0, 0.0, 0.0), + internal val pidConstants: PIDCoefficients = PIDCoefficients(0.0, 0.0, 0.0), + internal val ffCoefficients: SimpleFFCoefficients = SimpleFFCoefficients(0.0, 0.0, 0.0), ) { - /** Proportional gain for velocity PID. */ - var kP by pidConstants::kP - /** Integral gain for velocity PID. */ - var kI by pidConstants::kI - /** Derivative gain for velocity PID. */ - var kD by pidConstants::kD - - /** Static friction compensation for velocity control. */ - var kS by ffCoefficients::kS - /** Velocity feedforward gain for velocity control. */ - var kV by ffCoefficients::kV - /** Acceleration feedforward gain for velocity control. */ - var kA by ffCoefficients::kA -} \ No newline at end of file + /** Proportional gain for velocity PID. */ + var kP by pidConstants::kP + + /** Integral gain for velocity PID. */ + var kI by pidConstants::kI + + /** Derivative gain for velocity PID. */ + var kD by pidConstants::kD + + /** Static friction compensation for velocity control. */ + var kS by ffCoefficients::kS + + /** Velocity feedforward gain for velocity control. */ + var kV by ffCoefficients::kV + + /** Acceleration feedforward gain for velocity control. */ + var kA by ffCoefficients::kA +}