From 3df8fb60543b371b37fb5ea7bf5d1fccb1975883 Mon Sep 17 00:00:00 2001 From: Jeff Hutchison Date: Fri, 20 Sep 2019 15:24:09 -0400 Subject: [PATCH 1/2] Bump deps --- build.gradle | 6 +++--- telemetry/build.gradle | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/build.gradle b/build.gradle index 0dcb3e17..d5021eda 100644 --- a/build.gradle +++ b/build.gradle @@ -5,14 +5,14 @@ buildscript { } dependencies { classpath 'com.jfrog.bintray.gradle:gradle-bintray-plugin:1.+' - classpath "edu.wpi.first:GradleRIO:2019.1.1" - classpath "com.diffplug.spotless:spotless-plugin-gradle:3.15.0" + classpath "edu.wpi.first:GradleRIO:2019.4.1" + classpath "com.diffplug.spotless:spotless-plugin-gradle:3.24.2" } } configure(subprojects) { group = 'org.strykeforce.thirdcoast' - version = '19.3.0' + version = '19.4.0' apply plugin: 'java-library' apply plugin: 'idea' diff --git a/telemetry/build.gradle b/telemetry/build.gradle index 7288efd5..a02eddfb 100644 --- a/telemetry/build.gradle +++ b/telemetry/build.gradle @@ -1,6 +1,6 @@ buildscript { ext { - kotlin_version = '1.3.20' + kotlin_version = '1.3.50' junit_version = '5.2.0' } @@ -10,7 +10,7 @@ buildscript { } dependencies { classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlin_version" - classpath "org.jetbrains.dokka:dokka-gradle-plugin:0.9.17" + classpath "org.jetbrains.dokka:dokka-gradle-plugin:0.9.18" } } @@ -29,14 +29,14 @@ dependencies { compile "org.jetbrains.kotlin:kotlin-stdlib-jdk8:$kotlin_version" // telemetry - implementation "org.eclipse.jetty:jetty-server:9.4.12.v20180830" + implementation "org.eclipse.jetty:jetty-server:9.4.19.v20190610" implementation "com.squareup.moshi:moshi:1.8.0" kapt "com.squareup.moshi:moshi-kotlin-codegen:1.8.0" - implementation "com.squareup.okhttp3:okhttp:3.12.0" + implementation "com.squareup.okhttp3:okhttp:3.12.5" implementation project(':swerve') // Logging - compile 'io.github.microutils:kotlin-logging:1.6.10' + compile 'io.github.microutils:kotlin-logging:1.7.6' // unit tests testImplementation 'org.skyscreamer:jsonassert:1.+' From d18551a7511789db2bad581b736995f759ded8ee Mon Sep 17 00:00:00 2001 From: Jeff Hutchison Date: Tue, 24 Sep 2019 16:14:05 -0400 Subject: [PATCH 2/2] Convert Measure enum to data class --- gradle.properties | 1 + .../thirdcoast/telemetry/AbstractInventory.kt | 2 +- .../thirdcoast/telemetry/TelemetryService.kt | 206 +++++++++--------- .../thirdcoast/telemetry/grapher/Measure.kt | 94 -------- .../telemetry/grapher/Subscription.kt | 8 +- .../thirdcoast/telemetry/item/CanifierItem.kt | 152 +++++++------ .../telemetry/item/DigitalInputItem.kt | 37 ++-- .../telemetry/item/DigitalOutputItem.kt | 36 ++- .../thirdcoast/telemetry/item/Measurable.kt | 61 +++--- .../thirdcoast/telemetry/item/Measure.kt | 3 + .../thirdcoast/telemetry/item/ServoItem.kt | 49 +++-- .../thirdcoast/telemetry/item/TalonItem.kt | 173 ++++++++------- .../item/UltrasonicRangefinderItem.kt | 48 ++-- .../telemetry/RobotInventoryTest.java | 13 +- .../telemetry/grapher/SubscriptionTest.java | 9 +- telemetry/src/test/resources/inventory.json | 6 +- .../src/test/resources/subscription.json | 6 +- 17 files changed, 419 insertions(+), 485 deletions(-) delete mode 100644 telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/grapher/Measure.kt create mode 100644 telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/Measure.kt diff --git a/gradle.properties b/gradle.properties index d9f09caa..f75c8c75 100644 --- a/gradle.properties +++ b/gradle.properties @@ -1 +1,2 @@ githubUrl=https://github.com/strykeforce/thirdcoast +kotlin.code.style=official diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/AbstractInventory.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/AbstractInventory.kt index 8cd94d7a..a92d99fd 100644 --- a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/AbstractInventory.kt +++ b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/AbstractInventory.kt @@ -43,7 +43,7 @@ private fun JsonWriter.writeItems(items: List): JsonWriter { private fun JsonWriter.writeMeasures(items: List): JsonWriter { beginArray() - items.associateBy({ it.type }, { it.measures }).forEach { type, measures -> + items.associateBy({ it.type }, { it.measures }).forEach { (type, measures) -> beginObject() name("deviceType").value(type) name("deviceMeasures") diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/TelemetryService.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/TelemetryService.kt index ab4511fc..60ac33a7 100644 --- a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/TelemetryService.kt +++ b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/TelemetryService.kt @@ -11,124 +11,126 @@ import java.util.function.Function private val logger = KotlinLogging.logger {} /** - * The Telemetry service registers [Measurable] instances for data collection and controls the - * starting and stopping of the service. When active, the services listens for incoming config - * messages via a HTTP REST service and sends data over UDP. + * The Telemetry service is the main interface for client applications that are telemetry-enabled. It registers + * [Measurable] instances for data collection and controls the starting and stopping of the service. When active, + * the services listens for incoming control messages via a HTTP REST-like service and sends data over UDP. + * + * The constructor takes a factory function to create a [TelemetryController] instance with a given [Inventory]. + * The default [TelemetryController] has a constructor that serves this purpose, for example: + * ``` + * TelemetryService ts = new TelemetryService(TelemetryController::new); + * ts.register(talon); + * ts.start(); + * ``` */ class TelemetryService(private val telemetryControllerFactory: Function) { - // current implementation passes this list to the inventory as a collection via component binding - // when start is called. The inventory copies this collection into a List, using its index in - // this list as the inventory id. + // Current implementation passes the `items` list to the inventory as a collection when start is called. The inventory + // sorts and copies this collection into a List, using its index in this list as the inventory id. This should provide + // a stable order of measurable items that assists the Grapher client when saving its configuration. - private val items = LinkedHashSet() - private var telemetryController: TelemetryController? = null + private val items = LinkedHashSet() + private var telemetryController: TelemetryController? = null - /** Start the Telemetry service and listen for client connections. */ - fun start() { - if (telemetryController != null) { - logger.info("already started") - return - } - telemetryController = telemetryControllerFactory.apply(RobotInventory(items)).also { it.start() } - logger.info("started telemetry controller") + /** + * Start the Telemetry service and listen for client connections. A new instance of [TelemetryController] is created + * that reflects the current list of [Measurable] items. + */ + fun start() { + if (telemetryController != null) { + logger.info("already started") + return } + telemetryController = telemetryControllerFactory.apply(RobotInventory(items)).also { it.start() } + logger.info("started telemetry controller") + } - /** Stop the Telemetry service. */ - fun stop() { - if (telemetryController == null) { - logger.info("already stopped") - return - } - telemetryController?.shutdown() - telemetryController = null - logger.info("stopped") + /** Stop the Telemetry service. */ + fun stop() { + if (telemetryController == null) { + logger.info("already stopped") + return } + telemetryController?.shutdown() + telemetryController = null + logger.info("stopped") + } - /** - * Un-register all Items. - * - * @throws IllegalStateException if TelemetryService is running. - */ - fun clear() { - checkNotStarted() - items.clear() - logger.info("item set was cleared") - } + /** + * Un-register all [Measurable] items. + * + * @throws IllegalStateException if TelemetryService is running. + */ + fun clear() { + check(telemetryController == null) { "TelemetryService must be stopped to clear registered items." } + items.clear() + logger.info("item set was cleared") + } - /** - * Registers an Item for telemetry sending. - * - * @param item the Item to register for data collection - * @throws IllegalStateException if TelemetryService is running. - */ - fun register(item: Measurable) { - checkNotStarted() - if (items.add(item)) { - logger.info { "registered item ${item.description}" } - return - } - logger.info { "item ${item.description} was already registered" } + /** + * Registers an Item for telemetry sending. + * + * @param item the [Measurable] to register for data collection + * @throws IllegalStateException if TelemetryService is running. + */ + fun register(item: Measurable) { + check(telemetryController == null) { "TelemetryService must be stopped to register an item." } + if (items.add(item)) { + logger.info { "registered item ${item.description}" } + return } + logger.info { "item ${item.description} was already registered" } + } - /** - * Register a collection for telemetry sending. - * - * @param collection the collection of Items to register for data collection - * @throws IllegalStateException if TelemetryService is running. - */ - fun registerAll(collection: Collection) { - checkNotStarted() - items.addAll(collection) - logger.info { "registered all: $collection" } - } + /** + * Register a collection of [Measurable] items for telemetry sending. + * + * @param collection the collection of Items to register for data collection + * @throws IllegalStateException if TelemetryService is running. + */ + fun registerAll(collection: Collection) = collection.forEach(this::register) - /** - * Convenience method to register a `TalonSRX` for telemetry sending. - * - * @param talon the TalonSRX to register for data collection - * @throws IllegalStateException if TelemetryService is running. - */ - fun register(talon: TalonSRX) { - register(TalonItem(talon)) - } + /** + * Convenience method to register a `TalonSRX` for telemetry sending. + * + * @param talon the TalonSRX to register for data collection + * @throws IllegalStateException if TelemetryService is running. + */ + fun register(talon: TalonSRX) { + register(TalonItem(talon)) + } - /** - * Convenience method to register a [SwerveDrive] for telemetry sending. - * - * @throws IllegalStateException if TelemetryService is running. - */ - fun register(swerveDrive: SwerveDrive) = swerveDrive.wheels.forEach { - register(TalonItem(it.azimuthTalon)) - register(TalonItem(it.driveTalon)) - } + /** + * Convenience method to register a [SwerveDrive] for telemetry sending. + * + * @throws IllegalStateException if TelemetryService is running. + */ + fun register(swerveDrive: SwerveDrive) = swerveDrive.wheels.forEach { + register(TalonItem(it.azimuthTalon)) + register(TalonItem(it.driveTalon)) + } - /** - * Get an unmodifiable view of the registered items. - * - * @return an unmodifiable Set of Items. - */ - fun getItems(): Set { - return Collections.unmodifiableSet(items) - } + /** + * Get an unmodifiable view of the registered items. + * + * @return an unmodifiable Set of Items. + */ + fun getItems(): Set { + return Collections.unmodifiableSet(items) + } - /** - * Unregister [item] from a stopped `TelemetryService`. - * - * @throws AssertionError if TelemetryService is running. - */ - fun remove(item: Measurable) { - checkNotStarted() - if (items.remove(item)) { - logger.info { "removed $item" } - return - } - throw AssertionError(item.toString()) + /** + * Unregister a [Measurable] item. This service must be stopped first. + * + * @throws AssertionError if TelemetryService is running. + */ + fun remove(item: Measurable) { + check(telemetryController == null) { "TelemetryService must be stopped to remove an item." } + if (items.remove(item)) { + logger.info { "removed $item" } + return } + throw AssertionError(item.toString()) + } - private fun checkNotStarted() { - if (telemetryController != null) { - throw IllegalStateException("TelemetryService must be stopped.") - } - } } diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/grapher/Measure.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/grapher/Measure.kt deleted file mode 100644 index ca873fd8..00000000 --- a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/grapher/Measure.kt +++ /dev/null @@ -1,94 +0,0 @@ -package org.strykeforce.thirdcoast.telemetry.grapher - -/** Available telemetry measurement types. */ -enum class Measure constructor(val description: String) { - - // BaseMotorController - BASE_ID("Base ID"), - BUS_VOLTAGE("Bus Voltage"), - CLOSED_LOOP_ERROR("Closed Loop Error (PID 0)"), - CLOSED_LOOP_TARGET("Closed-loop Setpoint (PID 0)"), - CONTROL_MODE("Control Mode"), - DEVICE_ID("Device ID"), - ERROR_DERIVATIVE("Error Derivative (PID 0)"), - FIRMWARE_VERSION("Firmware Version"), - INTEGRAL_ACCUMULATOR("Integral Accumulator (PID 0)"), - OUTPUT_PERCENT("Output Percentage"), - OUTPUT_VOLTAGE("Output Voltage"), - OUTPUT_CURRENT("Output Current"), - SELECTED_SENSOR_POSITION("Selected Sensor Position (PID 0)"), - SELECTED_SENSOR_VELOCITY("Selected Sensor Velocity (PID 0)"), - ACTIVE_TRAJECTORY_HEADING("Active Trajectory Heading"), - ACTIVE_TRAJECTORY_POSITION("Active Trajectory Position"), - ACTIVE_TRAJECTORY_VELOCITY("Active Trajectory Velocity"), - - // TalonSRX SensorCollection - ANALOG_IN("Analog Position Input"), - ANALOG_IN_RAW("Analog Raw Input"), - ANALOG_IN_VELOCITY("Analog Velocity Input"), - QUAD_A_PIN("Quad A Pin State"), - QUAD_B_PIN("Quad B Pin State"), - QUAD_IDX_PIN("Quad Index Pin State"), - PULSE_WIDTH_POSITION("Pulse Width Position"), - PULSE_WIDTH_RISE_TO_FALL("PWM Pulse Width"), - PULSE_WIDTH_RISE_TO_RISE("PWM Period"), - PULSE_WIDTH_VELOCITY("Pulse Width Velocity"), - QUAD_POSITION("Quadrature Position"), - QUAD_VELOCITY("Quadrature Velocity"), - FORWARD_LIMIT_SWITCH_CLOSED("Forward Limit Switch Closed"), - REVERSE_LIMIT_SWITCH_CLOSED("Reverse Limit Switch Closed"), - // FORWARD_SOFT_LIMIT("Forward Soft Limit"), - // REVERSE_SOFT_LIMIT("Reverse Soft Limit"), - - // 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"), - - // General - DISPLACEMENT_X("X Displacement"), - DISPLACEMENT_Y("Y Displacement"), - DISPLACEMENT_Z("Z Displacement"), - ROTATION_X("X Rotation (Roll)"), - ROTATION_Y("Y Rotation (Pitch)"), - ROTATION_Z("Z Rotation (Yaw)"), - ROTATION_RATE_X("X Rotation Rate (Roll)"), - ROTATION_RATE_Y("Y Rotation Rate (Pitch)"), - ROTATION_RATE_Z("Z Rotation Rate (Yaw)"), - - COMPONENT_FORWARD("Forward Component"), - COMPONENT_STRAFE("Strafe Component"), - COMPONENT_YAW("Yaw Component"), - - DISPLACEMENT_EXPECTED("Expected Displacement"), - DISPLACEMENT_ACTUAL("Actual Displacement"), - DISPLACEMENT_ERROR("Displacement Error"), - VELOCITY_EXPECTED("Expected Velocity"), - VELOCITY_ACTUAL("Actual Velocity"), - VELOCITY_ERROR("Velocity Error"), - ACCELERATION_EXPECTED("Expected Acceleration"), - ACCELERATION_ACTUAL("Actual Acceleration"), - ACCELERATION_ERROR("Acceleration Error"), - JERK_EXPECTED("Expected Jerk"), - JERK_ACTUAL("Actual Jerk"), - JERK_ERROR("Jerk Error"), - - ENCODER_EXPECTED("Expected Encoder Value"), - ENCODER_ACTUAL("Actual Encoder Value"), - ENCODER_ERROR("Encoder Value Error"), - - POSITION("Position"), - RANGE("Range"), - ANGLE("Angle"), - VALUE("Measured Value"), - UNKNOWN("Unknown Measurement"), -} diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/grapher/Subscription.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/grapher/Subscription.kt index 232f1960..b14917b8 100644 --- a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/grapher/Subscription.kt +++ b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/grapher/Subscription.kt @@ -5,6 +5,7 @@ import com.squareup.moshi.Moshi import mu.KotlinLogging import okio.BufferedSink import org.strykeforce.thirdcoast.telemetry.Inventory +import org.strykeforce.thirdcoast.telemetry.item.Measure import java.io.IOException import java.util.* import java.util.function.DoubleSupplier @@ -20,12 +21,7 @@ class Subscription(inventory: Inventory, val client: String, requestJson: String val request: RequestJson = RequestJson.fromJson(requestJson) ?: RequestJson.EMPTY request.subscription.forEach { val item = inventory.itemForId(it.itemId) - val measure = try { - Measure.valueOf(it.measurementId) - } catch (e: IllegalArgumentException) { - logger.error { "no such measure \"${it.measurementId}\", request JSON = \n$requestJson" } - Measure.UNKNOWN - } + val measure = Measure(it.measurementId, it.measurementId) measurements += item.measurementFor(measure) descriptions += "${item.description}: ${measure.description}" } diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/CanifierItem.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/CanifierItem.kt index bd17a7d7..1fd85b11 100644 --- a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/CanifierItem.kt +++ b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/CanifierItem.kt @@ -2,89 +2,99 @@ 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 `Measurable` item. */ +internal const val PWM0_PULSE_WIDTH = "PWM0_PULSE_WIDTH" +internal const val PWM0_PERIOD = "PWM0_PERIOD" +internal const val PWM0_PULSE_WIDTH_POSITION = "PWM0_PULSE_WIDTH_POSITION" +internal const val PWM1_PULSE_WIDTH = "PWM1_PULSE_WIDTH" +internal const val PWM1_PERIOD = "PWM1_PERIOD" +internal const val PWM1_PULSE_WIDTH_POSITION = "PWM1_PULSE_WIDTH_POSITION" +internal const val PWM2_PULSE_WIDTH = "PWM2_PULSE_WIDTH" +internal const val PWM2_PERIOD = "PWM2_PERIOD" +internal const val PWM2_PULSE_WIDTH_POSITION = "PWM2_PULSE_WIDTH_POSITION" +internal const val PWM3_PULSE_WIDTH = "PWM3_PULSE_WIDTH" +internal const val PWM3_PERIOD = "PWM3_PERIOD" +internal const val PWM3_PULSE_WIDTH_POSITION = "PWM3_PULSE_WIDTH_POSITION" +internal const val QUAD_POSITION = "QUAD_POSITION" +internal const val QUAD_VELOCITY = "QUAD_VELOCITY" + +/** Represents a `CANifier` telemetry-enable `Measurable` item. */ class CanifierItem @JvmOverloads constructor( - private val canifier: CANifier, - override val description: String = "CANifier ${canifier.deviceID}" + private val canifier: CANifier, + override val description: String = "CANifier ${canifier.deviceID}" ) : Measurable { - 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") - } - } + override val deviceId = canifier.deviceID + override val type = "canifier" + override val measures = setOf( + Measure(PWM0_PULSE_WIDTH, "PWM 0 Pulse Width"), + Measure(PWM0_PERIOD, "PWM 0 Period"), + Measure(PWM0_PULSE_WIDTH_POSITION, "PWM 0 Pulse Width Position"), + Measure(PWM1_PULSE_WIDTH, "PWM 1 Pulse Width"), + Measure(PWM1_PERIOD, "PWM 1 Period"), + Measure(PWM1_PULSE_WIDTH_POSITION, "PWM 1 Pulse Width Position"), + Measure(PWM2_PULSE_WIDTH, "PWM 2 Pulse Width"), + Measure(PWM2_PERIOD, "PWM 2 Period"), + Measure(PWM2_PULSE_WIDTH_POSITION, "PWM 2 Pulse Width Position"), + Measure(PWM3_PULSE_WIDTH, "PWM 3 Pulse Width"), + Measure(PWM3_PERIOD, "PWM 3 Period"), + Measure(PWM3_PULSE_WIDTH_POSITION, "PWM 3 Pulse Width Position"), + Measure(QUAD_POSITION, "Quadrature Position"), + Measure(QUAD_VELOCITY, "Quadrature Velocity") + ) - private fun pulseWidthFor(channel: CANifier.PWMChannel): Double { - val pulseWidthAndPeriod = DoubleArray(2) - canifier.getPWMInput(channel, pulseWidthAndPeriod) - return pulseWidthAndPeriod[0] - } + override fun measurementFor(measure: Measure): DoubleSupplier { - private fun periodFor(channel: CANifier.PWMChannel): Double { - val pulseWidthAndPeriod = DoubleArray(2) - canifier.getPWMInput(channel, pulseWidthAndPeriod) - return pulseWidthAndPeriod[1] - } + return when (measure.name) { + 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() } - private fun pulseWidthPositionFor(channel: CANifier.PWMChannel): Double { - val pulseWidthAndPeriod = DoubleArray(2) - canifier.getPWMInput(channel, pulseWidthAndPeriod) - return 4096.0 * pulseWidthAndPeriod[0] / pulseWidthAndPeriod[1] + else -> DoubleSupplier { 2767.0 } } + } - override fun equals(other: Any?): Boolean { - if (this === other) return true - if (javaClass != other?.javaClass) return false + private fun pulseWidthFor(channel: CANifier.PWMChannel): Double { + val pulseWidthAndPeriod = DoubleArray(2) + canifier.getPWMInput(channel, pulseWidthAndPeriod) + return pulseWidthAndPeriod[0] + } - other as CanifierItem + private fun periodFor(channel: CANifier.PWMChannel): Double { + val pulseWidthAndPeriod = DoubleArray(2) + canifier.getPWMInput(channel, pulseWidthAndPeriod) + return pulseWidthAndPeriod[1] + } - if (deviceId != other.deviceId) return false + private fun pulseWidthPositionFor(channel: CANifier.PWMChannel): Double { + val pulseWidthAndPeriod = DoubleArray(2) + canifier.getPWMInput(channel, pulseWidthAndPeriod) + return 4096.0 * pulseWidthAndPeriod[0] / pulseWidthAndPeriod[1] + } - return true - } + 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 - 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 - ) - } } diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/DigitalInputItem.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/DigitalInputItem.kt index 1c0e9a3c..83d6b133 100644 --- a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/DigitalInputItem.kt +++ b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/DigitalInputItem.kt @@ -1,37 +1,36 @@ package org.strykeforce.thirdcoast.telemetry.item import edu.wpi.first.wpilibj.DigitalInput -import org.strykeforce.thirdcoast.telemetry.grapher.Measure -import org.strykeforce.thirdcoast.telemetry.grapher.Measure.VALUE import java.util.function.DoubleSupplier +internal const val VALUE = "VALUE" /** Represents a [DigitalInput] telemetry-enable `Measurable` item. */ class DigitalInputItem @JvmOverloads constructor( - private val digitalInput: DigitalInput, - override val description: String = "Digital Input ${digitalInput.channel}" + private val digitalInput: DigitalInput, + override val description: String = "Digital Input ${digitalInput.channel}" ) : Measurable { - override val deviceId = digitalInput.channel - override val type = "digitalInput" - override val measures = setOf(VALUE) + override val deviceId = digitalInput.channel + override val type = "digitalInput" + override val measures = setOf(Measure(VALUE, "Digital Input Value")) - override fun measurementFor(measure: Measure): DoubleSupplier { - require(measures.contains(measure)) - return DoubleSupplier { digitalInput.get().toDouble() } - } + override fun measurementFor(measure: Measure): DoubleSupplier { + require(measures.contains(measure)) + return DoubleSupplier { digitalInput.get().toDouble() } + } - override fun equals(other: Any?): Boolean { - if (this === other) return true - if (javaClass != other?.javaClass) return false + override fun equals(other: Any?): Boolean { + if (this === other) return true + if (javaClass != other?.javaClass) return false - other as DigitalInputItem + other as DigitalInputItem - if (deviceId != other.deviceId) return false + if (deviceId != other.deviceId) return false - return true - } + return true + } - override fun hashCode() = deviceId + override fun hashCode() = deviceId } diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/DigitalOutputItem.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/DigitalOutputItem.kt index 31152fa8..5e6565a8 100644 --- a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/DigitalOutputItem.kt +++ b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/DigitalOutputItem.kt @@ -2,36 +2,34 @@ package org.strykeforce.thirdcoast.telemetry.item import edu.wpi.first.wpilibj.DigitalInput import edu.wpi.first.wpilibj.DigitalOutput -import org.strykeforce.thirdcoast.telemetry.grapher.Measure -import org.strykeforce.thirdcoast.telemetry.grapher.Measure.VALUE import java.util.function.DoubleSupplier /** Represents a [DigitalInput] telemetry-enable `Measurable` item. */ class DigitalOutputItem @JvmOverloads constructor( - private val digitalOutput: DigitalOutput, - override val description: String = "Digital Output ${digitalOutput.channel}" + private val digitalOutput: DigitalOutput, + override val description: String = "Digital Output ${digitalOutput.channel}" ) : Measurable { - override val deviceId = digitalOutput.channel - override val type = "digitalOutput" - override val measures = setOf(VALUE) + override val deviceId = digitalOutput.channel + override val type = "digitalOutput" + override val measures = setOf(Measure(VALUE, "Digital Output Value")) - override fun measurementFor(measure: Measure): DoubleSupplier { - require(measures.contains(measure)) - return DoubleSupplier { digitalOutput.get().toDouble() } - } + override fun measurementFor(measure: Measure): DoubleSupplier { + require(measures.contains(measure)) + return DoubleSupplier { digitalOutput.get().toDouble() } + } - override fun equals(other: Any?): Boolean { - if (this === other) return true - if (javaClass != other?.javaClass) return false + override fun equals(other: Any?): Boolean { + if (this === other) return true + if (javaClass != other?.javaClass) return false - other as DigitalOutputItem + other as DigitalOutputItem - if (deviceId != other.deviceId) return false + if (deviceId != other.deviceId) return false - return true - } + return true + } - override fun hashCode() = deviceId + override fun hashCode() = deviceId } diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/Measurable.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/Measurable.kt index 62a21aa3..0437f054 100644 --- a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/Measurable.kt +++ b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/Measurable.kt @@ -1,6 +1,5 @@ package org.strykeforce.thirdcoast.telemetry.item -import org.strykeforce.thirdcoast.telemetry.grapher.Measure import java.util.function.DoubleSupplier /** @@ -13,36 +12,36 @@ import java.util.function.DoubleSupplier */ interface Measurable : Comparable { - /** - * Returns the underlying device id, for example, CAN bus address or PWM port. - */ - val deviceId: Int - - /** - * A `String` representing the underlying device type. - */ - val type: String - - /** - * The description of this item. - */ - val description: String - - /** - * `Set` of `Measure` parameters applicable to this item type. - */ - val measures: Set - - /** - * Suppliers that implement the measures. - * @return the supplier that gives the current measurement. - */ - fun measurementFor(measure: Measure): DoubleSupplier - - override fun compareTo(other: Measurable): Int { - val result = type.compareTo(other.type) - return if (result != 0) result else deviceId.compareTo(other.deviceId) - } + /** + * Returns the underlying device id, for example, CAN bus address or PWM port. + */ + val deviceId: Int + + /** + * A `String` representing the underlying device type. + */ + val type: String + + /** + * The description of this item. + */ + val description: String + + /** + * `Set` of `Measure` parameters applicable to this item type. + */ + val measures: Set + + /** + * Suppliers that implement the measures. + * @return the supplier that gives the current measurement. + */ + fun measurementFor(measure: Measure): DoubleSupplier + + override fun compareTo(other: Measurable): Int { + val result = type.compareTo(other.type) + return if (result != 0) result else deviceId.compareTo(other.deviceId) + } } diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/Measure.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/Measure.kt new file mode 100644 index 00000000..5e19404a --- /dev/null +++ b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/Measure.kt @@ -0,0 +1,3 @@ +package org.strykeforce.thirdcoast.telemetry.item + +data class Measure(val name: String, val description: String) diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/ServoItem.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/ServoItem.kt index 5d82a0d0..9947cea2 100644 --- a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/ServoItem.kt +++ b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/ServoItem.kt @@ -1,41 +1,44 @@ package org.strykeforce.thirdcoast.telemetry.item import edu.wpi.first.wpilibj.Servo -import org.strykeforce.thirdcoast.telemetry.grapher.Measure -import org.strykeforce.thirdcoast.telemetry.grapher.Measure.ANGLE -import org.strykeforce.thirdcoast.telemetry.grapher.Measure.POSITION import java.util.function.DoubleSupplier +internal const val POSITION = "POSITION" +internal const val ANGLE = "ANGLE" + /** Represents a [Servo] telemetry-enable `Measurable` item. */ class ServoItem @JvmOverloads constructor( - private val servo: Servo, - override val description: String = "Servo ${servo.channel}" + private val servo: Servo, + override val description: String = "Servo ${servo.channel}" ) : Measurable { - override val deviceId = servo.channel - override val type = "servo" - override val measures = setOf(POSITION, ANGLE) + override val deviceId = servo.channel + override val type = "servo" + override val measures = setOf( + Measure(POSITION, "Servo Position"), + Measure(ANGLE, "Servo Angle") + ) - override fun measurementFor(measure: Measure): DoubleSupplier { - require(measures.contains(measure)) + override fun measurementFor(measure: Measure): DoubleSupplier { + require(measures.contains(measure)) - return when (measure) { - POSITION -> DoubleSupplier { servo.position } - ANGLE -> DoubleSupplier { servo.angle } - else -> TODO("$measure not implemented") - } + return when (measure.name) { + POSITION -> DoubleSupplier { servo.position } + ANGLE -> DoubleSupplier { servo.angle } + else -> DoubleSupplier { 2767.0 } } + } - override fun equals(other: Any?): Boolean { - if (this === other) return true - if (javaClass != other?.javaClass) return false + override fun equals(other: Any?): Boolean { + if (this === other) return true + if (javaClass != other?.javaClass) return false - other as ServoItem + other as ServoItem - if (deviceId != other.deviceId) return false + if (deviceId != other.deviceId) return false - return true - } + return true + } - override fun hashCode() = deviceId + override fun hashCode() = deviceId } diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/TalonItem.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/TalonItem.kt index 4e0d8790..788fbe54 100644 --- a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/TalonItem.kt +++ b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/TalonItem.kt @@ -1,97 +1,112 @@ package org.strykeforce.thirdcoast.telemetry.item 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 +internal const val CLOSED_LOOP_TARGET = "CLOSED_LOOP_TARGET" +internal const val OUTPUT_CURRENT = "OUTPUT_CURRENT" +internal const val OUTPUT_VOLTAGE = "OUTPUT_VOLTAGE" +internal const val OUTPUT_PERCENT = "OUTPUT_PERCENT" +internal const val SELECTED_SENSOR_POSITION = "SELECTED_SENSOR_POSITION" +internal const val SELECTED_SENSOR_VELOCITY = "SELECTED_SENSOR_VELOCITY" +internal const val ACTIVE_TRAJECTORY_POSITION = "ACTIVE_TRAJECTORY_POSITION" +internal const val ACTIVE_TRAJECTORY_VELOCITY = "ACTIVE_TRAJECTORY_VELOCITY" +internal const val CLOSED_LOOP_ERROR = "CLOSED_LOOP_ERROR" +internal const val BUS_VOLTAGE = "BUS_VOLTAGE" +internal const val ERROR_DERIVATIVE = "ERROR_DERIVATIVE" +internal const val INTEGRAL_ACCUMULATOR = "INTEGRAL_ACCUMULATOR" +internal const val ANALOG_IN = "ANALOG_IN" +internal const val ANALOG_IN_RAW = "ANALOG_IN_RAW" +internal const val ANALOG_IN_VELOCITY = "ANALOG_IN_VELOCITY" +internal const val QUAD_A_PIN = "QUAD_A_PIN" +internal const val QUAD_B_PIN = "QUAD_B_PIN" +internal const val QUAD_IDX_PIN = "QUAD_IDX_PIN" +internal const val PULSE_WIDTH_POSITION = "PULSE_WIDTH_POSITION" +internal const val PULSE_WIDTH_VELOCITY = "PULSE_WIDTH_VELOCITY" +internal const val PULSE_WIDTH_RISE_TO_FALL = "PULSE_WIDTH_RISE_TO_FALL" +internal const val PULSE_WIDTH_RISE_TO_RISE = "PULSE_WIDTH_RISE_TO_RISE" +internal const val FORWARD_LIMIT_SWITCH_CLOSED = "FORWARD_LIMIT_SWITCH_CLOSED" +internal const val REVERSE_LIMIT_SWITCH_CLOSED = "REVERSE_LIMIT_SWITCH_CLOSED" + /** Represents a [TalonSRX] telemetry-enable `Measurable` item. */ class TalonItem @JvmOverloads constructor( - private val talon: TalonSRX, - override val description: String = "TalonSRX ${talon.deviceID}" + private val talon: TalonSRX, + override val description: String = "TalonSRX ${talon.deviceID}" ) : Measurable { - override val deviceId = talon.deviceID - override val type = "talon" - override val measures - get() = MEASURES + override val deviceId = talon.deviceID + override val type = "talon" + override val measures = setOf( + Measure(CLOSED_LOOP_TARGET, "Closed-loop Setpoint (PID 0)"), + Measure(OUTPUT_CURRENT, "Output Current"), + Measure(OUTPUT_VOLTAGE, "Output Voltage"), + Measure(OUTPUT_PERCENT, "Output Percentage"), + Measure(SELECTED_SENSOR_POSITION, "Selected Sensor Position (PID 0)"), + Measure(SELECTED_SENSOR_VELOCITY, "Selected Sensor Velocity (PID 0)"), + Measure(ACTIVE_TRAJECTORY_POSITION, "Active Trajectory Position"), + Measure(ACTIVE_TRAJECTORY_VELOCITY, "Active Trajectory Velocity"), + Measure(CLOSED_LOOP_ERROR, "Closed Loop Error (PID 0)"), + Measure(BUS_VOLTAGE, "Bus Voltage"), + Measure(ERROR_DERIVATIVE, "Error Derivative (PID 0)"), + Measure(INTEGRAL_ACCUMULATOR, "Integral Accumulator (PID 0)"), + Measure(ANALOG_IN, "Analog Position Input"), + Measure(ANALOG_IN_RAW, "Analog Raw Input"), + Measure(ANALOG_IN_VELOCITY, "Analog Velocity Input"), + Measure(QUAD_A_PIN, "Quad A Pin State"), + Measure(QUAD_B_PIN, "Quad B Pin State"), + Measure(QUAD_IDX_PIN, "Quad Index Pin State"), + Measure(PULSE_WIDTH_POSITION, "Pulse Width Position"), + Measure(PULSE_WIDTH_VELOCITY, "Pulse Width Velocity"), + Measure(PULSE_WIDTH_RISE_TO_FALL, "PWM Pulse Width"), + Measure(PULSE_WIDTH_RISE_TO_RISE, "PWM Period"), + Measure(FORWARD_LIMIT_SWITCH_CLOSED, "Forward Limit Switch Closed"), + Measure(REVERSE_LIMIT_SWITCH_CLOSED, "Reverse Limit Switch Closed") + ) - private val sensorCollection = requireNotNull(talon.sensorCollection) + private val sensorCollection = requireNotNull(talon.sensorCollection) - override fun measurementFor(measure: Measure): DoubleSupplier { - return when (measure) { - UNKNOWN -> DoubleSupplier { 2767.0 } - CLOSED_LOOP_TARGET -> DoubleSupplier { talon.getClosedLoopTarget(0) } - OUTPUT_CURRENT -> DoubleSupplier { talon.outputCurrent } - OUTPUT_VOLTAGE -> DoubleSupplier { talon.motorOutputVoltage } - OUTPUT_PERCENT -> DoubleSupplier { talon.motorOutputPercent } - SELECTED_SENSOR_POSITION -> DoubleSupplier { talon.getSelectedSensorPosition(0).toDouble() } - SELECTED_SENSOR_VELOCITY -> DoubleSupplier { talon.getSelectedSensorVelocity(0).toDouble() } - ACTIVE_TRAJECTORY_POSITION -> DoubleSupplier { talon.activeTrajectoryPosition.toDouble() } - ACTIVE_TRAJECTORY_VELOCITY -> DoubleSupplier { talon.activeTrajectoryVelocity.toDouble() } - CLOSED_LOOP_ERROR -> DoubleSupplier { talon.getClosedLoopError(0).toDouble() } - BUS_VOLTAGE -> DoubleSupplier { talon.busVoltage } - ERROR_DERIVATIVE -> DoubleSupplier { talon.getErrorDerivative(0) } - INTEGRAL_ACCUMULATOR -> DoubleSupplier { talon.getIntegralAccumulator(0) } - ANALOG_IN -> DoubleSupplier { sensorCollection.analogIn.toDouble() } - ANALOG_IN_RAW -> DoubleSupplier { sensorCollection.analogInRaw.toDouble() } - ANALOG_IN_VELOCITY -> DoubleSupplier { sensorCollection.analogInVel.toDouble() } - QUAD_POSITION -> DoubleSupplier { sensorCollection.quadraturePosition.toDouble() } - QUAD_VELOCITY -> DoubleSupplier { sensorCollection.quadratureVelocity.toDouble() } - QUAD_A_PIN -> DoubleSupplier { sensorCollection.pinStateQuadA.toDouble() } - QUAD_B_PIN -> DoubleSupplier { sensorCollection.pinStateQuadB.toDouble() } - QUAD_IDX_PIN -> DoubleSupplier { sensorCollection.pinStateQuadIdx.toDouble() } - PULSE_WIDTH_POSITION -> DoubleSupplier { (sensorCollection.pulseWidthPosition and 0xFFF).toDouble() } - PULSE_WIDTH_VELOCITY -> DoubleSupplier { sensorCollection.pulseWidthVelocity.toDouble() } - PULSE_WIDTH_RISE_TO_FALL -> DoubleSupplier { sensorCollection.pulseWidthRiseToFallUs.toDouble() } - PULSE_WIDTH_RISE_TO_RISE -> DoubleSupplier { sensorCollection.pulseWidthRiseToRiseUs.toDouble() } - FORWARD_LIMIT_SWITCH_CLOSED -> DoubleSupplier { sensorCollection.isFwdLimitSwitchClosed.toDouble() } - REVERSE_LIMIT_SWITCH_CLOSED -> DoubleSupplier { sensorCollection.isRevLimitSwitchClosed.toDouble() } - else -> TODO("$measure not implemented") - } + override fun measurementFor(measure: Measure): DoubleSupplier { + return when (measure.name) { + CLOSED_LOOP_TARGET -> DoubleSupplier { talon.getClosedLoopTarget(0) } + OUTPUT_CURRENT -> DoubleSupplier { talon.outputCurrent } + OUTPUT_VOLTAGE -> DoubleSupplier { talon.motorOutputVoltage } + OUTPUT_PERCENT -> DoubleSupplier { talon.motorOutputPercent } + SELECTED_SENSOR_POSITION -> DoubleSupplier { talon.getSelectedSensorPosition(0).toDouble() } + SELECTED_SENSOR_VELOCITY -> DoubleSupplier { talon.getSelectedSensorVelocity(0).toDouble() } + ACTIVE_TRAJECTORY_POSITION -> DoubleSupplier { talon.activeTrajectoryPosition.toDouble() } + ACTIVE_TRAJECTORY_VELOCITY -> DoubleSupplier { talon.activeTrajectoryVelocity.toDouble() } + CLOSED_LOOP_ERROR -> DoubleSupplier { talon.getClosedLoopError(0).toDouble() } + BUS_VOLTAGE -> DoubleSupplier { talon.busVoltage } + ERROR_DERIVATIVE -> DoubleSupplier { talon.getErrorDerivative(0) } + INTEGRAL_ACCUMULATOR -> DoubleSupplier { talon.getIntegralAccumulator(0) } + ANALOG_IN -> DoubleSupplier { sensorCollection.analogIn.toDouble() } + ANALOG_IN_RAW -> DoubleSupplier { sensorCollection.analogInRaw.toDouble() } + ANALOG_IN_VELOCITY -> DoubleSupplier { sensorCollection.analogInVel.toDouble() } + QUAD_POSITION -> DoubleSupplier { sensorCollection.quadraturePosition.toDouble() } + QUAD_VELOCITY -> DoubleSupplier { sensorCollection.quadratureVelocity.toDouble() } + QUAD_A_PIN -> DoubleSupplier { sensorCollection.pinStateQuadA.toDouble() } + QUAD_B_PIN -> DoubleSupplier { sensorCollection.pinStateQuadB.toDouble() } + QUAD_IDX_PIN -> DoubleSupplier { sensorCollection.pinStateQuadIdx.toDouble() } + PULSE_WIDTH_POSITION -> DoubleSupplier { (sensorCollection.pulseWidthPosition and 0xFFF).toDouble() } + PULSE_WIDTH_VELOCITY -> DoubleSupplier { sensorCollection.pulseWidthVelocity.toDouble() } + PULSE_WIDTH_RISE_TO_FALL -> DoubleSupplier { sensorCollection.pulseWidthRiseToFallUs.toDouble() } + PULSE_WIDTH_RISE_TO_RISE -> DoubleSupplier { sensorCollection.pulseWidthRiseToRiseUs.toDouble() } + FORWARD_LIMIT_SWITCH_CLOSED -> DoubleSupplier { sensorCollection.isFwdLimitSwitchClosed.toDouble() } + REVERSE_LIMIT_SWITCH_CLOSED -> DoubleSupplier { sensorCollection.isRevLimitSwitchClosed.toDouble() } + else -> DoubleSupplier { 2767.0 } } + } - override fun equals(other: Any?): Boolean { - if (this === other) return true - if (javaClass != other?.javaClass) return false - - other as TalonItem + override fun equals(other: Any?): Boolean { + if (this === other) return true + if (javaClass != other?.javaClass) return false - if (deviceId != other.deviceId) return false + other as TalonItem - return true - } + if (deviceId != other.deviceId) return false - override fun hashCode() = deviceId + return true + } - companion object { - val MEASURES = setOf( - CLOSED_LOOP_TARGET, - OUTPUT_CURRENT, - OUTPUT_VOLTAGE, - OUTPUT_PERCENT, - SELECTED_SENSOR_POSITION, - SELECTED_SENSOR_VELOCITY, - ACTIVE_TRAJECTORY_POSITION, - ACTIVE_TRAJECTORY_VELOCITY, - CLOSED_LOOP_ERROR, - BUS_VOLTAGE, - ERROR_DERIVATIVE, - INTEGRAL_ACCUMULATOR, - ANALOG_IN, - ANALOG_IN_RAW, - ANALOG_IN_VELOCITY, - QUAD_POSITION, - QUAD_VELOCITY, - QUAD_A_PIN, - QUAD_B_PIN, - QUAD_IDX_PIN, - PULSE_WIDTH_POSITION, - PULSE_WIDTH_VELOCITY, - PULSE_WIDTH_RISE_TO_FALL, - PULSE_WIDTH_RISE_TO_RISE, - FORWARD_LIMIT_SWITCH_CLOSED, - REVERSE_LIMIT_SWITCH_CLOSED - ) - } + override fun hashCode() = deviceId } diff --git a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/UltrasonicRangefinderItem.kt b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/UltrasonicRangefinderItem.kt index 9bb106f0..9c1f4df1 100644 --- a/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/UltrasonicRangefinderItem.kt +++ b/telemetry/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/UltrasonicRangefinderItem.kt @@ -2,43 +2,41 @@ package org.strykeforce.thirdcoast.telemetry.item import com.ctre.phoenix.CANifier import com.ctre.phoenix.CANifier.PWMChannel -import org.strykeforce.thirdcoast.telemetry.grapher.Measure -import org.strykeforce.thirdcoast.telemetry.grapher.Measure.VALUE import java.util.function.DoubleSupplier -/** Represents a PWM ultrasonic rangefinder telemetry-enable `Measurable` item. */ +/** Represents a PWM ultrasonic rangefinder telemetry-enable `Measurable` item connected to a `CANifier`. */ class UltrasonicRangefinderItem @JvmOverloads constructor( - canId: Int, - private val pwmChannel: PWMChannel, - override val description: String = "Sensor ${canId * 10 + pwmChannel.value}" + canId: Int, + private val pwmChannel: PWMChannel, + override val description: String = "Sensor ${canId * 10 + pwmChannel.value}" ) : Measurable { - override val deviceId = canId * 10 + pwmChannel.value - override val type = "sensor" - override val measures = setOf(VALUE) + override val deviceId = canId * 10 + pwmChannel.value + override val type = "sensor" + override val measures = setOf(Measure(VALUE, "PWM Duty Cycle")) - private val canifier: CANifier = CANifier(canId) - private val dutyCycleAndPeriod = DoubleArray(2) + private val canifier: CANifier = CANifier(canId) + private val dutyCycleAndPeriod = DoubleArray(2) - override fun measurementFor(measure: Measure): DoubleSupplier { - require(measures.contains(measure)) - return DoubleSupplier { - canifier.getPWMInput(pwmChannel, dutyCycleAndPeriod) - dutyCycleAndPeriod[0] - } + override fun measurementFor(measure: Measure): DoubleSupplier { + require(measures.contains(measure)) + return DoubleSupplier { + canifier.getPWMInput(pwmChannel, dutyCycleAndPeriod) + dutyCycleAndPeriod[0] } + } - override fun equals(other: Any?): Boolean { - if (this === other) return true - if (javaClass != other?.javaClass) return false + override fun equals(other: Any?): Boolean { + if (this === other) return true + if (javaClass != other?.javaClass) return false - other as UltrasonicRangefinderItem + other as UltrasonicRangefinderItem - if (deviceId != other.deviceId) return false + if (deviceId != other.deviceId) return false - return true - } + return true + } - override fun hashCode() = deviceId + override fun hashCode() = deviceId } diff --git a/telemetry/src/test/java/org/strykeforce/thirdcoast/telemetry/RobotInventoryTest.java b/telemetry/src/test/java/org/strykeforce/thirdcoast/telemetry/RobotInventoryTest.java index 9e551cb4..3349afae 100644 --- a/telemetry/src/test/java/org/strykeforce/thirdcoast/telemetry/RobotInventoryTest.java +++ b/telemetry/src/test/java/org/strykeforce/thirdcoast/telemetry/RobotInventoryTest.java @@ -10,14 +10,16 @@ import org.skyscreamer.jsonassert.JSONAssert; import org.strykeforce.thirdcoast.telemetry.grapher.ResourceHelper; import org.strykeforce.thirdcoast.telemetry.item.Measurable; +import org.strykeforce.thirdcoast.telemetry.item.Measure; import java.io.IOException; import java.nio.charset.Charset; -import java.util.*; +import java.util.Collection; +import java.util.List; +import java.util.Set; import static org.assertj.core.api.Assertions.assertThat; import static org.mockito.Mockito.when; -import static org.strykeforce.thirdcoast.telemetry.grapher.Measure.*; @ExtendWith(MockitoExtension.class) class RobotInventoryTest { @@ -39,16 +41,17 @@ void itemForIdea() { @Test void writeInventory() throws IOException, JSONException { - when(itemOne.getMeasures()).thenReturn(Set.of(POSITION, VALUE)); + when(itemOne.getMeasures()) + .thenReturn( + Set.of(new Measure("POSITION", "POSITION"), new Measure("VALUE", "VALUE description"))); when(itemOne.getType()).thenReturn("one"); - when(itemTwo.getMeasures()).thenReturn(Set.of(BUS_VOLTAGE)); + when(itemTwo.getMeasures()).thenReturn(Set.of(new Measure("BUS_VOLTAGE", "BUS_VOLTAGE"))); when(itemTwo.getType()).thenReturn("two"); RobotInventory inventory = new RobotInventory(items); Buffer buffer = new Buffer(); inventory.writeInventory(buffer); String actual = buffer.readString(Charset.defaultCharset()); - String golden = ResourceHelper.getString("/inventory.json"); JSONAssert.assertEquals(golden, actual, false); diff --git a/telemetry/src/test/java/org/strykeforce/thirdcoast/telemetry/grapher/SubscriptionTest.java b/telemetry/src/test/java/org/strykeforce/thirdcoast/telemetry/grapher/SubscriptionTest.java index 1f55bd0d..e0bbf295 100644 --- a/telemetry/src/test/java/org/strykeforce/thirdcoast/telemetry/grapher/SubscriptionTest.java +++ b/telemetry/src/test/java/org/strykeforce/thirdcoast/telemetry/grapher/SubscriptionTest.java @@ -12,6 +12,7 @@ import org.skyscreamer.jsonassert.JSONParser; import org.strykeforce.thirdcoast.telemetry.Inventory; import org.strykeforce.thirdcoast.telemetry.item.Measurable; +import org.strykeforce.thirdcoast.telemetry.item.Measure; import java.io.IOException; import java.nio.charset.Charset; @@ -19,7 +20,7 @@ import static org.mockito.Mockito.doReturn; import static org.mockito.Mockito.when; -import static org.strykeforce.thirdcoast.telemetry.grapher.Measure.*; + @ExtendWith(MockitoExtension.class) class SubscriptionTest { @@ -32,9 +33,9 @@ class SubscriptionTest { void setUp() { doReturn(itemZero).when(inventory).itemForId(0); doReturn(itemOne).when(inventory).itemForId(1); - doReturn((DoubleSupplier) () -> 27d).when(itemZero).measurementFor(BASE_ID); - doReturn((DoubleSupplier) () -> 67d).when(itemZero).measurementFor(VALUE); - doReturn((DoubleSupplier) () -> 2767d).when(itemOne).measurementFor(JERK_EXPECTED); + doReturn((DoubleSupplier) () -> 27d).when(itemZero).measurementFor(new Measure("BASE_ID","BASE_ID")); + doReturn((DoubleSupplier) () -> 67d).when(itemZero).measurementFor(new Measure("VALUE","VALUE")); + doReturn((DoubleSupplier) () -> 2767d).when(itemOne).measurementFor(new Measure("JERK_EXPECTED","JERK_EXPECTED")); when(itemZero.getDescription()).thenReturn("item zero"); when(itemOne.getDescription()).thenReturn("item one"); } diff --git a/telemetry/src/test/resources/inventory.json b/telemetry/src/test/resources/inventory.json index c37cc58b..712c8b41 100644 --- a/telemetry/src/test/resources/inventory.json +++ b/telemetry/src/test/resources/inventory.json @@ -15,11 +15,11 @@ "deviceMeasures": [ { "id": "VALUE", - "description": "Measured Value" + "description": "VALUE description" }, { "id": "POSITION", - "description": "Position" + "description": "POSITION" } ] }, @@ -28,7 +28,7 @@ "deviceMeasures": [ { "id": "BUS_VOLTAGE", - "description": "Bus Voltage" + "description": "BUS_VOLTAGE" } ] } diff --git a/telemetry/src/test/resources/subscription.json b/telemetry/src/test/resources/subscription.json index 44163568..6c810d89 100644 --- a/telemetry/src/test/resources/subscription.json +++ b/telemetry/src/test/resources/subscription.json @@ -2,8 +2,8 @@ "type": "subscription", "timestamp": 2767, "descriptions": [ - "item zero: Base ID", - "item zero: Measured Value", - "item one: Expected Jerk" + "item zero: BASE_ID", + "item zero: VALUE", + "item one: JERK_EXPECTED" ] }