Changed base unit for all providers to radians, and fixed spark max f…

…ollow invert behavior
team401 · Feb 25, 2020 · 7adca77 · 7adca77
1 parent 30cd547
commit 7adca77
Show file tree

Hide file tree

Showing 26 changed files with 186 additions and 164 deletions.
diff --git a/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/HardwareCanCoderDevice.kt b/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/HardwareCanCoderDevice.kt
@@ -2,7 +2,11 @@ package org.snakeskin.component.impl
 
 import com.ctre.phoenix.sensors.CANCoder
 import org.snakeskin.component.ICanCoderDevice
+import org.snakeskin.measure.distance.angular.AngularDistanceMeasureDegrees
+import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRadians
 import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRevolutions
+import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureDegreesPerSecond
+import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRadiansPerSecond
 import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRevolutionsPerSecond
 
 /**
@@ -11,16 +15,16 @@ import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRevolutionsP
  * returned by the functions of this class.
  */
 class HardwareCanCoderDevice(val device: CANCoder): ICanCoderDevice {
-    override fun getAngularPosition(): AngularDistanceMeasureRevolutions {
-        return AngularDistanceMeasureRevolutions(device.position / 360.0)
+    override fun getAngularPosition(): AngularDistanceMeasureRadians {
+        return AngularDistanceMeasureDegrees(device.position).toRadians()
     }
 
-    override fun setAngularPosition(angle: AngularDistanceMeasureRevolutions) {
+    override fun setAngularPosition(angle: AngularDistanceMeasureRadians) {
         device.position = angle.toDegrees().value
     }
 
-    override fun getAngularVelocity(): AngularVelocityMeasureRevolutionsPerSecond {
-        return AngularVelocityMeasureRevolutionsPerSecond(device.velocity / 360.0)
+    override fun getAngularVelocity(): AngularVelocityMeasureRadiansPerSecond {
+        return AngularVelocityMeasureDegreesPerSecond(device.velocity).toRadiansPerSecond()
     }
 
     override fun invertInput(invert: Boolean) {

diff --git a/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/HardwareTalonFxDevice.kt b/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/HardwareTalonFxDevice.kt
@@ -1,19 +1,17 @@
 package org.snakeskin.component.impl
 
-import com.ctre.phoenix.motorcontrol.ControlMode
 import com.ctre.phoenix.motorcontrol.DemandType
-import com.ctre.phoenix.motorcontrol.InvertType
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode
 import com.ctre.phoenix.motorcontrol.can.TalonFX
 import org.snakeskin.component.CTREFeedforwardScalingMode
 import org.snakeskin.component.ITalonFxDevice
 import org.snakeskin.component.provider.IFollowableProvider
-import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRevolutions
-import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRevolutionsPerSecond
+import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRadians
+import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRadiansPerSecond
 import org.snakeskin.runtime.SnakeskinRuntime
 
 class HardwareTalonFxDevice(val device: TalonFX, val ffMode: CTREFeedforwardScalingMode = CTREFeedforwardScalingMode.ScaleVbusSystem): ITalonFxDevice {
-    private val sensorTicksPerRevolution = 2048.0 //Constant for Falcon 500 integrated encoder
+    private val sensorTicksPerRadian = 2048.0 / (2.0 * Math.PI) //Constant for Falcon 500 integrated encoder
 
     private fun scaleFfVolts(voltage: Double): Double {
         return when (ffMode) {
@@ -60,44 +58,44 @@ class HardwareTalonFxDevice(val device: TalonFX, val ffMode: CTREFeedforwardScal
         return device.motorOutputVoltage
     }
 
-    override fun getAngularPosition(): AngularDistanceMeasureRevolutions {
+    override fun getAngularPosition(): AngularDistanceMeasureRadians {
         val ticks = device.selectedSensorPosition.toDouble()
-        return AngularDistanceMeasureRevolutions(ticks / sensorTicksPerRevolution)
+        return AngularDistanceMeasureRadians(ticks / sensorTicksPerRadian)
     }
 
-    override fun setAngularPosition(angle: AngularDistanceMeasureRevolutions) {
-        val ticks = (angle.value * sensorTicksPerRevolution).toInt()
+    override fun setAngularPosition(angle: AngularDistanceMeasureRadians) {
+        val ticks = (angle.value * sensorTicksPerRadian).toInt()
         device.selectedSensorPosition = ticks
     }
 
-    override fun getAngularVelocity(): AngularVelocityMeasureRevolutionsPerSecond {
-        val ticks = device.selectedSensorVelocity.toDouble()
-        return AngularVelocityMeasureRevolutionsPerSecond((ticks / sensorTicksPerRevolution) * 10.0) //Multiply by 10 to convert deciseconds (100 ms) to seconds
+    override fun getAngularVelocity(): AngularVelocityMeasureRadiansPerSecond {
+        val ticksPer100Ms = device.selectedSensorVelocity.toDouble()
+        return AngularVelocityMeasureRadiansPerSecond((ticksPer100Ms / sensorTicksPerRadian) * 10.0) //Multiply by 10 to convert deciseconds (100 ms) to seconds
     }
 
     override fun getOutputCurrent(): Double {
         return device.statorCurrent
     }
 
-    override fun setAngularPositionSetpoint(setpoint: AngularDistanceMeasureRevolutions, ffVolts: Double) {
-        val ticks = setpoint.value * sensorTicksPerRevolution
+    override fun setAngularPositionSetpoint(setpoint: AngularDistanceMeasureRadians, ffVolts: Double) {
+        val ticks = setpoint.value * sensorTicksPerRadian
         val ffPercent = scaleFfVolts(ffVolts)
         device.set(TalonFXControlMode.Position, ticks, DemandType.ArbitraryFeedForward, ffPercent)
     }
 
-    override fun setAngularVelocitySetpoint(setpoint: AngularVelocityMeasureRevolutionsPerSecond, ffVolts: Double) {
-        val ticksPer100Ms = (setpoint.value * sensorTicksPerRevolution) / 10.0 //Divide by 10 to convert seconds to deciseconds (100 ms)
+    override fun setAngularVelocitySetpoint(setpoint: AngularVelocityMeasureRadiansPerSecond, ffVolts: Double) {
+        val ticksPer100Ms = (setpoint.value * sensorTicksPerRadian) / 10.0 //Divide by 10 to convert seconds to deciseconds (100 ms)
         val ffPercent = scaleFfVolts(ffVolts)
         device.set(TalonFXControlMode.Velocity, ticksPer100Ms, DemandType.ArbitraryFeedForward, ffPercent)
     }
 
-    override fun setProfiledSetpoint(setpoint: AngularDistanceMeasureRevolutions, ffVolts: Double) {
-        val ticks = setpoint.value * sensorTicksPerRevolution
+    override fun setProfiledSetpoint(setpoint: AngularDistanceMeasureRadians, ffVolts: Double) {
+        val ticks = setpoint.value * sensorTicksPerRadian
         val ffPercent = scaleFfVolts(ffVolts)
         device.set(TalonFXControlMode.MotionMagic, ticks, DemandType.ArbitraryFeedForward, ffPercent)
     }
 
-    override fun invertOutput(invert: Boolean) {
+    override fun invert(invert: Boolean) {
         device.inverted = invert
     }
 

diff --git a/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/HardwareTalonSrxDevice.kt b/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/HardwareTalonSrxDevice.kt
@@ -6,11 +6,13 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRX
 import org.snakeskin.component.CTREFeedforwardScalingMode
 import org.snakeskin.component.ITalonSrxDevice
 import org.snakeskin.component.provider.IFollowableProvider
-import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRevolutions
-import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRevolutionsPerSecond
+import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRadians
+import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRadiansPerSecond
 import org.snakeskin.runtime.SnakeskinRuntime
 
-class HardwareTalonSrxDevice(val device: TalonSRX, val sensorTicksPerRevolution: Double = 4096.0, val ffMode: CTREFeedforwardScalingMode = CTREFeedforwardScalingMode.ScaleVbusSystem) : ITalonSrxDevice {
+class HardwareTalonSrxDevice(val device: TalonSRX, sensorTicksPerRevolution: Double = 4096.0, val ffMode: CTREFeedforwardScalingMode = CTREFeedforwardScalingMode.ScaleVbusSystem) : ITalonSrxDevice {
+    private val sensorTicksPerRadian = sensorTicksPerRevolution / (2.0 * Math.PI)
+
     private fun scaleFfVolts(voltage: Double): Double {
         return when (ffMode) {
             CTREFeedforwardScalingMode.Scale12V -> voltage / 12.0
@@ -57,44 +59,44 @@ class HardwareTalonSrxDevice(val device: TalonSRX, val sensorTicksPerRevolution:
         setPercentOutput(0.0)
     }
 
-    override fun getAngularPosition(): AngularDistanceMeasureRevolutions {
+    override fun getAngularPosition(): AngularDistanceMeasureRadians {
         val ticks = device.selectedSensorPosition.toDouble()
-        return AngularDistanceMeasureRevolutions(ticks / sensorTicksPerRevolution)
+        return AngularDistanceMeasureRadians(ticks / sensorTicksPerRadian)
     }
 
-    override fun setAngularPosition(angle: AngularDistanceMeasureRevolutions) {
-        val ticks = (angle.value * sensorTicksPerRevolution).toInt()
+    override fun setAngularPosition(angle: AngularDistanceMeasureRadians) {
+        val ticks = (angle.value * sensorTicksPerRadian).toInt()
         device.selectedSensorPosition = ticks
     }
 
-    override fun getAngularVelocity(): AngularVelocityMeasureRevolutionsPerSecond {
+    override fun getAngularVelocity(): AngularVelocityMeasureRadiansPerSecond {
         val ticks = device.selectedSensorVelocity.toDouble()
-        return AngularVelocityMeasureRevolutionsPerSecond((ticks / sensorTicksPerRevolution) * 10.0) //Multiply by 10 to convert deciseconds (100 ms) to seconds
+        return AngularVelocityMeasureRadiansPerSecond((ticks / sensorTicksPerRadian) * 10.0) //Multiply by 10 to convert deciseconds (100 ms) to seconds
     }
 
     override fun getOutputCurrent(): Double {
         return device.statorCurrent
     }
 
-    override fun setAngularPositionSetpoint(setpoint: AngularDistanceMeasureRevolutions, ffVolts: Double) {
-        val ticks = setpoint.value * sensorTicksPerRevolution
+    override fun setAngularPositionSetpoint(setpoint: AngularDistanceMeasureRadians, ffVolts: Double) {
+        val ticks = setpoint.value * sensorTicksPerRadian
         val ffPercent = scaleFfVolts(ffVolts)
         device.set(ControlMode.Position, ticks, DemandType.ArbitraryFeedForward, ffPercent)
     }
 
-    override fun setAngularVelocitySetpoint(setpoint: AngularVelocityMeasureRevolutionsPerSecond, ffVolts: Double) {
-        val ticksPer100Ms = (setpoint.value * sensorTicksPerRevolution) / 10.0 //Divide by 10 to convert seconds to deciseconds (100 ms)
+    override fun setAngularVelocitySetpoint(setpoint: AngularVelocityMeasureRadiansPerSecond, ffVolts: Double) {
+        val ticksPer100Ms = (setpoint.value * sensorTicksPerRadian) / 10.0 //Divide by 10 to convert seconds to deciseconds (100 ms)
         val ffPercent = scaleFfVolts(ffVolts)
         device.set(ControlMode.Velocity, ticksPer100Ms, DemandType.ArbitraryFeedForward, ffPercent)
     }
 
-    override fun setProfiledSetpoint(setpoint: AngularDistanceMeasureRevolutions, ffVolts: Double) {
-        val ticks = setpoint.value * sensorTicksPerRevolution
+    override fun setProfiledSetpoint(setpoint: AngularDistanceMeasureRadians, ffVolts: Double) {
+        val ticks = setpoint.value * sensorTicksPerRadian
         val ffPercent = scaleFfVolts(ffVolts)
         device.set(ControlMode.MotionMagic, ticks, DemandType.ArbitraryFeedForward, ffPercent)
     }
 
-    override fun invertOutput(invert: Boolean) {
+    override fun invert(invert: Boolean) {
         device.inverted = invert
     }
 

diff --git a/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/HardwareVictorSpxDevice.kt b/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/HardwareVictorSpxDevice.kt
@@ -38,7 +38,7 @@ class HardwareVictorSpxDevice(val device: VictorSPX): IVictorSpxDevice {
         setPercentOutput(0.0)
     }
 
-    override fun invertOutput(invert: Boolean) {
+    override fun invert(invert: Boolean) {
         device.inverted = invert
     }
 }
diff --git a/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/NullCanCoderDevice.kt b/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/NullCanCoderDevice.kt
@@ -1,8 +1,8 @@
 package org.snakeskin.component.impl
 
 import org.snakeskin.component.ICanCoderDevice
-import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRevolutions
-import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRevolutionsPerSecond
+import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRadians
+import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRadiansPerSecond
 
 /**
  * No-op implementation of the CANCoder
@@ -13,16 +13,16 @@ open class NullCanCoderDevice private constructor() : ICanCoderDevice {
         val producer = { INSTANCE }
     }
 
-    override fun getAngularPosition(): AngularDistanceMeasureRevolutions {
-        return AngularDistanceMeasureRevolutions(0.0)
+    override fun getAngularPosition(): AngularDistanceMeasureRadians {
+        return AngularDistanceMeasureRadians(0.0)
     }
 
-    override fun setAngularPosition(angle: AngularDistanceMeasureRevolutions) {
+    override fun setAngularPosition(angle: AngularDistanceMeasureRadians) {
         //no-op
     }
 
-    override fun getAngularVelocity(): AngularVelocityMeasureRevolutionsPerSecond {
-        return AngularVelocityMeasureRevolutionsPerSecond(0.0)
+    override fun getAngularVelocity(): AngularVelocityMeasureRadiansPerSecond {
+        return AngularVelocityMeasureRadiansPerSecond(0.0)
     }
 
     override fun invertInput(invert: Boolean) {

diff --git a/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/NullTalonFxDevice.kt b/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/NullTalonFxDevice.kt
@@ -2,8 +2,8 @@ package org.snakeskin.component.impl
 
 import org.snakeskin.component.ITalonFxDevice
 import org.snakeskin.component.provider.IFollowableProvider
-import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRevolutions
-import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRevolutionsPerSecond
+import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRadians
+import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRadiansPerSecond
 
 /**
  * No-op implementation of the Talon FX
@@ -42,39 +42,39 @@ open class NullTalonFxDevice private constructor(): ITalonFxDevice {
         return 0.0
     }
 
-    override fun getAngularPosition(): AngularDistanceMeasureRevolutions {
-        return AngularDistanceMeasureRevolutions(0.0)
+    override fun getAngularPosition(): AngularDistanceMeasureRadians {
+        return AngularDistanceMeasureRadians(0.0)
     }
 
-    override fun setAngularPosition(angle: AngularDistanceMeasureRevolutions) {
+    override fun setAngularPosition(angle: AngularDistanceMeasureRadians) {
         //no-op
     }
 
-    override fun getAngularVelocity(): AngularVelocityMeasureRevolutionsPerSecond {
-        return AngularVelocityMeasureRevolutionsPerSecond(0.0)
+    override fun getAngularVelocity(): AngularVelocityMeasureRadiansPerSecond {
+        return AngularVelocityMeasureRadiansPerSecond(0.0)
     }
 
     override fun getOutputCurrent(): Double {
         return 0.0
     }
 
-    override fun setAngularPositionSetpoint(setpoint: AngularDistanceMeasureRevolutions, ffVolts: Double) {
+    override fun setAngularPositionSetpoint(setpoint: AngularDistanceMeasureRadians, ffVolts: Double) {
         //no-op
     }
 
-    override fun setAngularVelocitySetpoint(setpoint: AngularVelocityMeasureRevolutionsPerSecond, ffVolts: Double) {
+    override fun setAngularVelocitySetpoint(setpoint: AngularVelocityMeasureRadiansPerSecond, ffVolts: Double) {
         //no-op
     }
 
-    override fun setProfiledSetpoint(setpoint: AngularDistanceMeasureRevolutions, ffVolts: Double) {
+    override fun setProfiledSetpoint(setpoint: AngularDistanceMeasureRadians, ffVolts: Double) {
         //no-op
     }
 
     override fun invertInput(invert: Boolean) {
         //no-op
     }
 
-    override fun invertOutput(invert: Boolean) {
+    override fun invert(invert: Boolean) {
         //no-op
     }
 }
diff --git a/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/NullTalonSrxDevice.kt b/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/NullTalonSrxDevice.kt
@@ -2,8 +2,8 @@ package org.snakeskin.component.impl
 
 import org.snakeskin.component.ITalonSrxDevice
 import org.snakeskin.component.provider.IFollowableProvider
-import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRevolutions
-import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRevolutionsPerSecond
+import org.snakeskin.measure.distance.angular.AngularDistanceMeasureRadians
+import org.snakeskin.measure.velocity.angular.AngularVelocityMeasureRadiansPerSecond
 
 /**
  * No-op implementation of the Talon SRX
@@ -42,39 +42,39 @@ open class NullTalonSrxDevice private constructor(): ITalonSrxDevice {
         return 0.0
     }
 
-    override fun getAngularPosition(): AngularDistanceMeasureRevolutions {
-        return AngularDistanceMeasureRevolutions(0.0)
+    override fun getAngularPosition(): AngularDistanceMeasureRadians {
+        return AngularDistanceMeasureRadians(0.0)
     }
 
-    override fun setAngularPosition(angle: AngularDistanceMeasureRevolutions) {
+    override fun setAngularPosition(angle: AngularDistanceMeasureRadians) {
         //no-op
     }
 
-    override fun getAngularVelocity(): AngularVelocityMeasureRevolutionsPerSecond {
-        return AngularVelocityMeasureRevolutionsPerSecond(0.0)
+    override fun getAngularVelocity(): AngularVelocityMeasureRadiansPerSecond {
+        return AngularVelocityMeasureRadiansPerSecond(0.0)
     }
 
     override fun getOutputCurrent(): Double {
         return 0.0
     }
 
-    override fun setAngularPositionSetpoint(setpoint: AngularDistanceMeasureRevolutions, ffVolts: Double) {
+    override fun setAngularPositionSetpoint(setpoint: AngularDistanceMeasureRadians, ffVolts: Double) {
         //no-op
     }
 
-    override fun setAngularVelocitySetpoint(setpoint: AngularVelocityMeasureRevolutionsPerSecond, ffVolts: Double) {
+    override fun setAngularVelocitySetpoint(setpoint: AngularVelocityMeasureRadiansPerSecond, ffVolts: Double) {
         //no-op
     }
 
-    override fun setProfiledSetpoint(setpoint: AngularDistanceMeasureRevolutions, ffVolts: Double) {
+    override fun setProfiledSetpoint(setpoint: AngularDistanceMeasureRadians, ffVolts: Double) {
         //no-op
     }
 
     override fun invertInput(invert: Boolean) {
         //no-op
     }
 
-    override fun invertOutput(invert: Boolean) {
+    override fun invert(invert: Boolean) {
         //no-op
     }
 }
diff --git a/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/NullVictorSpxDevice.kt b/SnakeSkin-CTRE/src/main/kotlin/org/snakeskin/component/impl/NullVictorSpxDevice.kt
@@ -40,7 +40,7 @@ open class NullVictorSpxDevice private constructor(): IVictorSpxDevice {
         return 0.0
     }
 
-    override fun invertOutput(invert: Boolean) {
+    override fun invert(invert: Boolean) {
         //no-op
     }
 }