From 9c74710e5bec516abcd8b74a49ea5d350b08979f Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Wed, 5 Nov 2025 17:27:31 -0800 Subject: [PATCH 01/12] Update ClosedLoopConstantsClasses.kt --- controls/ClosedLoopConstantsClasses.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controls/ClosedLoopConstantsClasses.kt b/controls/ClosedLoopConstantsClasses.kt index 68fb8b0..d534c4f 100644 --- a/controls/ClosedLoopConstantsClasses.kt +++ b/controls/ClosedLoopConstantsClasses.kt @@ -33,4 +33,5 @@ fun SimpleMotorFeedForwardConstants.toFeedForward() : SimpleMotorFeedforward { */ fun PIDConstants.toPID() : Controller.PID { return Controller.PID(this.P, this.I, this.D) -} \ No newline at end of file +} +val PIDConstants.PathPlannerPID : com.pathplanner.lib.config.PIDConstants get() = com.pathplanner.lib.config.PIDConstants(this.P, this.I, this.D) \ No newline at end of file From 7b5f1422e3d87a3762a4bbcc643cd7b3699222d3 Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Wed, 5 Nov 2025 17:39:00 -0800 Subject: [PATCH 02/12] Deprecated Useless functions --- async/EventTarget.kt | 1 + async/Promise.kt | 1 + async/Thenable.kt | 1 + controls/Controller.kt | 1 + controls/TurningPID.kt | 1 + 5 files changed, 5 insertions(+) diff --git a/async/EventTarget.kt b/async/EventTarget.kt index d6db93e..f65c96b 100644 --- a/async/EventTarget.kt +++ b/async/EventTarget.kt @@ -3,6 +3,7 @@ package frc.beaverlib.async import frc.beaverlib.misc.Symbol +@Deprecated("EventTarget is deprecated, if you need to use it, idk why you would need to") class EventTarget { private val listeners: MutableMap Unit> = mutableMapOf() private val onceListeners: MutableMap Unit> = mutableMapOf() diff --git a/async/Promise.kt b/async/Promise.kt index 2e73a47..0c97b16 100644 --- a/async/Promise.kt +++ b/async/Promise.kt @@ -5,6 +5,7 @@ import frc.beaverlib.misc.AggregateException import edu.wpi.first.wpilibj.event.BooleanEvent import edu.wpi.first.wpilibj.event.EventLoop +@Deprecated("Promise is deprecated, if you need to use it, idk why you would need to") class Promise(private val fn: (resolve: (value: T) -> Unit, reject: (error: Throwable) -> Unit) -> Unit) : Thenable { private var resolvedWith: T? = null private var rejectedWith: Throwable? = null diff --git a/async/Thenable.kt b/async/Thenable.kt index 82a8787..ffaed72 100644 --- a/async/Thenable.kt +++ b/async/Thenable.kt @@ -1,5 +1,6 @@ package frc.beaverlib.async +@Deprecated("Thenable is deprecated, if you need to use it, idk why you would need to") interface Thenable { fun then(success: (value: T) -> Promise): Promise fun then(success: (value: T) -> Promise, failure: (error: Throwable) -> Promise) : Promise diff --git a/controls/Controller.kt b/controls/Controller.kt index 5de4ca6..23da47a 100644 --- a/controls/Controller.kt +++ b/controls/Controller.kt @@ -8,6 +8,7 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward * A closed-loop controller controlling a one-dimensional output, * usually a motor. */ +@Deprecated("Controller is deprecated, as it is it is just a wrapper for PID controller that provides no functionality") interface Controller { var setpoint: Double diff --git a/controls/TurningPID.kt b/controls/TurningPID.kt index 4e30125..9b581ba 100644 --- a/controls/TurningPID.kt +++ b/controls/TurningPID.kt @@ -12,6 +12,7 @@ import kotlin.math.sign * @author Anthony, Mike */ @Suppress("MemberVisibilityCanBePrivate") +@Deprecated("TurningPID is deprecated, use the default WPI PID instead") class TurningPID(var kP: Double, var kD: Double) { var setPoint = 0.0 var timePrevious = Timer.getFPGATimestamp() From 7da9ad29383fc5006bc738103c57070da07c57a6 Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Wed, 5 Nov 2025 18:03:39 -0800 Subject: [PATCH 03/12] Vision --- misc/Signals.kt | 34 ++++++++++++++++ odometry/BeaverPhotonVision.kt | 49 +++++++++++++++++++++++ odometry/BeaverVisionCamera.kt | 36 +++++++++++++++++ odometry/Vision.kt | 73 ---------------------------------- 4 files changed, 119 insertions(+), 73 deletions(-) create mode 100644 misc/Signals.kt create mode 100644 odometry/BeaverPhotonVision.kt create mode 100644 odometry/BeaverVisionCamera.kt delete mode 100644 odometry/Vision.kt diff --git a/misc/Signals.kt b/misc/Signals.kt new file mode 100644 index 0000000..6ea7866 --- /dev/null +++ b/misc/Signals.kt @@ -0,0 +1,34 @@ +package frc.robot.beaverlib.misc + +/** + * A MutableMap used specifically for managing Lambdas + * Specified Type is used as the input for argument for listener Lambdas + */ +data class BiSignal( + val listeners : MutableMap Unit> = mutableMapOf() +) { + /** + * Adds a listener with the given name. The function will be run whenever the parent object calls the update function + * @param name String to denote the name of the listener, used to remove specific listeners later on + * @param function The function to run when the listener updates */ + fun add(name: String, function: (Type, Type2) -> Unit){ + listeners.put(name, function) + } + + /** + * Removes a listener with the given name + * @param name The name of the listener to remove */ + fun remove(name: String){ + listeners.remove(name) + } + + /** + * Runs all active listeners with the given input + * @param input The input for each of the listening functions + * */ + fun update(input: Type, input2: Type2) { + listeners.forEach { (_, listenerFunction) -> + listenerFunction(input, input2) // Pass source to the listener + } + } +} \ No newline at end of file diff --git a/odometry/BeaverPhotonVision.kt b/odometry/BeaverPhotonVision.kt new file mode 100644 index 0000000..311746d --- /dev/null +++ b/odometry/BeaverPhotonVision.kt @@ -0,0 +1,49 @@ +package beaverlib.odometry +// Filed adapted from 2898s 2023 Charged Up code +import edu.wpi.first.math.Matrix +import edu.wpi.first.math.Nat +import edu.wpi.first.math.geometry.* +import edu.wpi.first.math.numbers.N1 +import edu.wpi.first.math.numbers.N3 +import edu.wpi.first.wpilibj2.command.SubsystemBase +import frc.robot.beaverlib.misc.BiSignal +import frc.robot.beaverlib.odometry.BeaverVisionCamera +import org.photonvision.targeting.PhotonPipelineResult +import java.util.* + +/** + * A Vision SubsystemBase, that polls [cameras] each frame, and updates [listeners] with the apriltag results + * @param cameras A list of all of the cameras connected to pi, to pull for apriltag results + */ +class BeaverPhotonVision(vararg val cameras: BeaverVisionCamera) : SubsystemBase() { + /** Runs each of the Lambdas with the apriltag results, and the camera they came from, for every apriltag result. */ + val listeners = BiSignal() + + override fun periodic() { + for(camera in cameras){ + for (result in camera.results) { + listeners.update(result, camera) + } + } + } + + /** Updates all of the cameras pose estimators with the given reference pose */ + fun setAllCameraReferences(pose: Pose2d) { + for(camera in cameras) { + camera.setReference(pose) + } + } + + /** Returns the standard deviation matrix required by YAGSL given each of the standard deviation inputs + * @param STDVX Standard deviation in the X estimation + * @param STDVY Standard deviation in the Y estimation + * @param rotationSTD Standard deviation in the rotation estimation + * */ + fun getStandardDev(STDVX : Double, STDVY : Double, rotationSTD: Double): Matrix{ + val stdv = Matrix(Nat.N3(), Nat.N1()) + stdv.set(0,0, STDVX) + stdv.set(1,0, STDVY) + stdv.set(2,0, rotationSTD) + return stdv + } +} \ No newline at end of file diff --git a/odometry/BeaverVisionCamera.kt b/odometry/BeaverVisionCamera.kt new file mode 100644 index 0000000..1474781 --- /dev/null +++ b/odometry/BeaverVisionCamera.kt @@ -0,0 +1,36 @@ +package frc.robot.beaverlib.odometry + +import edu.wpi.first.apriltag.AprilTagFieldLayout +import edu.wpi.first.math.geometry.Pose2d +import edu.wpi.first.math.geometry.Pose3d +import edu.wpi.first.math.geometry.Transform3d +import org.photonvision.PhotonCamera +import org.photonvision.PhotonPoseEstimator +import org.photonvision.targeting.PhotonPipelineResult + +/** + * A class that represents a Photonvision camera + * @param name The cameras name, as it appears in PhotonVision + * @param robotToCamera The translation vector from the camera to the center of the robot + * @param strategy The primary pose estimation strategy this camera should use + * @param fallbackStrategy The secondary pose estimation this camera should use if [strategy] is not possible. (Will not be set if null) + */ +class BeaverVisionCamera(val name: String, val robotToCamera: Transform3d, layout: AprilTagFieldLayout, strategy: PhotonPoseEstimator.PoseStrategy, fallbackStrategy : PhotonPoseEstimator.PoseStrategy? = null ) { + val cam = PhotonCamera(name) + /** Gets all unread results (SHOULD ONLY BE USED BY BEAVERPHOTONVISION) */ + val results: List get() = cam.allUnreadResults + /** The pose estimator for this camera */ + val poseEstimator = PhotonPoseEstimator(layout, strategy, robotToCamera) + + init { + if(fallbackStrategy != null) poseEstimator.setMultiTagFallbackStrategy(fallbackStrategy) + } + + /** Returns an estimated Pose3d given a PhotonPipelineResult (That should have been generated by this camera) */ + fun getEstimatedRobotPose(result: PhotonPipelineResult) : Pose3d? { + val estimatedPose = poseEstimator.update(result) ?: return null + if (estimatedPose.isEmpty) return null + return estimatedPose.get().estimatedPose + } + fun setReference(pose: Pose2d) { poseEstimator.setReferencePose(pose) } +} \ No newline at end of file diff --git a/odometry/Vision.kt b/odometry/Vision.kt deleted file mode 100644 index 43375c2..0000000 --- a/odometry/Vision.kt +++ /dev/null @@ -1,73 +0,0 @@ -package beaverlib.odometry -// Filed adapted from 2898s 2023 Charged Up code -import edu.wpi.first.apriltag.AprilTag -import edu.wpi.first.apriltag.AprilTagFieldLayout -import edu.wpi.first.apriltag.AprilTagFields -import edu.wpi.first.math.Matrix -import edu.wpi.first.math.Nat -import edu.wpi.first.math.geometry.* -import edu.wpi.first.math.numbers.N1 -import edu.wpi.first.math.numbers.N3 -import edu.wpi.first.networktables.NetworkTableEvent -import edu.wpi.first.networktables.NetworkTableInstance -import edu.wpi.first.wpilibj.Timer -import edu.wpi.first.wpilibj2.command.SubsystemBase -import beaverlib.utils.Sugar.degreesToRadians -import org.photonvision.EstimatedRobotPose -import org.photonvision.PhotonCamera -import org.photonvision.PhotonPoseEstimator -import java.util.* - -val aprilTags1: MutableList = mutableListOf( - AprilTag(2, - Pose3d( - Translation3d(0.00,0.0,0.0), - Rotation3d(0.0,0.0,90.0.degreesToRadians()) - ) - ) , - AprilTag(1, - Pose3d( - Translation3d(0.4445,0.0,0.0), - Rotation3d(0.0,0.0,90.0.degreesToRadians()) - ) -) -) -val testLayout1 = AprilTagFieldLayout(aprilTags1,10.0,5.0) -val aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile) - -/* Needs to be part of main season robot code, because it depends on robot-specific constants - -(Needs to import Constants.OdometryConstants.VisionDeviation) - -Good example code though... - -//TODO: Update for Photon -class Vision ( - CameraName: String, - aprilTagLayout: AprilTagFieldLayout -) { - val cam = PhotonCamera(CameraName); - var robotToCam = Transform3d( - Translation3d(0.0, 0.0, 0.0), - Rotation3d(0.0, 0.0, 0.0) - ) - - val aprilTagFieldLayout : AprilTagFieldLayout = aprilTagLayout - val PoseEstimator = PhotonPoseEstimator( - aprilTagFieldLayout, - PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - cam, - robotToCam - ) - fun getEstimatedPose(prevEstimatedRobotPose: Pose2d?): EstimatedRobotPose? { - return null - PoseEstimator.setReferencePose(prevEstimatedRobotPose) - val pose = PoseEstimator.update() ?: return null - if(pose.isPresent) return pose.get() - return null - } - fun getStdDev() : Matrix { - return Constants.OdometryConstants.VisionDeviation - } -} -*/ \ No newline at end of file From 32cdfd74431ba4a62af516fb158c642b3cc1c08f Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Wed, 5 Nov 2025 18:25:23 -0800 Subject: [PATCH 04/12] Fixed 1 billion warnings --- async/Promise.kt | 1 + 1 file changed, 1 insertion(+) diff --git a/async/Promise.kt b/async/Promise.kt index 0c97b16..87c2082 100644 --- a/async/Promise.kt +++ b/async/Promise.kt @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.event.BooleanEvent import edu.wpi.first.wpilibj.event.EventLoop @Deprecated("Promise is deprecated, if you need to use it, idk why you would need to") +@Suppress("DEPRECATION") class Promise(private val fn: (resolve: (value: T) -> Unit, reject: (error: Throwable) -> Unit) -> Unit) : Thenable { private var resolvedWith: T? = null private var rejectedWith: Throwable? = null From 5c3e5498ff1c48cf067f0b799c7198d7cbe8d140 Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Wed, 5 Nov 2025 18:35:13 -0800 Subject: [PATCH 05/12] Update Vector2.kt --- utils/Geometry/Vector2.kt | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/utils/Geometry/Vector2.kt b/utils/Geometry/Vector2.kt index bf071b0..7952717 100644 --- a/utils/Geometry/Vector2.kt +++ b/utils/Geometry/Vector2.kt @@ -7,7 +7,9 @@ import beaverlib.utils.Units.Angular.AngleUnit import beaverlib.utils.Units.Linear.* import kotlin.math.absoluteValue import kotlin.math.atan2 +import kotlin.math.cos import kotlin.math.pow +import kotlin.math.sin import kotlin.math.sqrt class Vector2(val x: Double, val y: Double) { @@ -27,19 +29,30 @@ class Vector2(val x: Double, val y: Double) { fun zero() = Vector2(0.0, 0.0) fun crossProduct(vector1 : Vector2, vector2: Vector2 ) = (vector1.x * vector2.y) - (vector1.y * vector2.x) infix fun Vector2.dotProduct(other: Vector2) = (this.x * other.x) + (this.y * other.y) - + fun lerp(v1: Vector2, v2 : Vector2, amount : Double) : Vector2{ + return v1 + ((v2 - v1) * amount) + } } constructor(pose: Pose2d) : this(pose.x, pose.y) constructor(angle: AngleUnit) : this(angle.cos(), angle.sin()) + /** @param angle Angle to rotate the vector by in radians*/ + fun rotateBy(angle: Double) : Vector2{ + return Vector2(cos(angle) *x - sin(angle) * y, + sin(angle)*x + cos(angle)*y) + } - /** - * Distance from 0, 0, calculated using pythagorean theorem - * */ + /** @param angle Angle to rotate the vector by in radians*/ + fun rotateBy(angle: AngleUnit) : Vector2{ + return rotateBy(angle.asRadians) + } + + /** Distance from 0, 0, calculated using pythagorean theorem */ val magnitude get() = sqrt(x.pow(2) + y.pow(2)) - fun angle() = AngleUnit(atan2(y,x)) - fun angleTo(other : Vector2) = (this-other).angle() + /** Angle of the vector */ + val angle get() = AngleUnit(atan2(y,x)) + fun angleTo(other : Vector2) = (this-other).angle fun distance(other: Vector2): Double = (this-other).magnitude fun distance(pose: Pose2d): Double = distance(Vector2(pose)) fun xdistance(pos: Double): Double = x - pos From d6b476e2d30930b7ba0b7c7124d71267b64257ab Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Wed, 5 Nov 2025 18:45:46 -0800 Subject: [PATCH 06/12] Units fix --- utils/Units/Angular/AngleUnit.kt | 26 ++++++++++++++++++++-- utils/Units/Angular/AngularAcceleration.kt | 4 +++- utils/Units/Angular/AngularVelocity.kt | 2 ++ utils/Units/Linear/Acceleration.kt | 2 +- 4 files changed, 30 insertions(+), 4 deletions(-) diff --git a/utils/Units/Angular/AngleUnit.kt b/utils/Units/Angular/AngleUnit.kt index 9156803..8b1c26f 100644 --- a/utils/Units/Angular/AngleUnit.kt +++ b/utils/Units/Angular/AngleUnit.kt @@ -7,9 +7,13 @@ import beaverlib.utils.Sugar.radiansToDegrees import beaverlib.utils.Units.Linear.DistanceUnit import beaverlib.utils.Units.Time import kotlin.math.PI +import kotlin.math.absoluteValue +import kotlin.math.sign @JvmInline value class AngleUnit(val asRadians: Double) { + override fun toString(): String = "$asDegrees degrees" + operator fun plus(other : AngleUnit) = AngleUnit(asRadians + other.asRadians) operator fun minus(other : AngleUnit) = AngleUnit(asRadians - other.asRadians) operator fun times(factor : Double) = AngleUnit(asRadians * factor) @@ -35,7 +39,19 @@ value class AngleUnit(val asRadians: Double) { // Unit conversion math operator fun div(other : Time) = AngularVelocity(asRadians/other.asSeconds) - operator fun times(other : DistanceUnit) = DistanceUnit(asRadians/other.asMeters) + operator fun times(other : DistanceUnit) = DistanceUnit(asRadians * other.asMeters) + + fun angleDistanceTo(other: AngleUnit): AngleUnit { + val normal = this - other + val wrap = -((2 * PI).radians * normal.asRadians.sign - normal) +// println("normal: ${normal} wrap: ${wrap} sign: ${normal.sign} angleA: $angleA angleB: $angleB") + return if (normal.asRadians.absoluteValue < wrap.asRadians.absoluteValue) { normal } + else { wrap } + } + + fun angleDistanceWithin(maxError : AngleUnit, target : AngleUnit): Boolean { + return this.angleDistanceTo(target).asRadians.absoluteValue < maxError.asRadians + } /** * Taken from mean machines mean lib @@ -100,4 +116,10 @@ inline val Number.degrees get() = AngleUnit(this.toDouble().degreesToRadians()) // destructors inline val AngleUnit.asRotations get() = asRadians / TAU -inline val AngleUnit.asDegrees get() = asRadians.radiansToDegrees() \ No newline at end of file +inline val AngleUnit.asDegrees get() = asRadians.radiansToDegrees() + +/** Wraps the angle such that it is between 0 and 360 degrees, 0 degrees is the left side of the circle, and angle increases counterclockwise*/ +inline val AngleUnit.standardPosition: AngleUnit + get() = + if (this.asRadians >= 0.0) { AngleUnit(this.asRadians % (2 * PI)) } + else { AngleUnit((2 * PI) + (this.asRadians % (2 * PI))) } \ No newline at end of file diff --git a/utils/Units/Angular/AngularAcceleration.kt b/utils/Units/Angular/AngularAcceleration.kt index c2f589b..e279b49 100644 --- a/utils/Units/Angular/AngularAcceleration.kt +++ b/utils/Units/Angular/AngularAcceleration.kt @@ -6,6 +6,8 @@ import beaverlib.utils.Units.Linear.DistanceUnit import beaverlib.utils.Units.Time class AngularAcceleration (val asRadiansPerSecondSquared: Double) { + override fun toString(): String = "$asRotationsPerSecondSquared rotations/s^2" + //Basic Math operator fun plus(other : AngularAcceleration) = AngularAcceleration(asRadiansPerSecondSquared + other.asRadiansPerSecondSquared) operator fun minus(other : AngularAcceleration) = AngularAcceleration(asRadiansPerSecondSquared - other.asRadiansPerSecondSquared) @@ -16,7 +18,7 @@ class AngularAcceleration (val asRadiansPerSecondSquared: Double) { operator fun unaryPlus() = this operator fun unaryMinus() = AngularAcceleration(-asRadiansPerSecondSquared) // Unit conversion math - operator fun times(other: Time) = AngularVelocity(asRadiansPerSecondSquared / other.asSeconds) + operator fun times(other: Time) = AngularVelocity(asRadiansPerSecondSquared * other.asSeconds) operator fun times(other: DistanceUnit) = Acceleration(asRadiansPerSecondSquared * other.asMeters) } diff --git a/utils/Units/Angular/AngularVelocity.kt b/utils/Units/Angular/AngularVelocity.kt index 7c4d861..a42eedb 100644 --- a/utils/Units/Angular/AngularVelocity.kt +++ b/utils/Units/Angular/AngularVelocity.kt @@ -6,6 +6,8 @@ import beaverlib.utils.Units.Linear.VelocityUnit import beaverlib.utils.Units.Time class AngularVelocity (val asRadiansPerSecond: Double) { + override fun toString(): String = "$asRPM RPM" + //Basic Math operator fun plus(other : AngularVelocity) = AngularAcceleration(asRadiansPerSecond + other.asRadiansPerSecond) operator fun minus(other : AngularVelocity) = AngularAcceleration(asRadiansPerSecond - other.asRadiansPerSecond) diff --git a/utils/Units/Linear/Acceleration.kt b/utils/Units/Linear/Acceleration.kt index ca16a0d..71ef533 100644 --- a/utils/Units/Linear/Acceleration.kt +++ b/utils/Units/Linear/Acceleration.kt @@ -12,7 +12,7 @@ value class Acceleration (val asMetersPerSecondSquared: Double) { operator fun unaryPlus() = this operator fun unaryMinus() = Acceleration(-asMetersPerSecondSquared) operator fun compareTo(other: Acceleration) = asMetersPerSecondSquared.compareTo(other.asMetersPerSecondSquared) - override fun toString() = "$asMetersPerSecondSquared m/s" + override fun toString() = "$asMetersPerSecondSquared m/s^2" } // Constructors From 09a2bbbfd9f4ae32cdc211ef27a5821d004aa58d Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Mon, 10 Nov 2025 15:57:45 -0800 Subject: [PATCH 07/12] Units ): --- utils/Units/Angular/AngleUnit.kt | 8 +- utils/Units/Angular/AngularAcceleration.kt | 4 - utils/Units/Angular/AngularVelocity.kt | 6 +- utils/Units/Electrical/Current.kt | 3 - utils/Units/Electrical/Energy.kt | 30 +++++ utils/Units/Electrical/Power.kt | 30 +++++ utils/Units/Electrical/Resistance.kt | 2 - utils/Units/Electrical/VoltageUnit.kt | 5 - utils/Units/Force.kt | 2 - utils/Units/Linear/Acceleration.kt | 3 +- utils/Units/Linear/DistanceUnit.kt | 2 +- utils/Units/Linear/VelocityUnit.kt | 9 +- utils/Units/Time.kt | 1 - utils/Units/UnitMath.kt | 130 +++++++++++++++++++++ utils/Units/Weight.kt | 1 - 15 files changed, 203 insertions(+), 33 deletions(-) create mode 100644 utils/Units/Electrical/Energy.kt create mode 100644 utils/Units/Electrical/Power.kt create mode 100644 utils/Units/UnitMath.kt diff --git a/utils/Units/Angular/AngleUnit.kt b/utils/Units/Angular/AngleUnit.kt index 8b1c26f..52c0c97 100644 --- a/utils/Units/Angular/AngleUnit.kt +++ b/utils/Units/Angular/AngleUnit.kt @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Rotation2d import beaverlib.utils.Sugar.TAU import beaverlib.utils.Sugar.degreesToRadians import beaverlib.utils.Sugar.radiansToDegrees +import beaverlib.utils.Units.Frequency import beaverlib.utils.Units.Linear.DistanceUnit import beaverlib.utils.Units.Time import kotlin.math.PI @@ -23,6 +24,7 @@ value class AngleUnit(val asRadians: Double) { operator fun rem(other: Number) = AngleUnit(asRadians % other.toDouble()) operator fun unaryPlus() = this operator fun unaryMinus() = AngleUnit(-asRadians) + fun getCoterminal() : AngleUnit { var coterminalAngle = this % TAU if(coterminalAngle.asRadians < 0) return TAU.radians - coterminalAngle @@ -35,12 +37,6 @@ value class AngleUnit(val asRadians: Double) { fun sec() = AngleUnit.sec(this) fun cot() = AngleUnit.tan(this) - - - // Unit conversion math - operator fun div(other : Time) = AngularVelocity(asRadians/other.asSeconds) - operator fun times(other : DistanceUnit) = DistanceUnit(asRadians * other.asMeters) - fun angleDistanceTo(other: AngleUnit): AngleUnit { val normal = this - other val wrap = -((2 * PI).radians * normal.asRadians.sign - normal) diff --git a/utils/Units/Angular/AngularAcceleration.kt b/utils/Units/Angular/AngularAcceleration.kt index e279b49..65a5286 100644 --- a/utils/Units/Angular/AngularAcceleration.kt +++ b/utils/Units/Angular/AngularAcceleration.kt @@ -17,10 +17,6 @@ class AngularAcceleration (val asRadiansPerSecondSquared: Double) { operator fun rem(other: AngularAcceleration) = AngularAcceleration(asRadiansPerSecondSquared % other.asRadiansPerSecondSquared) operator fun unaryPlus() = this operator fun unaryMinus() = AngularAcceleration(-asRadiansPerSecondSquared) - // Unit conversion math - operator fun times(other: Time) = AngularVelocity(asRadiansPerSecondSquared * other.asSeconds) - operator fun times(other: DistanceUnit) = Acceleration(asRadiansPerSecondSquared * other.asMeters) - } // Constructors inline val Number.radiansPerSecondSquared get() = AngularAcceleration(this.toDouble()) diff --git a/utils/Units/Angular/AngularVelocity.kt b/utils/Units/Angular/AngularVelocity.kt index a42eedb..6fe960e 100644 --- a/utils/Units/Angular/AngularVelocity.kt +++ b/utils/Units/Angular/AngularVelocity.kt @@ -1,6 +1,7 @@ package beaverlib.utils.Units.Angular import beaverlib.utils.Sugar.TAU +import beaverlib.utils.Units.Frequency import beaverlib.utils.Units.Linear.DistanceUnit import beaverlib.utils.Units.Linear.VelocityUnit import beaverlib.utils.Units.Time @@ -17,11 +18,6 @@ class AngularVelocity (val asRadiansPerSecond: Double) { operator fun rem(other: AngularVelocity) = AngularVelocity(asRadiansPerSecond % other.asRadiansPerSecond) operator fun unaryPlus() = this operator fun unaryMinus() = AngularVelocity(-asRadiansPerSecond) - // Unit conversion math - operator fun times(other: Time) = AngleUnit(asRadiansPerSecond * other.asSeconds) - operator fun div(other: Time) = AngularAcceleration(asRadiansPerSecond / other.asSeconds) - - operator fun times(other: DistanceUnit) = VelocityUnit(asRadiansPerSecond * other.asMeters) } // Constructors inline val Number.radiansPerSecond get() = AngularVelocity(this.toDouble()) diff --git a/utils/Units/Electrical/Current.kt b/utils/Units/Electrical/Current.kt index dac61e5..f60bf2a 100644 --- a/utils/Units/Electrical/Current.kt +++ b/utils/Units/Electrical/Current.kt @@ -11,9 +11,6 @@ value class Current (val asAmps: Double) { operator fun unaryMinus() = Current(-asAmps) operator fun compareTo(other: Current) = asAmps.compareTo(other.asAmps) override fun toString() = "$asAmps amps" - - //Unit converting math - operator fun times(other: Resistance) = VoltageUnit(asAmps * other.asOhms) } //Constructors diff --git a/utils/Units/Electrical/Energy.kt b/utils/Units/Electrical/Energy.kt new file mode 100644 index 0000000..544d269 --- /dev/null +++ b/utils/Units/Electrical/Energy.kt @@ -0,0 +1,30 @@ +package frc.robot.beaverlib.utils.Units.Electrical + + +@JvmInline +value class Energy (val asJoules: Double) { + // Basic Math + operator fun plus(other: Energy) = Energy(asJoules + other.asJoules) + operator fun minus(other: Energy) = Energy(asJoules - other.asJoules) + operator fun times(factor: Double) = Energy(asJoules * factor) + operator fun div(factor: Double) = Energy(asJoules / factor) + operator fun unaryPlus() = this + operator fun unaryMinus() = Energy(-asJoules) + operator fun compareTo(other: Energy) = asJoules.compareTo(other.asJoules) + override fun toString() = "$asJoules joules" +} + +//Constructors +val Number.milliJoules get() = Energy(toDouble()/1000) +val Number.joules get() = Energy(toDouble()) + +/** + * WARNING: IF YOU ARE USING THIS YOU ARE PROBABLY GOING TO BLOW SOMETHING UP + * Why do we have this in the codebase? please remove it; we're trying to avoid spaghetti + */ +val Number.kiloJoules get() = Energy(toDouble()*1000) + + +//Destructors +val Energy.asMillijoules get() = asJoules*1000 +val Energy.asKilojoules get() = asJoules/1000 \ No newline at end of file diff --git a/utils/Units/Electrical/Power.kt b/utils/Units/Electrical/Power.kt new file mode 100644 index 0000000..cd34847 --- /dev/null +++ b/utils/Units/Electrical/Power.kt @@ -0,0 +1,30 @@ +package frc.robot.beaverlib.utils.Units.Electrical + + +@JvmInline +value class Power (val asWatts: Double) { + // Basic Math + operator fun plus(other: Power) = Energy(asWatts + other.asWatts) + operator fun minus(other: Power) = Energy(asWatts - other.asWatts) + operator fun times(factor: Double) = Energy(asWatts * factor) + operator fun div(factor: Double) = Energy(asWatts / factor) + operator fun unaryPlus() = this + operator fun unaryMinus() = Energy(-asWatts) + operator fun compareTo(other: Energy) = asWatts.compareTo(other.asWatts) + override fun toString() = "$asWatts joules" +} + +//Constructors +val Number.milliWatts get() = Energy(toDouble()/1000) +val Number.watts get() = Energy(toDouble()) + +/** + * WARNING: IF YOU ARE USING THIS YOU ARE PROBABLY GOING TO BLOW SOMETHING UP + * Why do we have this in the codebase? please remove it; we're trying to avoid spaghetti + */ +val Number.kilowatts get() = Energy(toDouble()*1000) + + +//Destructors +val Energy.asMilliwatts get() = asWatts*1000 +val Energy.asKilowatts get() = asWatts/1000 \ No newline at end of file diff --git a/utils/Units/Electrical/Resistance.kt b/utils/Units/Electrical/Resistance.kt index 406b3f8..1e6a374 100644 --- a/utils/Units/Electrical/Resistance.kt +++ b/utils/Units/Electrical/Resistance.kt @@ -11,8 +11,6 @@ value class Resistance(val asOhms : Double) { operator fun unaryMinus() = Resistance(-asOhms) operator fun compareTo(other: Resistance) = asOhms.compareTo(other.asOhms) override fun toString() = "$asOhms ohms" - //Unit conversion math - operator fun times(other: Current) = VoltageUnit(asOhms*other.asAmps) } //Constructors diff --git a/utils/Units/Electrical/VoltageUnit.kt b/utils/Units/Electrical/VoltageUnit.kt index 39afbae..af03e3f 100644 --- a/utils/Units/Electrical/VoltageUnit.kt +++ b/utils/Units/Electrical/VoltageUnit.kt @@ -11,11 +11,6 @@ value class VoltageUnit(val asVolts: Double) { operator fun unaryMinus() = VoltageUnit(-asVolts) operator fun compareTo(other: VoltageUnit) = asVolts.compareTo(other.asVolts) override fun toString() = "$asVolts v" - - //Unit conversion math - operator fun div(other: Current) = Resistance(asVolts / other.asAmps) - operator fun div(other: Resistance) = Current(asVolts / other.asOhms) - } //Constructors diff --git a/utils/Units/Force.kt b/utils/Units/Force.kt index 5343db9..9723e87 100644 --- a/utils/Units/Force.kt +++ b/utils/Units/Force.kt @@ -9,8 +9,6 @@ value class Force (val asNewtons: Double) { operator fun minus(other : Force) = Force(asNewtons - other.asNewtons) operator fun times(factor : Double) = Force(asNewtons * factor.toDouble()) operator fun div(factor: Double) = Force(asNewtons / factor.toDouble()) - operator fun div(factor : Acceleration) = Weight(asNewtons / factor.asMetersPerSecondSquared) - operator fun div(factor : Weight) = Acceleration(asNewtons / factor.asKilograms) operator fun rem(other: Weight) = Force(asNewtons % other.asKilograms) operator fun unaryPlus() = this operator fun unaryMinus() = Force(-asNewtons) diff --git a/utils/Units/Linear/Acceleration.kt b/utils/Units/Linear/Acceleration.kt index 71ef533..8ab2eb0 100644 --- a/utils/Units/Linear/Acceleration.kt +++ b/utils/Units/Linear/Acceleration.kt @@ -1,13 +1,14 @@ package beaverlib.utils.Units.Linear import beaverlib.utils.Units.Time +import edu.wpi.first.units.AccelerationUnit +import edu.wpi.first.units.Unit @JvmInline value class Acceleration (val asMetersPerSecondSquared: Double) { operator fun plus(other: Acceleration) = Acceleration(asMetersPerSecondSquared + other.asMetersPerSecondSquared) operator fun minus(other: Acceleration) = Acceleration(asMetersPerSecondSquared - other.asMetersPerSecondSquared) operator fun times(factor: Double) = Acceleration(asMetersPerSecondSquared * factor) - operator fun times(factor: Time) = VelocityUnit(asMetersPerSecondSquared * factor.asSeconds) operator fun div(factor: Double) = Acceleration(asMetersPerSecondSquared / factor) operator fun unaryPlus() = this operator fun unaryMinus() = Acceleration(-asMetersPerSecondSquared) diff --git a/utils/Units/Linear/DistanceUnit.kt b/utils/Units/Linear/DistanceUnit.kt index f531a49..48cffd6 100644 --- a/utils/Units/Linear/DistanceUnit.kt +++ b/utils/Units/Linear/DistanceUnit.kt @@ -1,5 +1,6 @@ package beaverlib.utils.Units.Linear +import beaverlib.utils.Units.Frequency import beaverlib.utils.Units.Time import kotlin.math.pow @@ -10,7 +11,6 @@ value class DistanceUnit(val asMeters: Double) { operator fun minus(other: DistanceUnit) = DistanceUnit(asMeters-other.asMeters) operator fun times(factor: Number) = DistanceUnit(asMeters*factor.toDouble()) operator fun div(factor: Number) = DistanceUnit(asMeters/factor.toDouble()) - operator fun div(factor: Time) = VelocityUnit(asMeters/factor.asSeconds) operator fun rem(other: DistanceUnit) = DistanceUnit(asMeters % other.asMeters) operator fun unaryMinus() = DistanceUnit(-asMeters) diff --git a/utils/Units/Linear/VelocityUnit.kt b/utils/Units/Linear/VelocityUnit.kt index 944a411..5ef1b45 100644 --- a/utils/Units/Linear/VelocityUnit.kt +++ b/utils/Units/Linear/VelocityUnit.kt @@ -1,5 +1,6 @@ package beaverlib.utils.Units.Linear +import beaverlib.utils.Units.Frequency import beaverlib.utils.Units.Time @JvmInline @@ -7,13 +8,17 @@ value class VelocityUnit(val asMetersPerSecond: Double) { operator fun plus(other: VelocityUnit) = VelocityUnit(asMetersPerSecond + other.asMetersPerSecond) operator fun minus(other: VelocityUnit) = VelocityUnit(asMetersPerSecond - other.asMetersPerSecond) operator fun times(factor: Double) = VelocityUnit(asMetersPerSecond * factor) - operator fun times(factor: Time) = DistanceUnit(asMetersPerSecond * factor.asSeconds) operator fun div(factor: Double) = VelocityUnit(asMetersPerSecond / factor) - operator fun div(factor: Time) = Acceleration(asMetersPerSecond / factor.asSeconds) operator fun unaryPlus() = this operator fun unaryMinus() = VelocityUnit(-asMetersPerSecond) operator fun compareTo(other: VelocityUnit) = asMetersPerSecond.compareTo(other.asMetersPerSecond) override fun toString() = "$asMetersPerSecond m/s" + + + + + + } // constructos inline val Number.metersPerSecond get() = VelocityUnit(this.toDouble()) diff --git a/utils/Units/Time.kt b/utils/Units/Time.kt index 914a9ea..6f76cc6 100644 --- a/utils/Units/Time.kt +++ b/utils/Units/Time.kt @@ -14,7 +14,6 @@ value class Time (val asSeconds: Double){ operator fun unaryMinus() = Time(-asSeconds) operator fun compareTo(other: Time) = asSeconds.compareTo(other.asSeconds) - override fun toString(): String = "$asSeconds seconds" } operator fun Number.div(other: Time) = Frequency(this.toDouble()/other.asSeconds) diff --git a/utils/Units/UnitMath.kt b/utils/Units/UnitMath.kt new file mode 100644 index 0000000..018b343 --- /dev/null +++ b/utils/Units/UnitMath.kt @@ -0,0 +1,130 @@ +package frc.robot.beaverlib.utils.Units + +import beaverlib.utils.Units.Angular.AngleUnit +import beaverlib.utils.Units.Angular.AngularAcceleration +import beaverlib.utils.Units.Angular.AngularVelocity +import beaverlib.utils.Units.Electrical.Current +import beaverlib.utils.Units.Electrical.Resistance +import beaverlib.utils.Units.Electrical.VoltageUnit +import beaverlib.utils.Units.Force +import beaverlib.utils.Units.Frequency +import beaverlib.utils.Units.Linear.Acceleration +import beaverlib.utils.Units.Linear.DistanceUnit +import beaverlib.utils.Units.Linear.VelocityUnit +import beaverlib.utils.Units.Time +import beaverlib.utils.Units.Weight +import frc.robot.beaverlib.utils.Units.Electrical.Energy +import frc.robot.beaverlib.utils.Units.Electrical.Power + +// Angle Unit +operator fun AngleUnit.times(other : DistanceUnit) = DistanceUnit(asRadians * other.asMeters) +operator fun DistanceUnit.times(other : AngleUnit) = DistanceUnit(asMeters * other.asRadians) + +operator fun AngleUnit.times(other : Frequency) = AngularVelocity(asRadians * other.asHertz) +operator fun Frequency.times(other : AngleUnit) = AngularVelocity(asHertz * other.asRadians) + +operator fun AngleUnit.div(other : Time) = AngularVelocity(asRadians/other.asSeconds) +operator fun AngleUnit.div(other: AngleUnit) = asRadians / other.asRadians +operator fun AngleUnit.div(other: AngularVelocity) = Time(asRadians / other.asRadiansPerSecond) + + +// Angular Velocity +operator fun AngularVelocity.times(other: Time) = AngleUnit(asRadiansPerSecond * other.asSeconds) +operator fun Time.times(other: AngularVelocity) = AngleUnit(asSeconds * other.asRadiansPerSecond) + +operator fun AngularVelocity.times(other: DistanceUnit) = VelocityUnit(asRadiansPerSecond * other.asMeters) +operator fun DistanceUnit.times(other: AngularVelocity) = VelocityUnit(asMeters * other.asRadiansPerSecond) + +operator fun AngularVelocity.times(other: Frequency) = AngularAcceleration(asRadiansPerSecond * other.asHertz) +operator fun Frequency.times(other: AngularVelocity) = AngularAcceleration(asHertz * other.asRadiansPerSecond) + +operator fun AngularVelocity.div(other: Time) = AngularAcceleration(asRadiansPerSecond / other.asSeconds) +operator fun AngularVelocity.div(other: AngleUnit) = Frequency(asRadiansPerSecond / other.asRadians) +operator fun AngularVelocity.div(other: AngularAcceleration) = Time(asRadiansPerSecond / other.asRadiansPerSecondSquared) +operator fun AngularVelocity.div(other: AngularVelocity) = asRadiansPerSecond / other.asRadiansPerSecond +operator fun AngularVelocity.div(other: Frequency) = AngleUnit(asRadiansPerSecond / other.asHertz) + + +// Angular Acceleration +operator fun AngularAcceleration.times(other: Time) = AngularVelocity(asRadiansPerSecondSquared * other.asSeconds) +operator fun Time.times(other: AngularAcceleration) = AngularVelocity(asSeconds * other.asRadiansPerSecondSquared) + +operator fun AngularAcceleration.times(other: DistanceUnit) = Acceleration(asRadiansPerSecondSquared * other.asMeters) +operator fun DistanceUnit.times(other: AngularAcceleration) = Acceleration(asMeters * other.asRadiansPerSecondSquared) + +operator fun AngularAcceleration.div(other: Frequency) = AngularVelocity(asRadiansPerSecondSquared / other.asHertz) +operator fun AngularAcceleration.div(other: AngularVelocity) = Frequency(asRadiansPerSecondSquared * other.asRadiansPerSecond) +operator fun AngularAcceleration.div(other: AngularAcceleration) = asRadiansPerSecondSquared * other.asRadiansPerSecondSquared + + + +// Distance Unit +operator fun DistanceUnit.times(factor: Frequency) = VelocityUnit(asMeters*factor.asHertz) +operator fun Frequency.times(factor: DistanceUnit) = VelocityUnit(asHertz*factor.asMeters) + +operator fun DistanceUnit.div(factor: Time) = VelocityUnit(asMeters/factor.asSeconds) +operator fun DistanceUnit.div(factor: VelocityUnit) = Time(asMeters/factor.asMetersPerSecond) +operator fun DistanceUnit.div(factor: DistanceUnit) = asMeters/factor.asMeters + + + +// Velocity Unit +operator fun VelocityUnit.times(factor: Frequency) = Acceleration(asMetersPerSecond * factor.asHertz) +operator fun Frequency.times(factor: VelocityUnit) = Acceleration(asHertz * factor.asMetersPerSecond) + +operator fun VelocityUnit.times(factor: Time) = DistanceUnit(asMetersPerSecond * factor.asSeconds) +operator fun Time.times(factor: VelocityUnit) = DistanceUnit(asSeconds * factor.asMetersPerSecond) + +operator fun VelocityUnit.div(factor: DistanceUnit) = Frequency(asMetersPerSecond / factor.asMeters) +operator fun VelocityUnit.div(factor: Time) = Acceleration(asMetersPerSecond / factor.asSeconds) +operator fun VelocityUnit.div(factor: Frequency) = Acceleration(asMetersPerSecond / factor.asHertz) +operator fun VelocityUnit.div(factor: VelocityUnit) = asMetersPerSecond / factor.asMetersPerSecond + +// Acceleration Unit +operator fun Acceleration.times(factor: Time) = VelocityUnit(asMetersPerSecondSquared * factor.asSeconds) +operator fun Time.times(factor: Acceleration) = VelocityUnit(asSeconds * factor.asMetersPerSecondSquared) + +operator fun Acceleration.times(factor: Weight) = Force(asMetersPerSecondSquared * factor.asKilograms) +operator fun Weight.times(factor: Acceleration) = Force(asKilograms * factor.asMetersPerSecondSquared) + +operator fun Acceleration.div(factor: Frequency) = VelocityUnit(asMetersPerSecondSquared / factor.asHertz) +operator fun Acceleration.div(factor: VelocityUnit) = Frequency(asMetersPerSecondSquared / factor.asMetersPerSecond) +operator fun Acceleration.div(factor: Acceleration) = asMetersPerSecondSquared / factor.asMetersPerSecondSquared + +// Force +operator fun Force.times(other: DistanceUnit) = Energy(asNewtons * other.asMeters) +operator fun DistanceUnit.times(other: Force) = Energy(asMeters * other.asNewtons) + +operator fun Force.div(other: Weight) = Acceleration(asNewtons / other.asKilograms) +operator fun Force.div(other: Acceleration) = Weight(asNewtons / other.asMetersPerSecondSquared) +operator fun Force.div(other: Force) = asNewtons / other.asNewtons + + +// Electrical +operator fun VoltageUnit.times(other: Current) = Energy(asVolts * other.asAmps) +operator fun Current.times(other: VoltageUnit) = Energy(asAmps * other.asVolts) + +operator fun Current.times(other : Resistance) = VoltageUnit(asAmps * other.asOhms) +operator fun Resistance.times(other : Current) = VoltageUnit(asOhms * other.asAmps) + +operator fun VoltageUnit.div(other: Resistance) = Current(asVolts / other.asOhms) +operator fun VoltageUnit.div(other: Current) = Resistance(asVolts / other.asAmps) + +operator fun VoltageUnit.div(other: VoltageUnit) = asVolts / other.asVolts +operator fun Current.div(other: Current) = asAmps / other.asAmps +operator fun Resistance.div(other: Resistance) = asOhms / other.asOhms + + +// Energy +operator fun Energy.times(other : Frequency) = Power(asJoules * other.asHertz) +operator fun Frequency.times(other : Energy) = Power(asHertz * other.asJoules) + +operator fun Energy.div(other : Time) = Power(asJoules / other.asSeconds) +operator fun Energy.div(other : Energy) = asJoules / other.asJoules + +// Power +operator fun Power.times(other : Time) = Energy(asWatts * other.asSeconds) +operator fun Time.times(other : Power) = Energy(asSeconds * other.asWatts) + +operator fun Power.div(other : Frequency) = Energy(asWatts / other.asHertz) +operator fun Power.div(other : Power) = asWatts / other.asWatts diff --git a/utils/Units/Weight.kt b/utils/Units/Weight.kt index 26a6561..f08c7af 100644 --- a/utils/Units/Weight.kt +++ b/utils/Units/Weight.kt @@ -15,7 +15,6 @@ value class Weight(val asKilograms: Double) { operator fun unaryPlus() = this operator fun unaryMinus() = Weight(-asKilograms) } -operator fun Acceleration.times(other: Weight) = Force(this.asMetersPerSecondSquared * other.asKilograms) //Constructors inline val Number.kg get() = Weight(toDouble()) From 5041112d028451afdfb7364ed6a238772ea43fae Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Mon, 10 Nov 2025 16:30:21 -0800 Subject: [PATCH 08/12] Units Fixed --- utils/Units/Angular/AngleUnit.kt | 9 +- utils/Units/Angular/AngularAcceleration.kt | 13 ++- utils/Units/Angular/AngularVelocity.kt | 17 ++- utils/Units/Electrical/Current.kt | 11 +- utils/Units/Electrical/Energy.kt | 14 ++- utils/Units/Electrical/Power.kt | 17 ++- utils/Units/Electrical/Resistance.kt | 7 +- utils/Units/Electrical/VoltageUnit.kt | 10 ++ utils/Units/Force.kt | 13 +++ utils/Units/Frequency.kt | 16 ++- utils/Units/Linear/Acceleration.kt | 16 ++- utils/Units/Linear/DistanceUnit.kt | 10 ++ utils/Units/Linear/VelocityUnit.kt | 16 ++- utils/Units/Time.kt | 15 +++ utils/Units/UnitMath.kt | 130 --------------------- utils/Units/Weight.kt | 5 +- 16 files changed, 169 insertions(+), 150 deletions(-) delete mode 100644 utils/Units/UnitMath.kt diff --git a/utils/Units/Angular/AngleUnit.kt b/utils/Units/Angular/AngleUnit.kt index 52c0c97..467d402 100644 --- a/utils/Units/Angular/AngleUnit.kt +++ b/utils/Units/Angular/AngleUnit.kt @@ -98,9 +98,14 @@ value class AngleUnit(val asRadians: Double) { val left = AngleUnit(PI) /** An angle pointing down in standard position (3PI/2 radians) */ val down = AngleUnit(PI*3/2) + } + operator fun times(other : DistanceUnit) = DistanceUnit(asRadians * other.asMeters) + operator fun times(other : Frequency) = AngularVelocity(asRadians * other.asHertz) - } + operator fun div(other : Time) = AngularVelocity(asRadians/other.asSeconds) + operator fun div(other: AngleUnit) = asRadians / other.asRadians + operator fun div(other: AngularVelocity) = Time(asRadians / other.asRadiansPerSecond) } inline val Rotation2d.beaverRadians get() = AngleUnit(this.radians) @@ -118,4 +123,4 @@ inline val AngleUnit.asDegrees get() = asRadians.radiansToDegrees() inline val AngleUnit.standardPosition: AngleUnit get() = if (this.asRadians >= 0.0) { AngleUnit(this.asRadians % (2 * PI)) } - else { AngleUnit((2 * PI) + (this.asRadians % (2 * PI))) } \ No newline at end of file + else { AngleUnit((2 * PI) + (this.asRadians % (2 * PI))) } diff --git a/utils/Units/Angular/AngularAcceleration.kt b/utils/Units/Angular/AngularAcceleration.kt index 65a5286..0665811 100644 --- a/utils/Units/Angular/AngularAcceleration.kt +++ b/utils/Units/Angular/AngularAcceleration.kt @@ -1,6 +1,7 @@ package beaverlib.utils.Units.Angular import beaverlib.utils.Sugar.TAU +import beaverlib.utils.Units.Frequency import beaverlib.utils.Units.Linear.Acceleration import beaverlib.utils.Units.Linear.DistanceUnit import beaverlib.utils.Units.Time @@ -17,6 +18,15 @@ class AngularAcceleration (val asRadiansPerSecondSquared: Double) { operator fun rem(other: AngularAcceleration) = AngularAcceleration(asRadiansPerSecondSquared % other.asRadiansPerSecondSquared) operator fun unaryPlus() = this operator fun unaryMinus() = AngularAcceleration(-asRadiansPerSecondSquared) + + operator fun times(other: Time) = AngularVelocity(asRadiansPerSecondSquared * other.asSeconds) + + operator fun times(other: DistanceUnit) = Acceleration(asRadiansPerSecondSquared * other.asMeters) + + operator fun div(other: Frequency) = AngularVelocity(asRadiansPerSecondSquared / other.asHertz) + operator fun div(other: AngularVelocity) = Frequency(asRadiansPerSecondSquared * other.asRadiansPerSecond) + operator fun div(other: AngularAcceleration) = asRadiansPerSecondSquared * other.asRadiansPerSecondSquared + } // Constructors inline val Number.radiansPerSecondSquared get() = AngularAcceleration(this.toDouble()) @@ -25,4 +35,5 @@ inline val Number.RPMSquared get() = AngularAcceleration((this.toDouble()*TAU)/6 // Destructors inline val AngularAcceleration.asRotationsPerSecondSquared get() = asRadiansPerSecondSquared / TAU -inline val AngularAcceleration.asRPMSquared get() = ((asRadiansPerSecondSquared / TAU) * 60) \ No newline at end of file +inline val AngularAcceleration.asRPMSquared get() = ((asRadiansPerSecondSquared / TAU) * 60) + diff --git a/utils/Units/Angular/AngularVelocity.kt b/utils/Units/Angular/AngularVelocity.kt index 6fe960e..a234c79 100644 --- a/utils/Units/Angular/AngularVelocity.kt +++ b/utils/Units/Angular/AngularVelocity.kt @@ -18,6 +18,19 @@ class AngularVelocity (val asRadiansPerSecond: Double) { operator fun rem(other: AngularVelocity) = AngularVelocity(asRadiansPerSecond % other.asRadiansPerSecond) operator fun unaryPlus() = this operator fun unaryMinus() = AngularVelocity(-asRadiansPerSecond) + + operator fun times(other: Time) = AngleUnit(asRadiansPerSecond * other.asSeconds) + + operator fun times(other: DistanceUnit) = VelocityUnit(asRadiansPerSecond * other.asMeters) + + operator fun times(other: Frequency) = AngularAcceleration(asRadiansPerSecond * other.asHertz) + + operator fun div(other: Time) = AngularAcceleration(asRadiansPerSecond / other.asSeconds) + operator fun div(other: AngleUnit) = Frequency(asRadiansPerSecond / other.asRadians) + operator fun div(other: AngularAcceleration) = Time(asRadiansPerSecond / other.asRadiansPerSecondSquared) + operator fun div(other: AngularVelocity) = asRadiansPerSecond / other.asRadiansPerSecond + operator fun div(other: Frequency) = AngleUnit(asRadiansPerSecond / other.asHertz) + } // Constructors inline val Number.radiansPerSecond get() = AngularVelocity(this.toDouble()) @@ -26,4 +39,6 @@ inline val Number.RPM get() = AngularVelocity((this.toDouble()*TAU)/60) // Destructors inline val AngularVelocity.asRotationsPerSecond get() = asRadiansPerSecond / TAU -inline val AngularVelocity.asRPM get() = ((asRadiansPerSecond / TAU) * 60) \ No newline at end of file +inline val AngularVelocity.asRPM get() = ((asRadiansPerSecond / TAU) * 60) + + diff --git a/utils/Units/Electrical/Current.kt b/utils/Units/Electrical/Current.kt index f60bf2a..ef536ad 100644 --- a/utils/Units/Electrical/Current.kt +++ b/utils/Units/Electrical/Current.kt @@ -1,5 +1,7 @@ package beaverlib.utils.Units.Electrical +import frc.robot.beaverlib.utils.Units.Electrical.Energy + @JvmInline value class Current (val asAmps: Double) { // Basic Math @@ -11,6 +13,12 @@ value class Current (val asAmps: Double) { operator fun unaryMinus() = Current(-asAmps) operator fun compareTo(other: Current) = asAmps.compareTo(other.asAmps) override fun toString() = "$asAmps amps" + + // Unit Math + operator fun times(other: VoltageUnit) = Energy(asAmps * other.asVolts) + operator fun times(other : Resistance) = VoltageUnit(asAmps * other.asOhms) + operator fun div(other: Current) = asAmps / other.asAmps + } //Constructors @@ -26,4 +34,5 @@ val Number.kiloAmps get() = Current(toDouble()*1000) //Destructors val Current.asMilliAmps get() = asAmps*1000 -val Current.asAmps get() = asAmps/1000 \ No newline at end of file +val Current.asKiloamps get() = asAmps/1000 + diff --git a/utils/Units/Electrical/Energy.kt b/utils/Units/Electrical/Energy.kt index 544d269..fff9907 100644 --- a/utils/Units/Electrical/Energy.kt +++ b/utils/Units/Electrical/Energy.kt @@ -1,5 +1,8 @@ package frc.robot.beaverlib.utils.Units.Electrical +import beaverlib.utils.Units.Frequency +import beaverlib.utils.Units.Time + @JvmInline value class Energy (val asJoules: Double) { @@ -12,6 +15,14 @@ value class Energy (val asJoules: Double) { operator fun unaryMinus() = Energy(-asJoules) operator fun compareTo(other: Energy) = asJoules.compareTo(other.asJoules) override fun toString() = "$asJoules joules" + + // Unit Math + operator fun times(other : Frequency) = Power(asJoules * other.asHertz) + operator fun Frequency.times(other : Energy) = Power(asHertz * other.asJoules) + + operator fun div(other : Time) = Power(asJoules / other.asSeconds) + operator fun div(other : Energy) = asJoules / other.asJoules + } //Constructors @@ -27,4 +38,5 @@ val Number.kiloJoules get() = Energy(toDouble()*1000) //Destructors val Energy.asMillijoules get() = asJoules*1000 -val Energy.asKilojoules get() = asJoules/1000 \ No newline at end of file +val Energy.asKilojoules get() = asJoules/1000 + diff --git a/utils/Units/Electrical/Power.kt b/utils/Units/Electrical/Power.kt index cd34847..9167d30 100644 --- a/utils/Units/Electrical/Power.kt +++ b/utils/Units/Electrical/Power.kt @@ -1,5 +1,8 @@ package frc.robot.beaverlib.utils.Units.Electrical +import beaverlib.utils.Units.Frequency +import beaverlib.utils.Units.Time + @JvmInline value class Power (val asWatts: Double) { @@ -10,8 +13,15 @@ value class Power (val asWatts: Double) { operator fun div(factor: Double) = Energy(asWatts / factor) operator fun unaryPlus() = this operator fun unaryMinus() = Energy(-asWatts) - operator fun compareTo(other: Energy) = asWatts.compareTo(other.asWatts) + operator fun compareTo(other: Power) = asWatts.compareTo(other.asWatts) override fun toString() = "$asWatts joules" + + // Unit Math + operator fun times(other : Time) = Energy(asWatts * other.asSeconds) + + operator fun div(other : Frequency) = Energy(asWatts / other.asHertz) + operator fun div(other : Power) = asWatts / other.asWatts + } //Constructors @@ -26,5 +36,6 @@ val Number.kilowatts get() = Energy(toDouble()*1000) //Destructors -val Energy.asMilliwatts get() = asWatts*1000 -val Energy.asKilowatts get() = asWatts/1000 \ No newline at end of file +val Power.asMilliwatts get() = asWatts*1000 +val Power.asKilowatts get() = asWatts/1000 + diff --git a/utils/Units/Electrical/Resistance.kt b/utils/Units/Electrical/Resistance.kt index 1e6a374..83e6e5b 100644 --- a/utils/Units/Electrical/Resistance.kt +++ b/utils/Units/Electrical/Resistance.kt @@ -11,6 +11,10 @@ value class Resistance(val asOhms : Double) { operator fun unaryMinus() = Resistance(-asOhms) operator fun compareTo(other: Resistance) = asOhms.compareTo(other.asOhms) override fun toString() = "$asOhms ohms" + + // Unit Math + operator fun times(other : Current) = VoltageUnit(asOhms * other.asAmps) + operator fun div(other: Resistance) = asOhms / other.asOhms } //Constructors @@ -20,4 +24,5 @@ val Number.kiloOhms get() = Resistance(toDouble()*1000) //Destructors val Resistance.asMilliOhms get() = asOhms*1000 -val Resistance.asOhms get() = asOhms/1000 \ No newline at end of file +val Resistance.asOhms get() = asOhms/1000 + diff --git a/utils/Units/Electrical/VoltageUnit.kt b/utils/Units/Electrical/VoltageUnit.kt index af03e3f..19f7cd5 100644 --- a/utils/Units/Electrical/VoltageUnit.kt +++ b/utils/Units/Electrical/VoltageUnit.kt @@ -1,5 +1,7 @@ package beaverlib.utils.Units.Electrical +import frc.robot.beaverlib.utils.Units.Electrical.Energy + @JvmInline value class VoltageUnit(val asVolts: Double) { // Basic Math @@ -11,6 +13,13 @@ value class VoltageUnit(val asVolts: Double) { operator fun unaryMinus() = VoltageUnit(-asVolts) operator fun compareTo(other: VoltageUnit) = asVolts.compareTo(other.asVolts) override fun toString() = "$asVolts v" + + // Unit Math + operator fun times(other: Current) = Energy(asVolts * other.asAmps) + operator fun div(other: Resistance) = Current(asVolts / other.asOhms) + operator fun div(other: Current) = Resistance(asVolts / other.asAmps) + operator fun div(other: VoltageUnit) = asVolts / other.asVolts + } //Constructors @@ -26,3 +35,4 @@ val Number.kiloVolts get() = VoltageUnit(toDouble()*1000) val VoltageUnit.asMilliVolts get() = asVolts*1000 val VoltageUnit.asKilovolts get() = asVolts/1000 + diff --git a/utils/Units/Force.kt b/utils/Units/Force.kt index 9723e87..3ceea8d 100644 --- a/utils/Units/Force.kt +++ b/utils/Units/Force.kt @@ -1,7 +1,9 @@ package beaverlib.utils.Units import beaverlib.utils.Units.Linear.Acceleration +import beaverlib.utils.Units.Linear.DistanceUnit import beaverlib.utils.Units.Linear.earthGravity +import frc.robot.beaverlib.utils.Units.Electrical.Energy @JvmInline value class Force (val asNewtons: Double) { @@ -12,6 +14,17 @@ value class Force (val asNewtons: Double) { operator fun rem(other: Weight) = Force(asNewtons % other.asKilograms) operator fun unaryPlus() = this operator fun unaryMinus() = Force(-asNewtons) + + + // Unit Math + operator fun times(other: DistanceUnit) = Energy(asNewtons * other.asMeters) + operator fun DistanceUnit.times(other: Force) = Energy(asMeters * other.asNewtons) + + operator fun div(other: Weight) = Acceleration(asNewtons / other.asKilograms) + operator fun div(other: Acceleration) = Weight(asNewtons / other.asMetersPerSecondSquared) + operator fun div(other: Force) = asNewtons / other.asNewtons + + } // Constructors inline val Number.newtons get() = Force(toDouble()) diff --git a/utils/Units/Frequency.kt b/utils/Units/Frequency.kt index f197674..11c5bf4 100644 --- a/utils/Units/Frequency.kt +++ b/utils/Units/Frequency.kt @@ -1,10 +1,15 @@ package beaverlib.utils.Units +import beaverlib.utils.Units.Angular.AngleUnit +import beaverlib.utils.Units.Angular.AngularAcceleration +import beaverlib.utils.Units.Angular.AngularVelocity +import beaverlib.utils.Units.Linear.DistanceUnit +import beaverlib.utils.Units.Linear.VelocityUnit + class Frequency(val asHertz: Double) { operator fun plus(other: Frequency) = Frequency(asHertz + other.asHertz) operator fun minus(other: Frequency) = Frequency(asHertz - other.asHertz) operator fun times(factor: Frequency) = Frequency(asHertz - factor.asHertz) - operator fun times(other: Time) = other.asSeconds / asHertz operator fun times(factor: Number) = Frequency(asHertz - factor.toDouble()) operator fun div(factor: Number) = Frequency(asHertz / factor.toDouble()) @@ -14,6 +19,15 @@ class Frequency(val asHertz: Double) { operator fun compareTo(other: Frequency) = asHertz.compareTo(other.asHertz) override fun toString(): String = "$asHertz hz" + + operator fun times(other : AngleUnit) = AngularVelocity(asHertz * other.asRadians) + operator fun times(factor: DistanceUnit) = VelocityUnit(asHertz*factor.asMeters) + operator fun times(other: Time) = other.asSeconds / asHertz + operator fun times(other: AngularVelocity) = AngularAcceleration(asHertz * other.asRadiansPerSecond) + + + + } operator fun Number.div(other: Frequency) = Time(this.toDouble()/other.asHertz) diff --git a/utils/Units/Linear/Acceleration.kt b/utils/Units/Linear/Acceleration.kt index 8ab2eb0..0ea95a3 100644 --- a/utils/Units/Linear/Acceleration.kt +++ b/utils/Units/Linear/Acceleration.kt @@ -1,6 +1,9 @@ package beaverlib.utils.Units.Linear +import beaverlib.utils.Units.Force +import beaverlib.utils.Units.Frequency import beaverlib.utils.Units.Time +import beaverlib.utils.Units.Weight import edu.wpi.first.units.AccelerationUnit import edu.wpi.first.units.Unit @@ -14,6 +17,15 @@ value class Acceleration (val asMetersPerSecondSquared: Double) { operator fun unaryMinus() = Acceleration(-asMetersPerSecondSquared) operator fun compareTo(other: Acceleration) = asMetersPerSecondSquared.compareTo(other.asMetersPerSecondSquared) override fun toString() = "$asMetersPerSecondSquared m/s^2" + + operator fun times(factor: Time) = VelocityUnit(asMetersPerSecondSquared * factor.asSeconds) + + operator fun times(factor: Weight) = Force(asMetersPerSecondSquared * factor.asKilograms) + + operator fun div(factor: Frequency) = VelocityUnit(asMetersPerSecondSquared / factor.asHertz) + operator fun div(factor: VelocityUnit) = Frequency(asMetersPerSecondSquared / factor.asMetersPerSecond) + operator fun div(factor: Acceleration) = asMetersPerSecondSquared / factor.asMetersPerSecondSquared + } // Constructors @@ -25,4 +37,6 @@ inline val Number.feetPerSecondSquared get() = Acceleration(this.toDouble() * 0. // Destructors inline val Acceleration.asFeetPerSecondSquared get() = asMetersPerSecondSquared / 0.3048 -val earthGravity = Acceleration(9.80665) \ No newline at end of file +val earthGravity = Acceleration(9.80665) + +// Unit Math \ No newline at end of file diff --git a/utils/Units/Linear/DistanceUnit.kt b/utils/Units/Linear/DistanceUnit.kt index 48cffd6..448058a 100644 --- a/utils/Units/Linear/DistanceUnit.kt +++ b/utils/Units/Linear/DistanceUnit.kt @@ -1,5 +1,8 @@ package beaverlib.utils.Units.Linear +import beaverlib.utils.Units.Angular.AngleUnit +import beaverlib.utils.Units.Angular.AngularAcceleration +import beaverlib.utils.Units.Angular.AngularVelocity import beaverlib.utils.Units.Frequency import beaverlib.utils.Units.Time import kotlin.math.pow @@ -21,6 +24,13 @@ value class DistanceUnit(val asMeters: Double) { override fun toString() = "$asInches inches" + operator fun times(factor: Frequency) = VelocityUnit(asMeters*factor.asHertz) + operator fun div(factor: Time) = VelocityUnit(asMeters/factor.asSeconds) + operator fun div(factor: VelocityUnit) = Time(asMeters/factor.asMetersPerSecond) + operator fun div(factor: DistanceUnit) = asMeters/factor.asMeters + operator fun times(other : AngleUnit) = DistanceUnit(asMeters * other.asRadians) + operator fun times(other: AngularAcceleration) = Acceleration(asMeters * other.asRadiansPerSecondSquared) + operator fun times(other: AngularVelocity) = VelocityUnit(asMeters * other.asRadiansPerSecond) } // Constructor inline val Number.meters get() = DistanceUnit(this.toDouble()) diff --git a/utils/Units/Linear/VelocityUnit.kt b/utils/Units/Linear/VelocityUnit.kt index 5ef1b45..b548767 100644 --- a/utils/Units/Linear/VelocityUnit.kt +++ b/utils/Units/Linear/VelocityUnit.kt @@ -14,11 +14,15 @@ value class VelocityUnit(val asMetersPerSecond: Double) { operator fun compareTo(other: VelocityUnit) = asMetersPerSecond.compareTo(other.asMetersPerSecond) override fun toString() = "$asMetersPerSecond m/s" - - - - - + operator fun times(factor: Frequency) = Acceleration(asMetersPerSecond * factor.asHertz) + operator fun times(factor: Time) = DistanceUnit(asMetersPerSecond * factor.asSeconds) + operator fun Frequency.times(factor: VelocityUnit) = Acceleration(asHertz * factor.asMetersPerSecond) + operator fun Time.times(factor: VelocityUnit) = DistanceUnit(asSeconds * factor.asMetersPerSecond) + + operator fun div(factor: DistanceUnit) = Frequency(asMetersPerSecond / factor.asMeters) + operator fun div(factor: Time) = Acceleration(asMetersPerSecond / factor.asSeconds) + operator fun div(factor: Frequency) = Acceleration(asMetersPerSecond / factor.asHertz) + operator fun div(factor: VelocityUnit) = asMetersPerSecond / factor.asMetersPerSecond } // constructos inline val Number.metersPerSecond get() = VelocityUnit(this.toDouble()) @@ -29,4 +33,4 @@ inline val Number.MPH get() = VelocityUnit(this.toDouble() * 0.44704) // decsontrucos inline val VelocityUnit.asFeetPerSecond get() = asMetersPerSecond / 0.3084 inline val VelocityUnit.asKilometerPerHour get() = asMetersPerSecond * 3.6 -inline val VelocityUnit.asMPH get() = asMetersPerSecond / 0.44704 \ No newline at end of file +inline val VelocityUnit.asMPH get() = asMetersPerSecond / 0.44704 diff --git a/utils/Units/Time.kt b/utils/Units/Time.kt index 6f76cc6..ca324ed 100644 --- a/utils/Units/Time.kt +++ b/utils/Units/Time.kt @@ -1,6 +1,13 @@ package beaverlib.utils.Units +import beaverlib.utils.Units.Angular.AngleUnit +import beaverlib.utils.Units.Angular.AngularAcceleration +import beaverlib.utils.Units.Angular.AngularVelocity +import beaverlib.utils.Units.Linear.Acceleration +import beaverlib.utils.Units.Linear.VelocityUnit import edu.wpi.first.math.Num +import frc.robot.beaverlib.utils.Units.Electrical.Energy +import frc.robot.beaverlib.utils.Units.Electrical.Power @JvmInline value class Time (val asSeconds: Double){ @@ -15,6 +22,14 @@ value class Time (val asSeconds: Double){ operator fun unaryMinus() = Time(-asSeconds) operator fun compareTo(other: Time) = asSeconds.compareTo(other.asSeconds) override fun toString(): String = "$asSeconds seconds" + + operator fun times(other: AngularAcceleration) = AngularVelocity(asSeconds * other.asRadiansPerSecondSquared) + operator fun times(other: AngularVelocity) = AngleUnit(asSeconds * other.asRadiansPerSecond) + operator fun times(other : Power) = Energy(asSeconds * other.asWatts) + operator fun times(factor: Acceleration) = VelocityUnit(asSeconds * factor.asMetersPerSecondSquared) + + + } operator fun Number.div(other: Time) = Frequency(this.toDouble()/other.asSeconds) diff --git a/utils/Units/UnitMath.kt b/utils/Units/UnitMath.kt deleted file mode 100644 index 018b343..0000000 --- a/utils/Units/UnitMath.kt +++ /dev/null @@ -1,130 +0,0 @@ -package frc.robot.beaverlib.utils.Units - -import beaverlib.utils.Units.Angular.AngleUnit -import beaverlib.utils.Units.Angular.AngularAcceleration -import beaverlib.utils.Units.Angular.AngularVelocity -import beaverlib.utils.Units.Electrical.Current -import beaverlib.utils.Units.Electrical.Resistance -import beaverlib.utils.Units.Electrical.VoltageUnit -import beaverlib.utils.Units.Force -import beaverlib.utils.Units.Frequency -import beaverlib.utils.Units.Linear.Acceleration -import beaverlib.utils.Units.Linear.DistanceUnit -import beaverlib.utils.Units.Linear.VelocityUnit -import beaverlib.utils.Units.Time -import beaverlib.utils.Units.Weight -import frc.robot.beaverlib.utils.Units.Electrical.Energy -import frc.robot.beaverlib.utils.Units.Electrical.Power - -// Angle Unit -operator fun AngleUnit.times(other : DistanceUnit) = DistanceUnit(asRadians * other.asMeters) -operator fun DistanceUnit.times(other : AngleUnit) = DistanceUnit(asMeters * other.asRadians) - -operator fun AngleUnit.times(other : Frequency) = AngularVelocity(asRadians * other.asHertz) -operator fun Frequency.times(other : AngleUnit) = AngularVelocity(asHertz * other.asRadians) - -operator fun AngleUnit.div(other : Time) = AngularVelocity(asRadians/other.asSeconds) -operator fun AngleUnit.div(other: AngleUnit) = asRadians / other.asRadians -operator fun AngleUnit.div(other: AngularVelocity) = Time(asRadians / other.asRadiansPerSecond) - - -// Angular Velocity -operator fun AngularVelocity.times(other: Time) = AngleUnit(asRadiansPerSecond * other.asSeconds) -operator fun Time.times(other: AngularVelocity) = AngleUnit(asSeconds * other.asRadiansPerSecond) - -operator fun AngularVelocity.times(other: DistanceUnit) = VelocityUnit(asRadiansPerSecond * other.asMeters) -operator fun DistanceUnit.times(other: AngularVelocity) = VelocityUnit(asMeters * other.asRadiansPerSecond) - -operator fun AngularVelocity.times(other: Frequency) = AngularAcceleration(asRadiansPerSecond * other.asHertz) -operator fun Frequency.times(other: AngularVelocity) = AngularAcceleration(asHertz * other.asRadiansPerSecond) - -operator fun AngularVelocity.div(other: Time) = AngularAcceleration(asRadiansPerSecond / other.asSeconds) -operator fun AngularVelocity.div(other: AngleUnit) = Frequency(asRadiansPerSecond / other.asRadians) -operator fun AngularVelocity.div(other: AngularAcceleration) = Time(asRadiansPerSecond / other.asRadiansPerSecondSquared) -operator fun AngularVelocity.div(other: AngularVelocity) = asRadiansPerSecond / other.asRadiansPerSecond -operator fun AngularVelocity.div(other: Frequency) = AngleUnit(asRadiansPerSecond / other.asHertz) - - -// Angular Acceleration -operator fun AngularAcceleration.times(other: Time) = AngularVelocity(asRadiansPerSecondSquared * other.asSeconds) -operator fun Time.times(other: AngularAcceleration) = AngularVelocity(asSeconds * other.asRadiansPerSecondSquared) - -operator fun AngularAcceleration.times(other: DistanceUnit) = Acceleration(asRadiansPerSecondSquared * other.asMeters) -operator fun DistanceUnit.times(other: AngularAcceleration) = Acceleration(asMeters * other.asRadiansPerSecondSquared) - -operator fun AngularAcceleration.div(other: Frequency) = AngularVelocity(asRadiansPerSecondSquared / other.asHertz) -operator fun AngularAcceleration.div(other: AngularVelocity) = Frequency(asRadiansPerSecondSquared * other.asRadiansPerSecond) -operator fun AngularAcceleration.div(other: AngularAcceleration) = asRadiansPerSecondSquared * other.asRadiansPerSecondSquared - - - -// Distance Unit -operator fun DistanceUnit.times(factor: Frequency) = VelocityUnit(asMeters*factor.asHertz) -operator fun Frequency.times(factor: DistanceUnit) = VelocityUnit(asHertz*factor.asMeters) - -operator fun DistanceUnit.div(factor: Time) = VelocityUnit(asMeters/factor.asSeconds) -operator fun DistanceUnit.div(factor: VelocityUnit) = Time(asMeters/factor.asMetersPerSecond) -operator fun DistanceUnit.div(factor: DistanceUnit) = asMeters/factor.asMeters - - - -// Velocity Unit -operator fun VelocityUnit.times(factor: Frequency) = Acceleration(asMetersPerSecond * factor.asHertz) -operator fun Frequency.times(factor: VelocityUnit) = Acceleration(asHertz * factor.asMetersPerSecond) - -operator fun VelocityUnit.times(factor: Time) = DistanceUnit(asMetersPerSecond * factor.asSeconds) -operator fun Time.times(factor: VelocityUnit) = DistanceUnit(asSeconds * factor.asMetersPerSecond) - -operator fun VelocityUnit.div(factor: DistanceUnit) = Frequency(asMetersPerSecond / factor.asMeters) -operator fun VelocityUnit.div(factor: Time) = Acceleration(asMetersPerSecond / factor.asSeconds) -operator fun VelocityUnit.div(factor: Frequency) = Acceleration(asMetersPerSecond / factor.asHertz) -operator fun VelocityUnit.div(factor: VelocityUnit) = asMetersPerSecond / factor.asMetersPerSecond - -// Acceleration Unit -operator fun Acceleration.times(factor: Time) = VelocityUnit(asMetersPerSecondSquared * factor.asSeconds) -operator fun Time.times(factor: Acceleration) = VelocityUnit(asSeconds * factor.asMetersPerSecondSquared) - -operator fun Acceleration.times(factor: Weight) = Force(asMetersPerSecondSquared * factor.asKilograms) -operator fun Weight.times(factor: Acceleration) = Force(asKilograms * factor.asMetersPerSecondSquared) - -operator fun Acceleration.div(factor: Frequency) = VelocityUnit(asMetersPerSecondSquared / factor.asHertz) -operator fun Acceleration.div(factor: VelocityUnit) = Frequency(asMetersPerSecondSquared / factor.asMetersPerSecond) -operator fun Acceleration.div(factor: Acceleration) = asMetersPerSecondSquared / factor.asMetersPerSecondSquared - -// Force -operator fun Force.times(other: DistanceUnit) = Energy(asNewtons * other.asMeters) -operator fun DistanceUnit.times(other: Force) = Energy(asMeters * other.asNewtons) - -operator fun Force.div(other: Weight) = Acceleration(asNewtons / other.asKilograms) -operator fun Force.div(other: Acceleration) = Weight(asNewtons / other.asMetersPerSecondSquared) -operator fun Force.div(other: Force) = asNewtons / other.asNewtons - - -// Electrical -operator fun VoltageUnit.times(other: Current) = Energy(asVolts * other.asAmps) -operator fun Current.times(other: VoltageUnit) = Energy(asAmps * other.asVolts) - -operator fun Current.times(other : Resistance) = VoltageUnit(asAmps * other.asOhms) -operator fun Resistance.times(other : Current) = VoltageUnit(asOhms * other.asAmps) - -operator fun VoltageUnit.div(other: Resistance) = Current(asVolts / other.asOhms) -operator fun VoltageUnit.div(other: Current) = Resistance(asVolts / other.asAmps) - -operator fun VoltageUnit.div(other: VoltageUnit) = asVolts / other.asVolts -operator fun Current.div(other: Current) = asAmps / other.asAmps -operator fun Resistance.div(other: Resistance) = asOhms / other.asOhms - - -// Energy -operator fun Energy.times(other : Frequency) = Power(asJoules * other.asHertz) -operator fun Frequency.times(other : Energy) = Power(asHertz * other.asJoules) - -operator fun Energy.div(other : Time) = Power(asJoules / other.asSeconds) -operator fun Energy.div(other : Energy) = asJoules / other.asJoules - -// Power -operator fun Power.times(other : Time) = Energy(asWatts * other.asSeconds) -operator fun Time.times(other : Power) = Energy(asSeconds * other.asWatts) - -operator fun Power.div(other : Frequency) = Energy(asWatts / other.asHertz) -operator fun Power.div(other : Power) = asWatts / other.asWatts diff --git a/utils/Units/Weight.kt b/utils/Units/Weight.kt index f08c7af..c65407b 100644 --- a/utils/Units/Weight.kt +++ b/utils/Units/Weight.kt @@ -7,13 +7,14 @@ value class Weight(val asKilograms: Double) { operator fun plus(other : Weight) = Weight(asKilograms + other.asKilograms) operator fun minus(other : Weight) = Weight(asKilograms - other.asKilograms) operator fun times(factor : Number) = Weight(asKilograms * factor.toDouble()) - operator fun times(factor : Acceleration) = Weight(asKilograms * factor.asMetersPerSecondSquared) - operator fun div(factor: Double) = Weight(asKilograms / factor.toDouble()) operator fun rem(other: Weight) = Weight(asKilograms % other.asKilograms) operator fun unaryPlus() = this operator fun unaryMinus() = Weight(-asKilograms) + + operator fun times(factor: Acceleration) = Force(asKilograms * factor.asMetersPerSecondSquared) + } //Constructors From f1fd45149cbf599ca558d7a786c430bb7d3916a9 Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Wed, 3 Dec 2025 16:58:13 -0800 Subject: [PATCH 09/12] Added unit function to Vector2 --- utils/Geometry/Vector2.kt | 122 ++++++++++++++++++++++++-------------- 1 file changed, 76 insertions(+), 46 deletions(-) diff --git a/utils/Geometry/Vector2.kt b/utils/Geometry/Vector2.kt index 7952717..650fa33 100644 --- a/utils/Geometry/Vector2.kt +++ b/utils/Geometry/Vector2.kt @@ -1,88 +1,118 @@ package beaverlib.utils.geometry -// File adapted from 2898's bpsrobotics engine -import edu.wpi.first.math.Num + +// File adapted from 2898's bpsrobotics engine +import beaverlib.utils.Units.Angular.AngleUnit +import beaverlib.utils.Units.Linear.Acceleration +import beaverlib.utils.Units.Linear.DistanceUnit +import beaverlib.utils.Units.Linear.VelocityUnit +import beaverlib.utils.Units.Linear.asFeetPerSecondSquared import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d -import beaverlib.utils.Units.Angular.AngleUnit -import beaverlib.utils.Units.Linear.* -import kotlin.math.absoluteValue -import kotlin.math.atan2 -import kotlin.math.cos -import kotlin.math.pow -import kotlin.math.sin -import kotlin.math.sqrt +import kotlin.math.* class Vector2(val x: Double, val y: Double) { companion object { - @JvmName("FromNumber") - fun new(x : Number, y : Number) = Vector2(x.toDouble(), y.toDouble()) + @JvmName("FromNumber") fun new(x: Number, y: Number) = Vector2(x.toDouble(), y.toDouble()) + @JvmName("fromMagnitude&Angle") - fun new(angle : AngleUnit, magnitude : Double) = Vector2(angle.cos()*magnitude, angle.sin()*magnitude) + fun new(angle: AngleUnit, magnitude: Double) = + Vector2(angle.cos() * magnitude, angle.sin() * magnitude) + @JvmName("fromDistance") fun new(x: DistanceUnit, y: DistanceUnit) = Vector2(x.asMeters, y.asMeters) + @JvmName("fromVelocity") - fun new(x: VelocityUnit, y: VelocityUnit) = Vector2(x.asMetersPerSecond, y.asMetersPerSecond) - @JvmName("fromAcceleration") - fun new(x: Acceleration, y: Acceleration) = Vector2(x.asMetersPerSecondSquared, y.asFeetPerSecondSquared) + fun new(x: VelocityUnit, y: VelocityUnit) = + Vector2(x.asMetersPerSecond, y.asMetersPerSecond) + @JvmName("fromAcceleration") + fun new(x: Acceleration, y: Acceleration) = + Vector2(x.asMetersPerSecondSquared, y.asFeetPerSecondSquared) fun zero() = Vector2(0.0, 0.0) - fun crossProduct(vector1 : Vector2, vector2: Vector2 ) = (vector1.x * vector2.y) - (vector1.y * vector2.x) + + fun crossProduct(vector1: Vector2, vector2: Vector2) = + (vector1.x * vector2.y) - (vector1.y * vector2.x) + infix fun Vector2.dotProduct(other: Vector2) = (this.x * other.x) + (this.y * other.y) - fun lerp(v1: Vector2, v2 : Vector2, amount : Double) : Vector2{ + + fun lerp(v1: Vector2, v2: Vector2, amount: Double): Vector2 { return v1 + ((v2 - v1) * amount) } } + constructor(pose: Pose2d) : this(pose.x, pose.y) - constructor(angle: AngleUnit) : this(angle.cos(), angle.sin()) + constructor(angle: AngleUnit) : this(angle.cos(), angle.sin()) - /** @param angle Angle to rotate the vector by in radians*/ - fun rotateBy(angle: Double) : Vector2{ - return Vector2(cos(angle) *x - sin(angle) * y, - sin(angle)*x + cos(angle)*y) + /** @param angle Angle to rotate the vector by in radians */ + fun rotateBy(angle: Double): Vector2 { + return Vector2(cos(angle) * x - sin(angle) * y, sin(angle) * x + cos(angle) * y) } - /** @param angle Angle to rotate the vector by in radians*/ - fun rotateBy(angle: AngleUnit) : Vector2{ + /** @param angle Angle to rotate the vector by in radians */ + fun rotateBy(angle: AngleUnit): Vector2 { return rotateBy(angle.asRadians) } /** Distance from 0, 0, calculated using pythagorean theorem */ - val magnitude get() = sqrt(x.pow(2) + y.pow(2)) + val magnitude + get() = sqrt(x.pow(2) + y.pow(2)) + /** Angle of the vector */ - val angle get() = AngleUnit(atan2(y,x)) - fun angleTo(other : Vector2) = (this-other).angle - fun distance(other: Vector2): Double = (this-other).magnitude - fun distance(pose: Pose2d): Double = distance(Vector2(pose)) - fun xdistance(pos: Double): Double = x - pos - fun xdistance(pos: Vector2): Double = x - pos.x - fun xdistance(pos: Pose2d): Double = x - pos.x - fun ydistance(pos: Double): Double = y - pos - fun ydistance(pos: Vector2): Double = y - pos.y - fun ydistance(pos: Pose2d): Double = y - pos.y - - operator fun plus(other: Vector2) = Vector2(x + other.x, y + other.y) - operator fun minus(other: Vector2) = Vector2(x - other.x,y - other.y) - operator fun times(other: Double) = Vector2(x * other,y * other) - operator fun div(other: Double) = Vector2(x / other,y / other) - operator fun unaryMinus() = Vector2(-x,-y) - operator fun unaryPlus() = this + val angle + get() = AngleUnit(atan2(y, x)) + + /** The vector in the same direction as this one with a magnitude of 1 */ + val unit + get() = this / this.magnitude + + fun angleTo(other: Vector2) = (this - other).angle + + fun distance(other: Vector2): Double = (this - other).magnitude + + fun distance(pose: Pose2d): Double = distance(Vector2(pose)) + + fun xdistance(pos: Double): Double = x - pos + + fun xdistance(pos: Vector2): Double = x - pos.x + + fun xdistance(pos: Pose2d): Double = x - pos.x + + fun ydistance(pos: Double): Double = y - pos + + fun ydistance(pos: Vector2): Double = y - pos.y + + fun ydistance(pos: Pose2d): Double = y - pos.y + + operator fun plus(other: Vector2) = Vector2(x + other.x, y + other.y) + + operator fun minus(other: Vector2) = Vector2(x - other.x, y - other.y) + + operator fun times(other: Double) = Vector2(x * other, y * other) + + operator fun div(other: Double) = Vector2(x / other, y / other) + + operator fun unaryMinus() = Vector2(-x, -y) + + operator fun unaryPlus() = this override fun toString() = "(x: ${x}, y: ${y})" /** * Reflects the point across a horizontal line - * @return Reflected point + * * @param x The x value of the vertical line to reflect across + * @return Reflected point * @author Ozy King */ - fun reflectHorizontally(x: Double) = Vector2(x + (x - this.x),y) + fun reflectHorizontally(x: Double) = Vector2(x + (x - this.x), y) /** * Creates a new Pose2d from the coordinate object and rotation + * * @param rotation The rotation of the pose, in degrees * @return A new Pose2d contructed from the coordinate and the rotation */ fun toPose2d(rotation: Double) = Pose2d(x, y, Rotation2d.fromDegrees(rotation)) -} \ No newline at end of file +} From 2cc37ef232b20bc9f16f1bc57bc5054341a2c769 Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Wed, 3 Dec 2025 17:46:38 -0800 Subject: [PATCH 10/12] Remove Async Code --- async/EventTarget.kt | 40 ------ async/Promise.kt | 281 ------------------------------------------- async/Thenable.kt | 8 -- 3 files changed, 329 deletions(-) delete mode 100644 async/EventTarget.kt delete mode 100644 async/Promise.kt delete mode 100644 async/Thenable.kt diff --git a/async/EventTarget.kt b/async/EventTarget.kt deleted file mode 100644 index f65c96b..0000000 --- a/async/EventTarget.kt +++ /dev/null @@ -1,40 +0,0 @@ -//File adapted from 2898's code -package frc.beaverlib.async - -import frc.beaverlib.misc.Symbol - -@Deprecated("EventTarget is deprecated, if you need to use it, idk why you would need to") -class EventTarget { - private val listeners: MutableMap Unit> = mutableMapOf() - private val onceListeners: MutableMap Unit> = mutableMapOf() - fun addListener(fn: (ev: DataType) -> Unit): Symbol { - val s = Symbol() - listeners[s] = fn - return s - } - fun addOnceListener(fn: (ev: DataType) -> Unit): Symbol { - val s = Symbol() - onceListeners[s] = fn - return s - } - fun removeListener(identifier: Symbol) { - listeners.remove(identifier) - onceListeners.remove(identifier) - } - fun dispatch(event: DataType) { - listeners.forEach { (_, u) -> - try { u(event) } - catch (e: Throwable) { - println("Uncaught in event listener: $e") - } - } - onceListeners.forEach { (t, u) -> - try { u(event) } - catch (e: Throwable) { - println("Uncaught in event listener: $e") - } - onceListeners.remove(t) - } - } - -} \ No newline at end of file diff --git a/async/Promise.kt b/async/Promise.kt deleted file mode 100644 index 87c2082..0000000 --- a/async/Promise.kt +++ /dev/null @@ -1,281 +0,0 @@ -//File adapted from 2898's code -package frc.beaverlib.async - -import frc.beaverlib.misc.AggregateException -import edu.wpi.first.wpilibj.event.BooleanEvent -import edu.wpi.first.wpilibj.event.EventLoop - -@Deprecated("Promise is deprecated, if you need to use it, idk why you would need to") -@Suppress("DEPRECATION") -class Promise(private val fn: (resolve: (value: T) -> Unit, reject: (error: Throwable) -> Unit) -> Unit) : Thenable { - private var resolvedWith: T? = null - private var rejectedWith: Throwable? = null - var hasFulfilled: Boolean = false - var currentState: PromiseState = PromiseState.NOT_RUN - private var listenerApplied = false - private val onResolved = EventTarget() - private val onRejected = EventTarget() - fun hasResolved(loop: EventLoop) = BooleanEvent(loop) { currentState == PromiseState.RESOLVED } - fun hasRejected(loop: EventLoop) = BooleanEvent(loop) { currentState == PromiseState.REJECTED } - - init { - run() - } - private fun run() { - if (currentState != PromiseState.NOT_RUN) return - currentState = PromiseState.WAITING - fn({d -> resolve(d)}, {e -> reject(e)}) - } - private fun resolve(value: T) { - if (currentState != PromiseState.WAITING) return - currentState = PromiseState.RESOLVED - resolvedWith = value - hasFulfilled = true - onResolved.dispatch(value) - } - private fun reject(error: Throwable) { - if (currentState != PromiseState.WAITING) return - currentState = PromiseState.REJECTED - rejectedWith = error - hasFulfilled = true - onRejected.dispatch(error) - } - override fun then(success: (value: T) -> Promise) - = Promise { rs, rj -> - if (hasFulfilled) { - try { - val x = success(resolvedWith ?: throw rejectedWith ?: Throwable("nothing exception: if found, tell Grant to debug Promise code")) - x.onResolved.addListener { value2 -> rs(value2) } - x.onRejected.addListener(rj) - } catch (e: Throwable) { - rj(e) - } - } else { - onResolved.addListener { value -> - try { - val x = success(value) - x.onResolved.addListener { value2 -> rs(value2) } - x.onRejected.addListener(rj) - } catch (e: Throwable) { - rj(e) - } - } - onRejected.addListener { error -> rj(error) } - } - } - override fun then( - success: (value: T) -> Promise, - failure: (error: Throwable) -> Promise - ): Promise { - return Promise { rs, rj -> - if (hasFulfilled) { - if (resolvedWith != null) { - try { - val x = success(resolvedWith!!) - x.onResolved.addListener { value2 -> rs(value2) } - x.onRejected.addListener(rj) - } catch (e: Throwable) { - rj(e) - } - } else if (rejectedWith != null) { - try { - val x = failure(rejectedWith!!) - x.onResolved.addListener { value2 -> rs(value2) } - x.onRejected.addListener(rj) - } catch (e: Throwable) { - rj(e) - } - } else rj(Throwable("nothing exception: if found, tell Grant to debug Promise code")) - } else { - onResolved.addListener { value -> - try { - val x = success(value) - x.onResolved.addListener { value2 -> rs(value2) } - x.onRejected.addListener(rj) - } catch (e: Throwable) { - rj(e) - } - } - onRejected.addListener { error -> - try { - val x = failure(error) - x.onResolved.addListener { value2 -> rs(value2) } - x.onRejected.addListener(rj) - } catch (e: Throwable) { - rj(e) - } - } - } - } - } - - override fun catch(failure: (error: Throwable) -> Promise): Promise { - return Promise { rs, rj -> - if (hasFulfilled) { - if (resolvedWith != null) { - rs(resolvedWith!!) - } else if (rejectedWith != null) { - try { - val x = failure(rejectedWith!!) - x.onResolved.addListener { value2 -> rs(value2) } - x.onRejected.addListener(rj) - } catch (e: Throwable) { - rj(e) - } - } - } else { - onResolved.addListener { value -> rs(value) } - onRejected.addListener { error -> - try { - val x = failure(error) - x.onResolved.addListener { value2 -> rs(value2) } - x.onRejected.addListener(rj) - } catch (e: Throwable) { - rj(e) - } - } - } - } - } - companion object { - enum class PromiseState { - NOT_RUN, WAITING, RESOLVED, REJECTED - } - enum class PromiseSettledState { - RESOLVED, REJECTED - } - - /** - * Returns a Promise that resolves when any promise passed resolves, and rejects when all promises passed reject. - */ - fun any(iter: Set>): Promise { - val icpy = iter.map { v -> v } - val l = icpy.size - var rjc = 0 - val rejections = mutableListOf() - return Promise { rs, rj -> - if (l == 0) rj(IllegalArgumentException("Iterable passed to Promise.any was empty")) - else icpy.forEach { item -> - item.onResolved.addListener(rs) - item.onRejected.addListener { error -> - rjc++ - rejections.add(error) - if (rjc >= l) rj(AggregateException(null, null, *rejections.toTypedArray())) - } - } - } - } - - /** - * Returns a Promise that resolves when all promises passed resolve, and rejects when any promise rejects. - */ - fun all(iter: Set>): Promise> { - val icpy = iter.map { v -> v } - val l = icpy.size - val resolutions = mutableSetOf() - return Promise { rs, rj -> - if (l == 0) rs(setOf()) - else icpy.forEach { item -> - item.onResolved.addListener { - resolutions.add(it) - if (resolutions.size >= l) rs(resolutions) - } - item.onRejected.addListener(rj) - } - } - } - - /** - * Returns a Promise that adopts the state of the first promise to resolve or reject. - */ - fun race(iter: Set>): Promise { - return Promise { rs, rj -> - iter.forEach { - it.onResolved.addListener(rs) - it.onRejected.addListener(rj) - } - } - } - - /** - * Returns a Promise that resolves when all promises resolve or reject, with an iterable of the eventual states. - */ - fun allSettled(iter: Set>): Promise>> { - val fulfillments = mutableSetOf>() - return Promise { rs, _ -> - if (iter.isEmpty()) rs(setOf()) - else iter.forEach { item -> - item.onResolved.addListener { value -> - fulfillments.add(object : PromiseFulfillment { - override val status = PromiseSettledState.RESOLVED - override val value = value - override val reason = null - }) - if (fulfillments.size >= iter.size) rs(fulfillments) - } - item.onRejected.addListener { error -> - fulfillments.add(object : PromiseFulfillment { - override val status = PromiseSettledState.REJECTED - override val value = null - override val reason = error - }) - if (fulfillments.size >= iter.size) rs(fulfillments) - } - } - } - } - - /** - * Returns a Promise that resolves with the values from all the promises in the iterable that resolved. - * Rejections are ignored. - */ - fun filter(iter: Set>): Promise> { - val fulfillments = mutableSetOf() - return Promise { rs, _ -> - if (iter.isEmpty()) rs(setOf()) - else iter.forEach { - it.onResolved.addListener { value -> - fulfillments.add(value) - if (fulfillments.size >= iter.size) rs(fulfillments) - } - } - } - } - - /** - * Returns a Promise that resolves immediately with the given value. - */ - fun resolve(value: T): Promise { - return Promise { resolve, _ -> resolve(value) } - } - - /** - * Returns a Promise that rejects immediately with the given reason. - */ - fun reject(error: Throwable): Promise { - return Promise { _, reject -> reject(error) } - } - - /** - * Returns an object containing a new Promise and two functions to resolve or reject it. - */ - fun withResolvers(): ResolversObject { - val p = Promise{ _, _ -> } - return object : ResolversObject { - override val promise = p - override val resolve: (value: T) -> Unit = {value -> p.resolve(value)} - override val reject: (error: Throwable) -> Unit = {error -> p.reject(error)} - } - } - interface PromiseFulfillment { - val status: PromiseSettledState - val value: T? - val reason: Throwable? - } - interface ResolversObject { - val promise: Promise - val resolve: (value: T) -> Unit - val reject: (error: Throwable) -> Unit - } - } -} \ No newline at end of file diff --git a/async/Thenable.kt b/async/Thenable.kt deleted file mode 100644 index ffaed72..0000000 --- a/async/Thenable.kt +++ /dev/null @@ -1,8 +0,0 @@ -package frc.beaverlib.async - -@Deprecated("Thenable is deprecated, if you need to use it, idk why you would need to") -interface Thenable { - fun then(success: (value: T) -> Promise): Promise - fun then(success: (value: T) -> Promise, failure: (error: Throwable) -> Promise) : Promise - fun catch(failure: (error: Throwable) -> Promise): Promise -} \ No newline at end of file From f0e3c8dfa12b235f5e675c6e5151eb4f86f6cbc3 Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Fri, 5 Dec 2025 17:13:19 -0800 Subject: [PATCH 11/12] Improved Vector2 compatibility with Pose2d --- utils/Geometry/Vector2.kt | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/utils/Geometry/Vector2.kt b/utils/Geometry/Vector2.kt index 650fa33..6958761 100644 --- a/utils/Geometry/Vector2.kt +++ b/utils/Geometry/Vector2.kt @@ -89,6 +89,10 @@ class Vector2(val x: Double, val y: Double) { operator fun minus(other: Vector2) = Vector2(x - other.x, y - other.y) + operator fun plus(other: Pose2d) = this + other.vector2 + + operator fun minus(other: Pose2d) = this - other.vector2 + operator fun times(other: Double) = Vector2(x * other, y * other) operator fun div(other: Double) = Vector2(x / other, y / other) @@ -116,3 +120,6 @@ class Vector2(val x: Double, val y: Double) { */ fun toPose2d(rotation: Double) = Pose2d(x, y, Rotation2d.fromDegrees(rotation)) } + +val Pose2d.vector2 + get() = Vector2(this) From e8f81573b8a9466f1f054851c79cd39c57dcb61d Mon Sep 17 00:00:00 2001 From: BubbleShade Date: Mon, 8 Dec 2025 00:49:48 -0800 Subject: [PATCH 12/12] Added BeaverSysIDRoutine.kt --- utils/sysID/BeaverSysIDMotor.kt | 41 ++++++++++++++++++++++++ utils/sysID/BeaverSysIDRoutine.kt | 53 +++++++++++++++++++++++++++++++ 2 files changed, 94 insertions(+) create mode 100644 utils/sysID/BeaverSysIDMotor.kt create mode 100644 utils/sysID/BeaverSysIDRoutine.kt diff --git a/utils/sysID/BeaverSysIDMotor.kt b/utils/sysID/BeaverSysIDMotor.kt new file mode 100644 index 0000000..ffd4cd5 --- /dev/null +++ b/utils/sysID/BeaverSysIDMotor.kt @@ -0,0 +1,41 @@ +package frc.robot.beaverlib.utils.sysID + +import com.revrobotics.spark.SparkMax +import edu.wpi.first.units.Units.* +import edu.wpi.first.units.measure.MutAngle +import edu.wpi.first.units.measure.MutAngularVelocity +import edu.wpi.first.units.measure.MutVoltage +import edu.wpi.first.units.measure.Voltage +import edu.wpi.first.wpilibj.RobotController +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog + +class BeaverSysIDMotor(val name: String, val motor: SparkMax) { + private val m_appliedVoltage: MutVoltage = Volts.mutable(0.0) + private val m_angle: MutAngle = Radians.mutable(0.0) + private val m_velocity: MutAngularVelocity = RadiansPerSecond.mutable(0.0) + + fun setVoltage(voltage: Voltage?) { + voltage ?: return + motor.set(voltage.`in`(Volts) / RobotController.getBatteryVoltage()) + } + + fun log(log: SysIdRoutineLog) { + SmartDashboard.putNumber( + "Shooter/$name/Voltage", + motor.get() * RobotController.getBatteryVoltage(), + ) + SmartDashboard.putNumber("Shooter/$name/Position", motor.encoder.position) + SmartDashboard.putNumber("Shooter/$name/Velocity", motor.encoder.velocity) + + log.motor(name) + .voltage( + m_appliedVoltage.mut_replace( + motor.get() * RobotController.getBatteryVoltage(), + Volts, + ) + ) + .angularPosition(m_angle.mut_replace(motor.encoder.position, Rotations)) + .angularVelocity(m_velocity.mut_replace(motor.encoder.velocity, RotationsPerSecond)) + } +} diff --git a/utils/sysID/BeaverSysIDRoutine.kt b/utils/sysID/BeaverSysIDRoutine.kt new file mode 100644 index 0000000..f97ff98 --- /dev/null +++ b/utils/sysID/BeaverSysIDRoutine.kt @@ -0,0 +1,53 @@ +package frc.robot.beaverlib.utils.sysID + +import beaverlib.utils.Units.Time +import beaverlib.utils.Units.seconds +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.SubsystemBase +import edu.wpi.first.wpilibj2.command.WaitCommand +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism + +class BeaverSysIDRoutine(subsystem: SubsystemBase, vararg motors: BeaverSysIDMotor) { + val routine = + SysIdRoutine( + SysIdRoutine.Config(), + Mechanism( + { volts -> motors.forEach { motor -> motor.setVoltage(volts) } }, + { log -> + // Record a frame for the shooter motor. + motors.forEach { motor -> motor.log(log) } + }, + subsystem, + ), + ) + + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + fun sysIdQuasistatic(direction: SysIdRoutine.Direction?): Command { + return routine.quasistatic(direction) + } + + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + fun sysIdDynamic(direction: SysIdRoutine.Direction?): Command { + return routine.dynamic(direction) + } + + fun fullSysID(waitTime: Time = 3.0.seconds): Command { + return routine + .dynamic(SysIdRoutine.Direction.kForward) + .andThen(WaitCommand(waitTime.asSeconds)) + .andThen(routine.dynamic(SysIdRoutine.Direction.kReverse)) + .andThen(WaitCommand(waitTime.asSeconds)) + .andThen(routine.quasistatic(SysIdRoutine.Direction.kForward)) + .andThen(WaitCommand(waitTime.asSeconds)) + .andThen(routine.quasistatic(SysIdRoutine.Direction.kReverse)) + } +}