diff --git a/async/EventTarget.kt b/async/EventTarget.kt deleted file mode 100644 index d6db93e..0000000 --- a/async/EventTarget.kt +++ /dev/null @@ -1,39 +0,0 @@ -//File adapted from 2898's code -package frc.beaverlib.async - -import frc.beaverlib.misc.Symbol - -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 2e73a47..0000000 --- a/async/Promise.kt +++ /dev/null @@ -1,279 +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 - -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 82a8787..0000000 --- a/async/Thenable.kt +++ /dev/null @@ -1,7 +0,0 @@ -package frc.beaverlib.async - -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 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 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() 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 diff --git a/utils/Geometry/Vector2.kt b/utils/Geometry/Vector2.kt index bf071b0..6958761 100644 --- a/utils/Geometry/Vector2.kt +++ b/utils/Geometry/Vector2.kt @@ -1,75 +1,125 @@ 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.pow -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 { + 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) + } + /** @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() - 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 + /** Distance from 0, 0, calculated using pythagorean theorem */ + val magnitude + get() = sqrt(x.pow(2) + y.pow(2)) + + /** Angle of the vector */ + 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 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) + + 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 +} + +val Pose2d.vector2 + get() = Vector2(this) diff --git a/utils/Units/Angular/AngleUnit.kt b/utils/Units/Angular/AngleUnit.kt index 9156803..467d402 100644 --- a/utils/Units/Angular/AngleUnit.kt +++ b/utils/Units/Angular/AngleUnit.kt @@ -4,12 +4,17 @@ 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 +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) @@ -19,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 @@ -31,11 +37,17 @@ value class AngleUnit(val asRadians: Double) { fun sec() = AngleUnit.sec(this) fun cot() = AngleUnit.tan(this) + 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 } + } - - // Unit conversion math - operator fun div(other : Time) = AngularVelocity(asRadians/other.asSeconds) - operator fun times(other : DistanceUnit) = DistanceUnit(asRadians/other.asMeters) + fun angleDistanceWithin(maxError : AngleUnit, target : AngleUnit): Boolean { + return this.angleDistanceTo(target).asRadians.absoluteValue < maxError.asRadians + } /** * Taken from mean machines mean lib @@ -86,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) @@ -100,4 +117,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))) } diff --git a/utils/Units/Angular/AngularAcceleration.kt b/utils/Units/Angular/AngularAcceleration.kt index c2f589b..0665811 100644 --- a/utils/Units/Angular/AngularAcceleration.kt +++ b/utils/Units/Angular/AngularAcceleration.kt @@ -1,11 +1,14 @@ 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 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) @@ -15,10 +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) - // 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) + 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()) @@ -27,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 7c4d861..a234c79 100644 --- a/utils/Units/Angular/AngularVelocity.kt +++ b/utils/Units/Angular/AngularVelocity.kt @@ -1,11 +1,14 @@ 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 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) @@ -15,11 +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) - // 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) + + 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()) @@ -28,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 dac61e5..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 @@ -12,8 +14,11 @@ value class Current (val asAmps: Double) { 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) + // 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 @@ -29,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 new file mode 100644 index 0000000..fff9907 --- /dev/null +++ b/utils/Units/Electrical/Energy.kt @@ -0,0 +1,42 @@ +package frc.robot.beaverlib.utils.Units.Electrical + +import beaverlib.utils.Units.Frequency +import beaverlib.utils.Units.Time + + +@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" + + // 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 +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 + diff --git a/utils/Units/Electrical/Power.kt b/utils/Units/Electrical/Power.kt new file mode 100644 index 0000000..9167d30 --- /dev/null +++ b/utils/Units/Electrical/Power.kt @@ -0,0 +1,41 @@ +package frc.robot.beaverlib.utils.Units.Electrical + +import beaverlib.utils.Units.Frequency +import beaverlib.utils.Units.Time + + +@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: 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 +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 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 406b3f8..83e6e5b 100644 --- a/utils/Units/Electrical/Resistance.kt +++ b/utils/Units/Electrical/Resistance.kt @@ -11,8 +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 conversion math - operator fun times(other: Current) = VoltageUnit(asOhms*other.asAmps) + + // Unit Math + operator fun times(other : Current) = VoltageUnit(asOhms * other.asAmps) + operator fun div(other: Resistance) = asOhms / other.asOhms } //Constructors @@ -22,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 39afbae..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 @@ -12,9 +14,11 @@ value class VoltageUnit(val asVolts: Double) { 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) + // 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 } @@ -31,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 5343db9..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) { @@ -9,11 +11,20 @@ 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) + + + // 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 ca16a0d..0ea95a3 100644 --- a/utils/Units/Linear/Acceleration.kt +++ b/utils/Units/Linear/Acceleration.kt @@ -1,18 +1,31 @@ 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 @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) operator fun compareTo(other: Acceleration) = asMetersPerSecondSquared.compareTo(other.asMetersPerSecondSquared) - override fun toString() = "$asMetersPerSecondSquared m/s" + 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 @@ -24,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 f531a49..448058a 100644 --- a/utils/Units/Linear/DistanceUnit.kt +++ b/utils/Units/Linear/DistanceUnit.kt @@ -1,5 +1,9 @@ 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 @@ -10,7 +14,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) @@ -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 944a411..b548767 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,21 @@ 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" + + 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()) @@ -24,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 914a9ea..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){ @@ -14,8 +21,15 @@ 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/Weight.kt b/utils/Units/Weight.kt index 26a6561..c65407b 100644 --- a/utils/Units/Weight.kt +++ b/utils/Units/Weight.kt @@ -7,15 +7,15 @@ 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) + } -operator fun Acceleration.times(other: Weight) = Force(this.asMetersPerSecondSquared * other.asKilograms) //Constructors inline val Number.kg get() = Weight(toDouble()) 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)) + } +}