Skip to content

Commit 3269b1c

Browse files
jhhjhh
committed
Add support for TalonFX Measureable item
1 parent 17e5200 commit 3269b1c

File tree

4 files changed

+123
-10
lines changed

4 files changed

+123
-10
lines changed

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ plugins {
88
}
99

1010
group = 'org.strykeforce'
11-
version = '20.0.0'
11+
version = '20.0.1'
1212

1313
sourceCompatibility = JavaVersion.VERSION_11
1414
targetCompatibility = JavaVersion.VERSION_11

‎src/main/kotlin/org/strykeforce/thirdcoast/telemetry/TelemetryService.kt‎

Lines changed: 16 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,12 @@
11
package org.strykeforce.thirdcoast.telemetry
22

3+
import com.ctre.phoenix.motorcontrol.can.TalonFX
34
import com.ctre.phoenix.motorcontrol.can.TalonSRX
45
import mu.KotlinLogging
56
import org.strykeforce.thirdcoast.swerve.SwerveDrive
7+
import org.strykeforce.thirdcoast.talon.TalonFXItem
68
import org.strykeforce.thirdcoast.telemetry.item.Measurable
7-
import org.strykeforce.thirdcoast.telemetry.item.TalonItem
9+
import org.strykeforce.thirdcoast.telemetry.item.TalonSRXItem
810
import java.util.*
911
import java.util.function.Function
1012

@@ -97,7 +99,17 @@ class TelemetryService(private val telemetryControllerFactory: Function<Inventor
9799
* @throws IllegalStateException if TelemetryService is running.
98100
*/
99101
fun register(talon: TalonSRX) {
100-
register(TalonItem(talon))
102+
register(TalonSRXItem(talon))
103+
}
104+
105+
/**
106+
* Convenience method to register a `TalonFX` for telemetry sending.
107+
*
108+
* @param talon the TalonSRX to register for data collection
109+
* @throws IllegalStateException if TelemetryService is running.
110+
*/
111+
fun register(talon: TalonFX) {
112+
register(TalonFXItem(talon))
101113
}
102114

103115
/**
@@ -106,8 +118,8 @@ class TelemetryService(private val telemetryControllerFactory: Function<Inventor
106118
* @throws IllegalStateException if TelemetryService is running.
107119
*/
108120
fun register(swerveDrive: SwerveDrive) = swerveDrive.wheels.forEach {
109-
register(TalonItem(it.azimuthTalon))
110-
register(TalonItem(it.driveTalon))
121+
register(TalonSRXItem(it.azimuthTalon))
122+
register(TalonSRXItem(it.driveTalon))
111123
}
112124

113125
/**

‎src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/TalonFXItem.kt‎

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
package org.strykeforce.thirdcoast.talon
2+
3+
import com.ctre.phoenix.motorcontrol.can.TalonFX
4+
import org.strykeforce.thirdcoast.telemetry.item.Measurable
5+
import org.strykeforce.thirdcoast.telemetry.item.Measure
6+
import java.util.function.DoubleSupplier
7+
8+
internal const val CLOSED_LOOP_TARGET = "CLOSED_LOOP_TARGET"
9+
internal const val STATOR_CURRENT = "STATOR_CURRENT"
10+
internal const val SUPPLY_CURRENT = "SUPPLY_CURRENT"
11+
internal const val OUTPUT_VOLTAGE = "OUTPUT_VOLTAGE"
12+
internal const val OUTPUT_PERCENT = "OUTPUT_PERCENT"
13+
internal const val INTEGRATED_SENSOR_POSITION = "INTEGRATED_SENSOR_POSITION"
14+
internal const val INTEGRATED_SENSOR_VELOCITY = "INTEGRATED_SENSOR_VELOCITY"
15+
internal const val INTEGRATED_SENSOR_ABSOLUTE_POSITION = "INTEGRATED_SENSOR_ABSOLUTE_POSITION"
16+
internal const val SELECTED_SENSOR_POSITION = "SELECTED_SENSOR_POSITION"
17+
internal const val SELECTED_SENSOR_VELOCITY = "SELECTED_SENSOR_VELOCITY"
18+
internal const val ACTIVE_TRAJECTORY_POSITION = "ACTIVE_TRAJECTORY_POSITION"
19+
internal const val ACTIVE_TRAJECTORY_VELOCITY = "ACTIVE_TRAJECTORY_VELOCITY"
20+
internal const val ACTIVE_TRAJECTORY_ARB_FEED_FWD = "ACTIVE_TRAJECTORY_ARB_FEED_FWD"
21+
internal const val CLOSED_LOOP_ERROR = "CLOSED_LOOP_ERROR"
22+
internal const val BUS_VOLTAGE = "BUS_VOLTAGE"
23+
internal const val ERROR_DERIVATIVE = "ERROR_DERIVATIVE"
24+
internal const val INTEGRAL_ACCUMULATOR = "INTEGRAL_ACCUMULATOR"
25+
internal const val FORWARD_LIMIT_SWITCH_CLOSED = "FORWARD_LIMIT_SWITCH_CLOSED"
26+
internal const val REVERSE_LIMIT_SWITCH_CLOSED = "REVERSE_LIMIT_SWITCH_CLOSED"
27+
internal const val TEMPERATURE = "TEMPERATURE"
28+
29+
class TalonFXItem @JvmOverloads constructor(
30+
private val talonFX: TalonFX,
31+
override val description: String = "TalonFX ${talonFX.deviceID}"
32+
) : Measurable {
33+
34+
override val deviceId = talonFX.deviceID
35+
override val type = "talonFX"
36+
override val measures = setOf(
37+
Measure(CLOSED_LOOP_TARGET, "Closed-loop Setpoint (PID 0)"),
38+
Measure(STATOR_CURRENT, "Stator Current"),
39+
Measure(SUPPLY_CURRENT, "Supply Current"),
40+
Measure(OUTPUT_VOLTAGE, "Output Voltage"),
41+
Measure(OUTPUT_PERCENT, "Output Percent"),
42+
Measure(INTEGRATED_SENSOR_POSITION, "Integrated Sensor Position"),
43+
Measure(INTEGRATED_SENSOR_ABSOLUTE_POSITION, "Integrated Sensor ABS Position"),
44+
Measure(INTEGRATED_SENSOR_VELOCITY, "Integrated Sensor Velocity"),
45+
Measure(SELECTED_SENSOR_POSITION, "Selected Sensor Position"),
46+
Measure(SELECTED_SENSOR_VELOCITY, "Selected Sensor Velocity"),
47+
Measure(ACTIVE_TRAJECTORY_POSITION, "Active Trajectory Position"),
48+
Measure(ACTIVE_TRAJECTORY_VELOCITY, "Active Trajectory Velocity"),
49+
Measure(ACTIVE_TRAJECTORY_ARB_FEED_FWD, "Active Trajectory Arb. Feed FWD"),
50+
Measure(CLOSED_LOOP_ERROR, "Closed Loop Error (PID 0)"),
51+
Measure(BUS_VOLTAGE, "Bus Voltage"),
52+
Measure(ERROR_DERIVATIVE, "Error Derivative (PID 0)"),
53+
Measure(INTEGRAL_ACCUMULATOR, "Integral Accumulator (PID 0)"),
54+
Measure(FORWARD_LIMIT_SWITCH_CLOSED, "Forward Limit Switch Closed"),
55+
Measure(REVERSE_LIMIT_SWITCH_CLOSED, "Reverse Limit Switch Closed"),
56+
Measure(TEMPERATURE, "Controller Temperature")
57+
58+
)
59+
60+
private val sensorCollection = requireNotNull(talonFX.sensorCollection)
61+
62+
63+
override fun measurementFor(measure: Measure): DoubleSupplier {
64+
return when (measure.name) {
65+
CLOSED_LOOP_TARGET -> DoubleSupplier { talonFX.closedLoopTarget }
66+
STATOR_CURRENT -> DoubleSupplier { talonFX.statorCurrent }
67+
SUPPLY_CURRENT -> DoubleSupplier { talonFX.supplyCurrent }
68+
OUTPUT_VOLTAGE -> DoubleSupplier { talonFX.motorOutputVoltage }
69+
OUTPUT_PERCENT -> DoubleSupplier { talonFX.motorOutputPercent }
70+
INTEGRATED_SENSOR_VELOCITY -> DoubleSupplier { sensorCollection.integratedSensorVelocity }
71+
SELECTED_SENSOR_POSITION -> DoubleSupplier { talonFX.selectedSensorPosition.toDouble() }
72+
SELECTED_SENSOR_VELOCITY -> DoubleSupplier { talonFX.selectedSensorVelocity.toDouble() }
73+
ACTIVE_TRAJECTORY_POSITION -> DoubleSupplier { talonFX.activeTrajectoryPosition.toDouble() }
74+
ACTIVE_TRAJECTORY_VELOCITY -> DoubleSupplier { talonFX.activeTrajectoryVelocity.toDouble() }
75+
ACTIVE_TRAJECTORY_ARB_FEED_FWD -> DoubleSupplier { talonFX.activeTrajectoryArbFeedFwd }
76+
CLOSED_LOOP_ERROR -> DoubleSupplier { talonFX.closedLoopError.toDouble() }
77+
BUS_VOLTAGE -> DoubleSupplier { talonFX.busVoltage }
78+
ERROR_DERIVATIVE -> DoubleSupplier { talonFX.errorDerivative }
79+
INTEGRAL_ACCUMULATOR -> DoubleSupplier { talonFX.integralAccumulator }
80+
FORWARD_LIMIT_SWITCH_CLOSED -> DoubleSupplier { sensorCollection.isFwdLimitSwitchClosed.toDouble() }
81+
REVERSE_LIMIT_SWITCH_CLOSED -> DoubleSupplier { sensorCollection.isRevLimitSwitchClosed.toDouble() }
82+
INTEGRATED_SENSOR_POSITION -> DoubleSupplier { sensorCollection.integratedSensorPosition }
83+
INTEGRATED_SENSOR_ABSOLUTE_POSITION -> DoubleSupplier { sensorCollection.integratedSensorAbsolutePosition }
84+
TEMPERATURE -> DoubleSupplier { talonFX.temperature }
85+
else -> DoubleSupplier { 2767.0 }
86+
}
87+
}
88+
89+
override fun equals(other: Any?): Boolean {
90+
if (this == other) return true
91+
if (javaClass != other?.javaClass) return false
92+
other as TalonFXItem
93+
if (deviceId != other.deviceId) return false
94+
return true
95+
}
96+
97+
override fun hashCode() = deviceId
98+
}

‎src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/TalonItem.kt‎ renamed to ‎src/main/kotlin/org/strykeforce/thirdcoast/telemetry/item/TalonSRXItem.kt‎

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,8 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRX
44
import java.util.function.DoubleSupplier
55

66
internal const val CLOSED_LOOP_TARGET = "CLOSED_LOOP_TARGET"
7-
internal const val OUTPUT_CURRENT = "OUTPUT_CURRENT"
7+
internal const val STATOR_CURRENT = "STATOR_CURRENT"
8+
internal const val SUPPLY_CURRENT = "SUPPLY_CURRENT"
89
internal const val OUTPUT_VOLTAGE = "OUTPUT_VOLTAGE"
910
internal const val OUTPUT_PERCENT = "OUTPUT_PERCENT"
1011
internal const val SELECTED_SENSOR_POSITION = "SELECTED_SENSOR_POSITION"
@@ -30,7 +31,7 @@ internal const val REVERSE_LIMIT_SWITCH_CLOSED = "REVERSE_LIMIT_SWITCH_CLOSED"
3031
internal const val TEMPERATURE = "TEMPERATURE"
3132

3233
/** Represents a [TalonSRX] telemetry-enable `Measurable` item. */
33-
class TalonItem @JvmOverloads constructor(
34+
class TalonSRXItem @JvmOverloads constructor(
3435
private val talon: TalonSRX,
3536
override val description: String = "TalonSRX ${talon.deviceID}"
3637
) : Measurable {
@@ -39,7 +40,8 @@ class TalonItem @JvmOverloads constructor(
3940
override val type = "talon"
4041
override val measures = setOf(
4142
Measure(CLOSED_LOOP_TARGET, "Closed-loop Setpoint (PID 0)"),
42-
Measure(OUTPUT_CURRENT, "Output Current"),
43+
Measure(org.strykeforce.thirdcoast.talon.STATOR_CURRENT, "Stator Current"),
44+
Measure(org.strykeforce.thirdcoast.talon.SUPPLY_CURRENT, "Supply Current"),
4345
Measure(OUTPUT_VOLTAGE, "Output Voltage"),
4446
Measure(OUTPUT_PERCENT, "Output Percentage"),
4547
Measure(SELECTED_SENSOR_POSITION, "Selected Sensor Position (PID 0)"),
@@ -72,7 +74,8 @@ class TalonItem @JvmOverloads constructor(
7274
override fun measurementFor(measure: Measure): DoubleSupplier {
7375
return when (measure.name) {
7476
CLOSED_LOOP_TARGET -> DoubleSupplier { talon.getClosedLoopTarget(0) }
75-
OUTPUT_CURRENT -> DoubleSupplier { talon.outputCurrent }
77+
org.strykeforce.thirdcoast.talon.STATOR_CURRENT -> DoubleSupplier { talon.statorCurrent }
78+
org.strykeforce.thirdcoast.talon.SUPPLY_CURRENT -> DoubleSupplier { talon.supplyCurrent }
7679
OUTPUT_VOLTAGE -> DoubleSupplier { talon.motorOutputVoltage }
7780
OUTPUT_PERCENT -> DoubleSupplier { talon.motorOutputPercent }
7881
SELECTED_SENSOR_POSITION -> DoubleSupplier { talon.getSelectedSensorPosition(0).toDouble() }
@@ -106,7 +109,7 @@ class TalonItem @JvmOverloads constructor(
106109
if (this === other) return true
107110
if (javaClass != other?.javaClass) return false
108111

109-
other as TalonItem
112+
other as TalonSRXItem
110113

111114
if (deviceId != other.deviceId) return false
112115

0 commit comments

Comments
 (0)