From 609dade0d35c6865f6e3be8844fbadac4f5eb7ad Mon Sep 17 00:00:00 2001
From: Saraansh Wadkar <saraanshwadkar@gmail.com>
Date: Fri, 19 Jan 2024 19:31:12 -0500
Subject: [PATCH] add wrapper class on velocity and position voltage requests

---
 .../team4099/lib/phoenix6/PositionVoltage.kt  | 61 ++++++++++++++++++
 .../team4099/lib/phoenix6/VelocityVoltage.kt  | 64 +++++++++++++++++++
 2 files changed, 125 insertions(+)
 create mode 100644 src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt
 create mode 100644 src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt

diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt
new file mode 100644
index 00000000..51816cb9
--- /dev/null
+++ b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt
@@ -0,0 +1,61 @@
+package com.team4099.lib.phoenix6
+
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inRotations
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.inRotationsPerSecond
+import org.team4099.lib.units.perSecond
+import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6
+
+class PositionVoltage(
+    var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity
+    var enableFOC: Boolean = true,
+    var feedforward: ElectricalPotential = 0.0.volts,
+    var slot: Int = 0,
+    var overrideBrakeDurNeutral: Boolean = false,
+    var limitForwardMotion: Boolean = false,
+    var limitReverseMotion: Boolean = false,
+    var velocity: AngularVelocity = 0.0.degrees.perSecond,
+) {
+
+    val positionVoltagePhoenix6 = PositionVoltagePhoenix6(position.inRotations, velocity.inRotationsPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion)
+
+    fun setPosition(new_position: Angle) {
+        position = new_position
+        positionVoltagePhoenix6.Position = new_position.inRotations
+    }
+
+    fun setEnableFOC(new_enableFOC: Boolean) {
+        enableFOC = new_enableFOC
+        positionVoltagePhoenix6.EnableFOC = new_enableFOC
+    }
+
+    fun setFeedforward(new_feedforward: ElectricalPotential) {
+        feedforward = new_feedforward
+        positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts
+    }
+
+    fun setSlot(new_slot: Int) {
+        slot = new_slot
+        positionVoltagePhoenix6.Slot = new_slot
+    }
+
+    fun setOverrideBrakeDurNeutral(new_override: Boolean) {
+        overrideBrakeDurNeutral = new_override
+        positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
+    }
+
+    fun setLimitForwardMotion(new_limitForward: Boolean) {
+        limitForwardMotion = new_limitForward
+        positionVoltagePhoenix6.LimitForwardMotion = new_limitForward
+    }
+
+    fun setLimitReverseMotion(new_limitReverse: Boolean) {
+        limitReverseMotion = new_limitReverse
+        positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse
+    }
+}
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt
new file mode 100644
index 00000000..6efd986b
--- /dev/null
+++ b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt
@@ -0,0 +1,64 @@
+package com.team4099.lib.phoenix6
+
+import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6
+import org.team4099.lib.units.AngularAcceleration
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.inRotationsPerSecond
+import org.team4099.lib.units.inRotationsPerSecondPerSecond
+import org.team4099.lib.units.perSecond
+
+class VelocityVoltage(var velocity: AngularVelocity,
+                           var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond,
+                           var enableFOC:Boolean = true,
+                           var feedforward: ElectricalPotential = 0.0.volts,
+                           var slot:Int = 0,
+                           var overrideBrakeDurNeutral: Boolean = false,
+                           var limitForwardMotion: Boolean = false,
+                           var limitReverseMotion: Boolean = false){
+
+    val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion)
+
+    fun setVelocity(new_velocity: AngularVelocity) {
+        velocity = new_velocity
+        velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond
+    }
+
+    fun setAcceleration(new_accel: AngularAcceleration) {
+        acceleration = new_accel
+        velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond
+    }
+
+    fun setEnableFOC(new_enableFOC: Boolean) {
+        enableFOC = new_enableFOC
+        velocityVoltagePhoenix6.EnableFOC = new_enableFOC
+    }
+
+    fun setFeedforward(new_feedforward: ElectricalPotential) {
+        feedforward = new_feedforward
+        velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts
+    }
+
+    fun setSlot(new_slot: Int) {
+        slot = new_slot
+        velocityVoltagePhoenix6.Slot = new_slot
+    }
+
+    fun setOverrideBrakeDurNeutral(new_override: Boolean) {
+        overrideBrakeDurNeutral = new_override
+        velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
+    }
+
+    fun setLimitForwardMotion(new_limitForward: Boolean) {
+        limitForwardMotion = new_limitForward
+        velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward
+    }
+
+    fun setLimitReverseMotion(new_limitReverse: Boolean) {
+        limitReverseMotion = new_limitReverse
+        velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse
+    }
+}
\ No newline at end of file