Skip to content

Commit 2c10129

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #117 from strykeforce/fxs-healthcheck
Make Health Check work for FXS
2 parents 34083e5 + 48a5728 commit 2c10129

File tree

7 files changed

+456
-5
lines changed

7 files changed

+456
-5
lines changed

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ plugins {
1111
}
1212

1313
group = "org.strykeforce"
14-
version = "25.0.5"
14+
version = "25.0.11"
1515

1616
java {
1717
targetCompatibility = JavaVersion.VERSION_17

‎src/main/java/org/strykeforce/swerve/FXSwerveModule.java‎

Lines changed: 39 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -168,10 +168,47 @@ public void loadAndSetAzimuthZeroReference() {
168168
logger.info("swerve module {}: azimuth absolute position = {}", index, azimuthAbsoluteCounts);
169169
double azimuthSetpoint =
170170
encoderOpposed ? reference - azimuthAbsoluteCounts : azimuthAbsoluteCounts - reference;
171-
azimuthTalon.setPosition(azimuthSetpoint);
172-
logger.info("swerve module {}: set azimuth encoder = {}", index, azimuthSetpoint);
171+
StatusCode error = azimuthTalon.setPosition(azimuthSetpoint, 0.5);
172+
if (error != StatusCode.OK) {
173+
azimuthTalon.setPosition(azimuthSetpoint, 0.5);
174+
}
175+
double position = azimuthTalon.getPosition().getValueAsDouble();
176+
logger.info(
177+
"swerve module {}: set azimuth encoder = {}, actual = {}",
178+
index,
179+
azimuthSetpoint,
180+
position);
181+
182+
setAzimuthPosition(azimuthSetpoint);
183+
}
184+
185+
public boolean zeroAndCheck() {
186+
int index = getWheelIndex();
187+
String key = String.format("SwerveDrive/wheel.%d", index);
188+
double reference = Preferences.getDouble(key, Double.MIN_VALUE);
189+
if (reference == Double.MIN_VALUE) {
190+
logger.error("no saved azimuth zero reference for swerve module {}", index);
191+
}
192+
logger.info("swerve module {}: loaded azimuth zero reference = {}", index, reference);
193+
194+
double azimuthAbsoluteCounts = getAzimuthAbsoluteEncoderCounts();
195+
logger.info("swerve module {}: azimuth absolute position = {}", index, azimuthAbsoluteCounts);
196+
double azimuthSetpoint =
197+
encoderOpposed ? reference - azimuthAbsoluteCounts : azimuthAbsoluteCounts - reference;
198+
StatusCode error = azimuthTalon.setPosition(azimuthSetpoint, 0.5);
199+
if (error != StatusCode.OK) {
200+
azimuthTalon.setPosition(azimuthSetpoint, 0.5);
201+
}
202+
double position = azimuthTalon.getPosition().getValueAsDouble();
203+
logger.info(
204+
"swerve module {}: set azimuth encoder = {}, actual = {}",
205+
index,
206+
azimuthSetpoint,
207+
position);
173208

174209
setAzimuthPosition(azimuthSetpoint);
210+
return !(azimuthAbsoluteCounts == 1.0
211+
|| Math.abs(position - azimuthSetpoint) > azimuthAbsoluteCounts / 100);
175212
}
176213

177214
public TalonFXS getAzimuthTalon() {

‎src/main/kotlin/org/strykeforce/healthcheck/internal/Builders.kt‎

Lines changed: 142 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,11 +3,13 @@ package org.strykeforce.healthcheck.internal
33
import com.ctre.phoenix.motorcontrol.can.BaseTalon
44
import com.ctre.phoenix6.controls.Follower
55
import com.ctre.phoenix6.hardware.TalonFX
6+
import com.ctre.phoenix6.hardware.TalonFXS
67
import edu.wpi.first.wpilibj2.command.Subsystem
78
import edu.wpi.first.wpilibj2.command.SubsystemBase
89
import mu.KotlinLogging
910
import okhttp3.internal.toImmutableList
1011
import org.strykeforce.healthcheck.*
12+
import org.strykeforce.swerve.FXSwerveModule
1113
import org.strykeforce.swerve.SwerveDrive
1214
import org.strykeforce.swerve.V6TalonSwerveModule
1315
import java.lang.reflect.Field
@@ -178,7 +180,7 @@ class IOHealthCheckBuilder(private val io: Checkable) {
178180
//queue the swerveDrive field for removal in fieldsToRemove
179181
healthCheckFields.forEach {
180182
if(it.type == SwerveDrive::class.java) {
181-
healthChecks.addAll(SwerveDriveHealthCheckIOBuilder(io, it).build())
183+
healthChecks.addAll(FXSwerveDriveHealthCheckIOBuilder(io, it).build())
182184
fieldsToRemove.add(it)
183185
}
184186
}
@@ -188,7 +190,10 @@ class IOHealthCheckBuilder(private val io: Checkable) {
188190
healthCheckFields.forEach {
189191
if(it.type == TalonFX::class.java) {
190192
healthChecks.add(P6TalonHealthCheckIOBuilder(io, it).build())
191-
} else {
193+
} else if(it.type == TalonFXS::class.java) {
194+
healthChecks.add(FXSTalonHealthCheckIOBuilder(io, it).build())
195+
}
196+
else {
192197
healthChecks.add(TalonHealthCheckIOBuilder(io, it).build())
193198
}
194199
}
@@ -199,6 +204,7 @@ class IOHealthCheckBuilder(private val io: Checkable) {
199204
// remove the follower check since run during leader
200205
val followerHealthChecks = healthChecks.mapNotNull { it as? TalonFollowerHealthCheck }
201206
val p6FollowerHealthChecks = healthChecks.mapNotNull { it as? P6TalonFollowerHealthCheck }
207+
val fxsFollowerHealthChecks = healthChecks.mapNotNull { it as? FXSTalonFollowerHealthCheck }
202208
for(fhc in followerHealthChecks) {
203209
val lhc = healthChecks.mapNotNull { it as? TalonHealthCheck }.find { it.talon.deviceID == fhc.leaderId }
204210
if(lhc == null) {
@@ -222,6 +228,17 @@ class IOHealthCheckBuilder(private val io: Checkable) {
222228
}
223229
healthChecks.removeAll(p6FollowerHealthChecks)
224230

231+
for(fhc in fxsFollowerHealthChecks) {
232+
val lhc = healthChecks.mapNotNull { it as? FXSTalonHealthCheck }.find { it.talonFxs.deviceID == fhc.leaderId }
233+
if(lhc == null) {
234+
logger.error { "$io: no leader (id = ${fhc.leaderId}) found for follower (id = ${fhc.talonFxs.deviceID})" }
235+
continue
236+
}
237+
//leader is timed or position check, add follower to each child case
238+
lhc.healthChecks.map { it as FXSTalonHealthCheckCase }.forEach{it.addFollowerTalon(fhc.talonFxs)}
239+
}
240+
healthChecks.removeAll(fxsFollowerHealthChecks)
241+
225242
//gget all methods from subsystem annotated with @AfterHealthCHeck
226243
val afterHealthCheckMethods = io.afterHealthCheckAnnotatedMethods()
227244

@@ -399,6 +416,57 @@ class SwerveDriveHealthCheckIOBuilder(private val io: Checkable, private val fi
399416
}
400417
}
401418

419+
class FXSwerveDriveHealthCheckIOBuilder(private val io: Checkable, private val field: Field) {
420+
private val swerveDrive = field.get(io) as? SwerveDrive ?: throw IllegalArgumentException("$io: field '${field.name}' is not a SwerveDrive")
421+
422+
fun build(): List<HealthCheck> {
423+
val talonHealthChecks = mutableListOf<HealthCheck>()
424+
425+
var azimuthLeader: TalonFXS? = null
426+
var driveLeader: TalonFX? = null
427+
var driveLeaderId: Int ?= null
428+
var azimuthLeaderId: Int ?= null
429+
val azimuthFollowers = mutableListOf<TalonFXS>()
430+
val driveFollowers = mutableListOf<TalonFX>()
431+
432+
// extract talons from swerve drive modules and
433+
// create TalonTimedHealthChecks for leader azimuth and drive talons
434+
// create TalonFollowerHealthCheck for remainder
435+
// follow associated leaders
436+
swerveDrive.swerveModules.forEach {
437+
val module =
438+
it as? FXSwerveModule ?: throw IllegalArgumentException("$io: $it is not FXTalonSwerveModule")
439+
val azimuth = module.azimuthTalon
440+
val drive = module.driveTalon
441+
442+
if (azimuth.deviceID == AZIMUTH_LEADER_ID) {
443+
talonHealthChecks.add(FXSTalonTimedHealthCheckBuilder(azimuth).build())
444+
azimuthLeader = azimuth
445+
} else {
446+
talonHealthChecks.add(FXSTalonFollowerHealthCheck(azimuth, AZIMUTH_LEADER_ID))
447+
azimuthFollowers.add(azimuth)
448+
}
449+
450+
if (drive.deviceID == DRIVE_LEADER_ID) {
451+
talonHealthChecks.add(P6TalonTimedHealthCheckBuilder(drive).build())
452+
driveLeader = drive
453+
driveLeaderId = drive.deviceID
454+
} else {
455+
talonHealthChecks.add(P6TalonFollowerHealthCheck(drive, DRIVE_LEADER_ID))
456+
driveFollowers.add(drive)
457+
}
458+
}
459+
460+
requireNotNull(azimuthLeader) { "$io: swerve azimuth talon with id $AZIMUTH_LEADER_ID not found" }
461+
requireNotNull(driveLeader) { "$io: swerve drive talon with id $DRIVE_LEADER_ID not found" }
462+
463+
azimuthFollowers.forEach { it.setControl(Follower(azimuthLeaderId ?: AZIMUTH_LEADER_ID, false))}
464+
driveFollowers.forEach { it.setControl(Follower(driveLeaderId ?: DRIVE_LEADER_ID,false) )}
465+
466+
return talonHealthChecks
467+
}
468+
}
469+
402470
class TalonHealthCheckBuilder(private val subsystem: Subsystem, private val field: Field) {
403471

404472
val talon = field.get(subsystem) as? BaseTalon
@@ -529,6 +597,38 @@ class P6TalonHealthCheckIOBuilder(private val io: Checkable, private val field:
529597
}
530598
}
531599

600+
class FXSTalonHealthCheckIOBuilder(private val io: Checkable, private val field: Field) {
601+
val talonFxs = field.get(io) as? TalonFXS?: throw java.lang.IllegalArgumentException("$io: field '${field.name}' is not subclass of TalonFXS")
602+
603+
fun build(): FXSTalonHealthCheck {
604+
val timedAnnotation = field.getAnnotation(Timed::class.java)
605+
val positionAnnotation = field.getAnnotation(Position::class.java)
606+
val followAnnotation = field.getAnnotation(Follow::class.java)
607+
608+
if(arrayListOf(timedAnnotation, positionAnnotation, followAnnotation).filterNotNull().count() > 1)
609+
throw IllegalArgumentException("$io: only one of @Timed, @Position, or @Follow may be specified.")
610+
611+
if(timedAnnotation != null) {
612+
val builder = FXSTalonTimedHealthCheckBuilder(talonFxs)
613+
builder.percentOutput = timedAnnotation.percentOutput
614+
builder.duration = timedAnnotation.duration
615+
return builder.build()
616+
}
617+
618+
if(positionAnnotation != null) {
619+
val builder = FXSTalonPositionHealthCheckBuilder(talonFxs)
620+
builder.percentOutput = positionAnnotation.percentOutput
621+
builder.encoderChange = positionAnnotation.encoderChange
622+
return builder.build()
623+
}
624+
625+
if(followAnnotation != null) return FXSTalonFollowerHealthCheck(talonFxs, followAnnotation.leader)
626+
627+
//Default to timed if not specified
628+
return FXSTalonTimedHealthCheckBuilder(talonFxs).build()
629+
}
630+
}
631+
532632
class TalonTimedHealthCheckBuilder(val talon: BaseTalon) {
533633

534634
var percentOutput = doubleArrayOf(0.2, 1.0, -0.2, -1.0)
@@ -568,6 +668,25 @@ class P6TalonTimedHealthCheckBuilder(val talonFx: TalonFX) {
568668
}
569669
}
570670

671+
class FXSTalonTimedHealthCheckBuilder(val talonFxs: TalonFXS) {
672+
var percentOutput = doubleArrayOf(0.2, 1.0, -0.2, -1.0)
673+
var duration = 2.0
674+
var limits: DoubleArray? = null
675+
676+
fun build(): FXSTalonHealthCheck {
677+
val cases = percentOutput.let {
678+
var previousCase: FXSTalonTimedHealthCheckCase? = null
679+
it.map { pctOut ->
680+
val case = FXSTalonTimedHealthCheckCase(previousCase, talonFxs, pctOut, (duration * 1e6).toLong())
681+
previousCase = case
682+
case
683+
}
684+
}
685+
686+
return FXSTalonTimedHealthCheck(talonFxs, cases, percentOutput)
687+
}
688+
}
689+
571690

572691
class TalonPositionHealthCheckBuilder(val talon: BaseTalon) {
573692
var percentOutput = doubleArrayOf(0.25, -0.25)
@@ -610,3 +729,24 @@ class P6TalonPositionHealthCheckBuilder(var talonFx: TalonFX) {
610729
return P6TalonPositionHealthCheck(talonFx, cases, percentOutput)
611730
}
612731
}
732+
733+
class FXSTalonPositionHealthCheckBuilder(var talonFxs: TalonFXS) {
734+
var percentOutput = doubleArrayOf(0.25, -0.25)
735+
var encoderChange = 0.0
736+
var limits: DoubleArray? = null
737+
738+
fun build(): FXSTalonHealthCheck {
739+
if(encoderChange == 0.0) logger.warn { "Talon ${talonFxs.deviceID}: position health check encoderChange is zero" }
740+
741+
val cases = percentOutput.let {
742+
var previousCase: FXSTalonPositionHealthCheckCase? = null
743+
it.map { pctOut ->
744+
val case = FXSTalonPositionHealthCheckCase(previousCase, talonFxs, pctOut, encoderChange)
745+
previousCase = case
746+
case
747+
}
748+
}
749+
750+
return FXSTalonPositionHealthCheck(talonFxs, cases, percentOutput)
751+
}
752+
}

‎src/main/kotlin/org/strykeforce/healthcheck/internal/HealthCheckData.kt‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@ package org.strykeforce.healthcheck.internal
22

33
import com.ctre.phoenix.motorcontrol.can.BaseTalon
44
import com.ctre.phoenix6.hardware.TalonFX
5+
import com.ctre.phoenix6.hardware.TalonFXS
56

67
data class DiagnosticLimits(
78
val currentMin: Double = 0.0, val currentMax: Double = 0.0, val speedMin: Double = 0.0, val speedMax: Double = 0.0
@@ -94,6 +95,49 @@ class P6TalonHealthCheckData(val case: Int, private val talonFx: TalonFX) {
9495
get() = statorCurrent.average()
9596
}
9697

98+
class FXSTalonHealthCheckData(val case: Int, private val talonFxs: TalonFXS) {
99+
val deviceId = talonFxs.deviceID
100+
val timestamp : MutableList<Long> = mutableListOf()
101+
val voltage: MutableList<Double> = mutableListOf()
102+
val position: MutableList<Double> = mutableListOf()
103+
val speed: MutableList<Double> = mutableListOf()
104+
val supplyCurrent: MutableList<Double> = mutableListOf()
105+
val statorCurrent: MutableList<Double> = mutableListOf()
106+
107+
fun measure(ts: Long) {
108+
timestamp.add(ts)
109+
voltage.add(talonFxs.motorVoltage.valueAsDouble)
110+
position.add(talonFxs.position.valueAsDouble)
111+
speed.add(talonFxs.velocity.valueAsDouble)
112+
supplyCurrent.add(talonFxs.supplyCurrent.valueAsDouble)
113+
statorCurrent.add(talonFxs.statorCurrent.valueAsDouble)
114+
}
115+
116+
fun reset() {
117+
timestamp.clear()
118+
voltage.clear()
119+
position.clear()
120+
speed.clear()
121+
supplyCurrent.clear()
122+
statorCurrent.clear()
123+
}
124+
125+
val id
126+
get() = talonFxs.deviceID
127+
128+
val averageVoltage
129+
get() = voltage.average()
130+
131+
val averageSpeed
132+
get() = speed.average()
133+
134+
val averageSupplyCurrent
135+
get() = supplyCurrent.average()
136+
137+
val averageStatorCurrent
138+
get() = statorCurrent.average()
139+
}
140+
97141
internal fun DoubleArray.limitsFor(iteration: Int): DiagnosticLimits {
98142
val index = iteration * 4
99143
if (size > index + 3) return DiagnosticLimits(

0 commit comments

Comments
 (0)