Merge branch 'feature/canifier-item'

Author: jhh

build.gradle

@@ -12,7 +12,7 @@ buildscript {
 
 subprojects {
     group = 'org.strykeforce.thirdcoast'
-    version = '18.7.2'
+    version = '18.7.3'
 
     apply plugin: 'java-library'
     apply plugin: 'idea'
@@ -23,6 +23,10 @@ subprojects {
         jcenter()
     }
 
+    wpi {
+        ctreVersion = "5.7.1.0"
+    }
+
     dependencies {
         implementation wpilib()
         implementation ctre()

telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/grapher/Measure.kt

@@ -32,8 +32,8 @@ enum class Measure constructor(val description: String) {
     QUAD_IDX_PIN("Quadrature Index State"),
     PULSE_WIDTH_POSITION("Pulse Width Position"),
     PULSE_WIDTH_VELOCITY("Pulse Width Velocity"),
-    PULSE_WIDTH_RISE_TO_FALL("Pulse Width Rise-Fall"),
-    PULSE_WIDTH_RISE_TO_RISE("Pulse Width Rise-Rise"),
+    PULSE_WIDTH_RISE_TO_FALL("PWM Pulse Width"),
+    PULSE_WIDTH_RISE_TO_RISE("PWM Period"),
     FORWARD_LIMIT_SWITCH_CLOSED("Forward Limit Switch Closed"),
     REVERSE_LIMIT_SWITCH_CLOSED("Reverse Limit Switch Closed"),
     //  FORWARD_SOFT_LIMIT("Forward Soft Limit"),
@@ -61,5 +61,19 @@ enum class Measure constructor(val description: String) {
     PATH_SEG_JERK("Trajectory Jerk"),
     PATH_SEG_HEADING("Trajectory Heading"),
     PATH_DISTANCE("Distance Travelled"),
-    PATH_POSITION_ERROR("Path Position Error")
+    PATH_POSITION_ERROR("Path Position Error"),
+
+    // Canifier
+    PWM0_PULSE_WIDTH("PWM 0 Pulse Width"),
+    PWM0_PERIOD("PWM 0 Period"),
+    PWM0_PULSE_WIDTH_POSITION("PWM 0 Pulse Width Position"),
+    PWM1_PULSE_WIDTH("PWM 1 Pulse Width"),
+    PWM1_PERIOD("PWM 1 Period"),
+    PWM1_PULSE_WIDTH_POSITION("PWM 1 Pulse Width Position"),
+    PWM2_PULSE_WIDTH("PWM 2 Pulse Width"),
+    PWM2_PERIOD("PWM 2 Period"),
+    PWM2_PULSE_WIDTH_POSITION("PWM 2 Pulse Width Position"),
+    PWM3_PULSE_WIDTH("PWM 3 Pulse Width"),
+    PWM3_PERIOD("PWM 3 Period"),
+    PWM3_PULSE_WIDTH_POSITION("PWM 3 Pulse Width Position"),
 }

telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/CanifierItem.kt

@@ -0,0 +1,90 @@
+package org.strykeforce.thirdcoast.telemetry.item
+
+import com.ctre.phoenix.CANifier
+import com.ctre.phoenix.CANifier.PWMChannel.*
+import com.ctre.phoenix.motorcontrol.can.TalonSRX
+import org.strykeforce.thirdcoast.telemetry.grapher.Measure
+import org.strykeforce.thirdcoast.telemetry.grapher.Measure.*
+import java.util.function.DoubleSupplier
+
+/** Represents a [TalonSRX] telemetry-enable Item.  */
+class CanifierItem @JvmOverloads constructor(
+    private val canifier: CANifier,
+    override val description: String = "CANifier ${canifier.deviceID}"
+) : Item {
+
+    override val deviceId = canifier.deviceID
+    override val type = "canifier"
+    override val measures
+        get() = MEASURES
+
+    override fun measurementFor(measure: Measure): DoubleSupplier {
+
+        return when (measure) {
+            UNKNOWN -> DoubleSupplier { 2767.0 }
+            PWM0_PULSE_WIDTH -> DoubleSupplier { pulseWidthFor(PWMChannel0) }
+            PWM0_PERIOD -> DoubleSupplier { periodFor(PWMChannel0) }
+            PWM0_PULSE_WIDTH_POSITION -> DoubleSupplier { pulseWidthPositionFor(PWMChannel0) }
+            PWM1_PULSE_WIDTH -> DoubleSupplier { pulseWidthFor(PWMChannel1) }
+            PWM1_PERIOD -> DoubleSupplier { periodFor(PWMChannel1) }
+            PWM1_PULSE_WIDTH_POSITION -> DoubleSupplier { pulseWidthPositionFor(PWMChannel1) }
+            PWM2_PULSE_WIDTH -> DoubleSupplier { pulseWidthFor(PWMChannel2) }
+            PWM2_PERIOD -> DoubleSupplier { periodFor(PWMChannel2) }
+            PWM2_PULSE_WIDTH_POSITION -> DoubleSupplier { pulseWidthPositionFor(PWMChannel2) }
+            PWM3_PULSE_WIDTH -> DoubleSupplier { pulseWidthFor(PWMChannel3) }
+            PWM3_PERIOD -> DoubleSupplier { periodFor(PWMChannel3) }
+            PWM3_PULSE_WIDTH_POSITION -> DoubleSupplier { pulseWidthPositionFor(PWMChannel3) }
+            QUAD_POSITION -> DoubleSupplier { canifier.quadraturePosition.toDouble() }
+            QUAD_VELOCITY -> DoubleSupplier { canifier.quadratureVelocity.toDouble() }
+
+            else -> TODO("$measure not implemented")
+        }
+    }
+
+    private fun pulseWidthFor(channel: CANifier.PWMChannel): Double {
+        val pulseWidthAndPeriod = DoubleArray(2)
+        canifier.getPWMInput(channel, pulseWidthAndPeriod)
+        return pulseWidthAndPeriod[0]
+    }
+
+    private fun periodFor(channel: CANifier.PWMChannel): Double {
+        val pulseWidthAndPeriod = DoubleArray(2)
+        canifier.getPWMInput(channel, pulseWidthAndPeriod)
+        return pulseWidthAndPeriod[1]
+    }
+
+    private fun pulseWidthPositionFor(channel: CANifier.PWMChannel): Double {
+        val pulseWidthAndPeriod = DoubleArray(2)
+        canifier.getPWMInput(channel, pulseWidthAndPeriod)
+        return 4096.0 * pulseWidthAndPeriod[0] / pulseWidthAndPeriod[1]
+    }
+
+    override fun equals(other: Any?): Boolean {
+        if (this === other) return true
+        if (javaClass != other?.javaClass) return false
+
+        other as CanifierItem
+
+        if (deviceId != other.deviceId) return false
+
+        return true
+    }
+
+    override fun hashCode() = deviceId
+
+    companion object {
+        val MEASURES = setOf(
+            PWM0_PULSE_WIDTH_POSITION,
+            QUAD_POSITION,
+            QUAD_VELOCITY,
+            PWM0_PULSE_WIDTH,
+            PWM0_PERIOD,
+            PWM1_PULSE_WIDTH,
+            PWM1_PERIOD,
+            PWM2_PULSE_WIDTH,
+            PWM2_PERIOD,
+            PWM3_PULSE_WIDTH,
+            PWM3_PERIOD
+        )
+    }
+}