From 75b7339aa3d2a9d1c580f9343b4cc122e35162b2 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sat, 10 Feb 2024 13:30:12 -0500 Subject: [PATCH 1/3] add equals to talonFX measureable --- build.gradle | 2 +- .../telemetry/measurable/TalonFXMeasureable.kt | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index a04b385..3f2e57e 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ plugins { } group = "org.strykeforce" -version = "24.0.2" +version = "24.0.3" sourceCompatibility = JavaVersion.VERSION_17 targetCompatibility = JavaVersion.VERSION_17 diff --git a/src/main/kotlin/org/strykeforce/telemetry/measurable/TalonFXMeasureable.kt b/src/main/kotlin/org/strykeforce/telemetry/measurable/TalonFXMeasureable.kt index bbe1917..0259a50 100644 --- a/src/main/kotlin/org/strykeforce/telemetry/measurable/TalonFXMeasureable.kt +++ b/src/main/kotlin/org/strykeforce/telemetry/measurable/TalonFXMeasureable.kt @@ -149,4 +149,15 @@ class TalonFXMeasureable @JvmOverloads constructor( Measure(DIFF_CLOSED_LOOP_REF_SLOPE, "Differential Closed Loop Reference Slope"){talonFX.differentialClosedLoopReferenceSlope.valueAsDouble}, Measure(DIFF_CLOSED_LOOP_SLOT, "Differential Closed Loop Slot"){talonFX.differentialClosedLoopSlot.valueAsDouble}, ) + + override fun equals(other: Any?) : Boolean { + if(this === other) return true + if(javaClass != other?.javaClass) return false + + other as TalonFXMeasureable + if(deviceId != other.deviceId) return false + return true + } + + override fun hashCode() = deviceId } \ No newline at end of file From 9045cd6a50cdb28ac1cae7e2891e30426648c8a7 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Mon, 12 Feb 2024 18:06:33 -0500 Subject: [PATCH 2/3] fix comparison on fault fx measure --- .../telemetry/measurable/TalonFXFaultMeasureable.kt | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/main/kotlin/org/strykeforce/telemetry/measurable/TalonFXFaultMeasureable.kt b/src/main/kotlin/org/strykeforce/telemetry/measurable/TalonFXFaultMeasureable.kt index 66704b5..f6299ac 100644 --- a/src/main/kotlin/org/strykeforce/telemetry/measurable/TalonFXFaultMeasureable.kt +++ b/src/main/kotlin/org/strykeforce/telemetry/measurable/TalonFXFaultMeasureable.kt @@ -100,4 +100,15 @@ class TalonFXFaultMeasureable @JvmOverloads constructor( Measure(STICKY_FAULT_UNSTABLE_SUPPV, "Sticky: Unstable Supply Voltage"){talonFX.stickyFault_UnstableSupplyV.valueAsDouble}, Measure(STICKY_FAULT_FUSED_NO_LIC, "Sticky: Fused Sensor No License"){talonFX.stickyFault_UsingFusedCANcoderWhileUnlicensed.valueAsDouble} ) + + override fun equals(other: Any?) : Boolean { + if(this === other) return true + if(javaClass != other?.javaClass) return false + + other as TalonFXFaultMeasureable + if(deviceId != other.deviceId) return false + return true + } + + override fun hashCode() = deviceId } \ No newline at end of file From facf988f1bb9a47d954a145f16577ab7b8152ea0 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sun, 18 Feb 2024 18:53:46 -0500 Subject: [PATCH 3/3] update healthcheck for phoenix6 not tested --- .../swerve/V6TalonSwerveModule.java | 15 +- .../strykeforce/healthcheck/Annotations.kt | 2 +- .../org/strykeforce/healthcheck/Checkable.kt | 8 + .../healthcheck/HealthCheckCommand.kt | 53 ++- .../healthcheck/internal/Builders.kt | 358 +++++++++++++++++- .../healthcheck/internal/HealthCheckData.kt | 44 +++ .../healthcheck/internal/HealthChecks.kt | 199 ++++++++++ .../healthcheck/internal/ReportServer.kt | 118 ++++++ .../healthcheck/internal/Visitors.kt | 38 ++ .../healthcheck/AnnotationsTest.kt | 4 +- .../strykeforce/healthcheck/TestSubsystem.kt | 4 +- .../swerve/TalonSwerveModuleTest.java | 16 + 12 files changed, 836 insertions(+), 23 deletions(-) create mode 100644 src/main/kotlin/org/strykeforce/healthcheck/Checkable.kt diff --git a/src/main/java/org/strykeforce/swerve/V6TalonSwerveModule.java b/src/main/java/org/strykeforce/swerve/V6TalonSwerveModule.java index a80126c..b63aae8 100644 --- a/src/main/java/org/strykeforce/swerve/V6TalonSwerveModule.java +++ b/src/main/java/org/strykeforce/swerve/V6TalonSwerveModule.java @@ -68,6 +68,8 @@ public class V6TalonSwerveModule implements SwerveModule { private int closedLoopSlot; + private boolean compensateLatency; + private V6TalonSwerveModule(V6Builder builder) { azimuthTalon = builder.azimuthTalon; @@ -82,6 +84,7 @@ private V6TalonSwerveModule(V6Builder builder) { units = builder.units; enableFOC = builder.enableFOC; closedLoopSlot = builder.closedLoopSlot; + compensateLatency = builder.compensateLatency; resetDriveEncoder(); } @@ -223,11 +226,14 @@ private double getDriveMetersPerSecond() { } private double getDrivePositionMeters() { + double latency = driveTalon.getPosition().getTimestamp().getLatency(); double shaftPosition = driveTalon.getPosition().getValue(); // rotations double motorPosition = shaftPosition / driveCountsPerRev; // default = 1.0 for counts double wheelPosition = motorPosition * driveGearRatio; double wheelPositionMeters = wheelPosition * wheelCircumferenceMeters; - return wheelPositionMeters; + double velocityMetersPerSecond = getDriveMetersPerSecond(); + double wheelPositionMetersComp = wheelPositionMeters + latency * velocityMetersPerSecond; + return compensateLatency ? wheelPositionMetersComp : wheelPositionMeters; } private void setDriveClosedLoopMetersPerSecond(double metersPerSecond) { @@ -303,6 +309,8 @@ public static class V6Builder { private int closedLoopSlot = 0; + private boolean compensateLatency = false; + public V6Builder() {} public V6Builder azimuthTalon(TalonSRX azimuthTalon) { @@ -365,6 +373,11 @@ public V6Builder closedLoopSlot(int slot) { return this; } + public V6Builder latencyCompensation(boolean compensate) { + this.compensateLatency = compensate; + return this; + } + public V6TalonSwerveModule build() { if (driveDeadbandMetersPerSecond < 0) { driveDeadbandMetersPerSecond = 0.01 * driveMaximumMetersPerSecond; diff --git a/src/main/kotlin/org/strykeforce/healthcheck/Annotations.kt b/src/main/kotlin/org/strykeforce/healthcheck/Annotations.kt index 77fdfa2..3659740 100644 --- a/src/main/kotlin/org/strykeforce/healthcheck/Annotations.kt +++ b/src/main/kotlin/org/strykeforce/healthcheck/Annotations.kt @@ -6,7 +6,7 @@ import com.ctre.phoenix.motorcontrol.can.BaseTalon annotation class HealthCheck(val order: Int = 0) @Target(AnnotationTarget.FIELD) -annotation class Position(val percentOutput: DoubleArray, val encoderChange: Int) +annotation class Position(val percentOutput: DoubleArray, val encoderChange: Double) @Target(AnnotationTarget.FIELD) annotation class Timed(val percentOutput: DoubleArray, val duration: Double = 5.0) diff --git a/src/main/kotlin/org/strykeforce/healthcheck/Checkable.kt b/src/main/kotlin/org/strykeforce/healthcheck/Checkable.kt new file mode 100644 index 0000000..031aa0c --- /dev/null +++ b/src/main/kotlin/org/strykeforce/healthcheck/Checkable.kt @@ -0,0 +1,8 @@ +package org.strykeforce.healthcheck + +interface Checkable { + + fun getName(): String + + +} \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/healthcheck/HealthCheckCommand.kt b/src/main/kotlin/org/strykeforce/healthcheck/HealthCheckCommand.kt index cb51d52..ec50007 100644 --- a/src/main/kotlin/org/strykeforce/healthcheck/HealthCheckCommand.kt +++ b/src/main/kotlin/org/strykeforce/healthcheck/HealthCheckCommand.kt @@ -3,11 +3,9 @@ package org.strykeforce.healthcheck import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem import edu.wpi.first.wpilibj2.command.button.InternalButton +import kotlinx.html.BUTTON import mu.KotlinLogging -import org.strykeforce.healthcheck.internal.DumpVisitor -import org.strykeforce.healthcheck.internal.ReportServer -import org.strykeforce.healthcheck.internal.RobotHealthCheck -import org.strykeforce.healthcheck.internal.RobotHealthCheckBuilder +import org.strykeforce.healthcheck.internal.* private val logger = KotlinLogging.logger {} @@ -52,5 +50,52 @@ class HealthCheckCommand(vararg subsystems: Subsystem) : Command() { isFinished = false } + override fun runsWhenDisabled() = true +} + +class IOHealthCheckCommand(var requirements: List, vararg ios: Checkable): Command() { + + companion object { + @JvmField + val BUTTON = InternalButton() + } + + private val robotHealthCheckBuilder = RobotHealCheckIOBuilder(*ios) + private lateinit var robotHealthCheck: RobotIOHealthCheck + + private var isFinished: Boolean = false + + private var reportServer: ReportServerIO? = null + + private val ioSet = ios.toSet() + + private val requirementsSet = requirements.toSet() + + override fun getRequirements() = requirementsSet + + override fun initialize() { + robotHealthCheck = robotHealthCheckBuilder.build() + robotHealthCheck.initialize() + BUTTON.setPressed(false) + } + + override fun execute() { + if(robotHealthCheck.isFinished) { + isFinished = true + return + } + robotHealthCheck.execute() + } + + override fun isFinished() = isFinished + + override fun end(interrupted: Boolean) { + reportServer?.stop() + reportServer = ReportServerIO(robotHealthCheck) + + DumpVisitor().visit(robotHealthCheck) + isFinished = false + } + override fun runsWhenDisabled() = true } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/healthcheck/internal/Builders.kt b/src/main/kotlin/org/strykeforce/healthcheck/internal/Builders.kt index f4e8084..9cc55f9 100644 --- a/src/main/kotlin/org/strykeforce/healthcheck/internal/Builders.kt +++ b/src/main/kotlin/org/strykeforce/healthcheck/internal/Builders.kt @@ -1,17 +1,19 @@ package org.strykeforce.healthcheck.internal import com.ctre.phoenix.motorcontrol.can.BaseTalon +import com.ctre.phoenix6.controls.Follower +import com.ctre.phoenix6.hardware.TalonFX import edu.wpi.first.wpilibj2.command.Subsystem import edu.wpi.first.wpilibj2.command.SubsystemBase import mu.KotlinLogging import okhttp3.internal.toImmutableList -import org.strykeforce.healthcheck.Follow -import org.strykeforce.healthcheck.Position -import org.strykeforce.healthcheck.Timed +import org.strykeforce.healthcheck.* import org.strykeforce.swerve.SwerveDrive import org.strykeforce.swerve.V5TalonSwerveModule +import org.strykeforce.swerve.V6TalonSwerveModule import java.lang.reflect.Field import java.lang.reflect.Method +import kotlin.math.log private val logger = KotlinLogging.logger {} @@ -25,6 +27,15 @@ class RobotHealthCheckBuilder(vararg subsystems: Subsystem) { } } +class RobotHealCheckIOBuilder(vararg ios: Checkable) { + private val ioSet = ios.toList() + + fun build(): RobotIOHealthCheck { + val ioHealthChecks = ioSet.map { IOHealthCheckBuilder(it).build() } + return RobotIOHealthCheck("Robot Health Check", ioHealthChecks) + } +} + private val kHealthCheckAnnotationClass = org.strykeforce.healthcheck.HealthCheck::class.java private val kBeforeHealthCheckAnnotationClass = org.strykeforce.healthcheck.BeforeHealthCheck::class.java private val kAfterHealthCheckAnnotationClass = org.strykeforce.healthcheck.AfterHealthCheck::class.java @@ -36,18 +47,31 @@ private fun Subsystem.healthCheckAnnotatedFields() = .sortedBy { it.getAnnotation(kHealthCheckAnnotationClass).order } .toMutableList() +private fun Checkable.healthCheckAnnotatedFields() = + this.javaClass.declaredFields.filter { it.isAnnotationPresent(kHealthCheckAnnotationClass) } + .sortedBy { it.getAnnotation(kHealthCheckAnnotationClass).order } + .toMutableList() + /** Get methods in subsystem that are annotated with `@BeforeHealthCheck`. The methods are sorted in order * of the method's name. */ private fun Subsystem.beforeHealthCheckAnnotatedMethods() = this.javaClass.declaredMethods.filter { it.isAnnotationPresent(kBeforeHealthCheckAnnotationClass) } .sortedBy { it.getAnnotation(kBeforeHealthCheckAnnotationClass).order } +private fun Checkable.beforeHealthCheckAnnotatedMethods() = + this.javaClass.declaredMethods.filter { it.isAnnotationPresent(kBeforeHealthCheckAnnotationClass) } + .sortedBy { it.getAnnotation(kBeforeHealthCheckAnnotationClass).order } + /** Get methods in subsystem that are annotated with `@AfterHealthCheck`. The methods are sorted in order * of the method's name. */ private fun Subsystem.afterHealthCheckAnnotatedMethods() = this.javaClass.declaredMethods.filter { it.isAnnotationPresent(kAfterHealthCheckAnnotationClass) } .sortedBy { it.getAnnotation(kAfterHealthCheckAnnotationClass).order } +private fun Checkable.afterHealthCheckAnnotatedMethods() = + this.javaClass.declaredMethods.filter { it.isAnnotationPresent(kAfterHealthCheckAnnotationClass) } + .sortedBy { it.getAnnotation(kAfterHealthCheckAnnotationClass).order } + class SubsystemHealthCheckBuilder(private val subsystem: Subsystem) { fun build(): SubsystemHealthCheck { @@ -87,7 +111,14 @@ class SubsystemHealthCheckBuilder(private val subsystem: Subsystem) { // remove all non-talon fields and create TalonHealthChecks from the remaining talon fields healthCheckFields.removeAll(fieldsToRemove) - healthChecks.addAll(healthCheckFields.map { TalonHealthCheckBuilder(subsystem, it).build() }) + healthCheckFields.forEach { + if(it.type == TalonFX::class.java) { + healthChecks.add(P6TalonHealthCheckBuilder(subsystem,it).build()) + } else { + healthChecks.add(TalonHealthCheckBuilder(subsystem,it).build()) + } + } +// healthChecks.addAll(healthCheckFields.map { TalonHealthCheckBuilder(subsystem, it).build() }) if (healthChecks.isEmpty()) logger.warn { "$subsystem: no health checks found" } // find any follower talon health checks and @@ -118,6 +149,91 @@ class SubsystemHealthCheckBuilder(private val subsystem: Subsystem) { } } +class IOHealthCheckBuilder(private val io: Checkable) { + fun build(): IOHealthCheck { + val healthChecks = mutableListOf() + + //get all methods from this io layer annotated with @BeforeHealthCheck + val beforeHealthCHeckMethods = io.beforeHealthCheckAnnotatedMethods() + + //create lifecyclehealthchecks from annotated methods and prepend them to health checks for this io layer + healthChecks.addAll(beforeHealthCHeckMethods.map { LifecycleHealthCheckIOBuilder(io, it).build() }) + + //get all fields from this subsystem with that are annotated with @HealthCheck + val healthCheckFields = io.healthCheckAnnotatedFields() + + //remove non-talon field(s), only expect swerve drive field + val fieldsToRemove = mutableSetOf() + + //make each annotated field accessible or remove it + healthCheckFields.forEach { + if(!it.trySetAccessible()) { + logger.error { "$io: trySetAccessible() failed for ${it.name}, removing!" } + fieldsToRemove.add(it) + } + } + healthCheckFields.removeAll(fieldsToRemove) + + //If swerve drive field annotated with HealthCheck, + //create TalonHealthCheck for each azimuth and drive talon and add to talonHealthChecks list + //queue the swerveDrive field for removal in fieldsToRemove + healthCheckFields.forEach { + if(it.type == SwerveDrive::class.java) { + healthChecks.addAll(SwerveDriveHealthCheckIOBuilder(io, it).build()) + fieldsToRemove.add(it) + } + } + + //remove all non-talon fields and create TalonHealthChecks from the remaining talon fields + healthCheckFields.removeAll(fieldsToRemove) + healthCheckFields.forEach { + if(it.type == TalonFX::class.java) { + healthChecks.add(P6TalonHealthCheckIOBuilder(io, it).build()) + } else { + healthChecks.add(TalonHealthCheckIOBuilder(io, it).build()) + } + } + // healthChecks.addAll(healthCheckFields.map { TalonHealthCheckIOBuilder(io, it).build() }) + if(healthCheckFields.isEmpty()) logger.warn { "$io: no health checks found" } + + // find any follower checks and add talon to be measured to their associated leader check + // remove the follower check since run during leader + val followerHealthChecks = healthChecks.mapNotNull { it as? TalonFollowerHealthCheck } + val p6FollowerHealthChecks = healthChecks.mapNotNull { it as? P6TalonFollowerHealthCheck } + for(fhc in followerHealthChecks) { + val lhc = healthChecks.mapNotNull { it as? TalonHealthCheck }.find { it.talon.deviceID == fhc.leaderId } + if(lhc == null) { + logger.error { "$io: no leader(id = ${fhc.leaderId}) found for follower (id = ${fhc.talon.deviceID})" } + continue + } + //leader health check is TalonTimedHealthCheck or PositionTimedHealthCheck + //add follower talon to each child case + lhc.healthChecks.map { it as TalonHealthCheckCase }.forEach{it.addFollowerTalon(fhc.talon)} + } + healthChecks.removeAll(followerHealthChecks) + + for(fhc in p6FollowerHealthChecks) { + val lhc = healthChecks.mapNotNull { it as? P6TalonHealthCheck }.find { it.talonFx.deviceID == fhc.leaderId } + if(lhc == null) { + logger.error { "$io: no leader (id = ${fhc.leaderId}) found for follower (id = ${fhc.talonFx.deviceID})" } + continue + } + //leader is timed or position check, add follower to each child case + lhc.healthChecks.map { it as P6TalonHealthCheckCase }.forEach{it.addFollowerTalon(fhc.talonFx)} + } + healthChecks.removeAll(p6FollowerHealthChecks) + + //gget all methods from subsystem annotated with @AfterHealthCHeck + val afterHealthCheckMethods = io.afterHealthCheckAnnotatedMethods() + + //create lifecycleHealthCheck from annotated methods and append them to health checks for this io layer + healthChecks.addAll(afterHealthCheckMethods.map { LifecycleHealthCheckIOBuilder(io, it).build() }) + + val name = (io as? Checkable)?.getName() ?: io.toString() + return IOHealthCheck(name, healthChecks.toImmutableList()) + } +} + class LifecycleHealthCheckBuilder(private val subsystem: Subsystem, private val method: Method) { fun build(): HealthCheck { val subsystemName = (subsystem as? SubsystemBase)?.name ?: subsystem.toString() @@ -150,6 +266,35 @@ class LifecycleHealthCheckBuilder(private val subsystem: Subsystem, private val } } +class LifecycleHealthCheckIOBuilder(private val io: Checkable, private val method: Method) { + fun build(): HealthCheck { + val ioName = io.getName() + + //throw exception if method annotated with @BeforeHealthCheck is inaccessible + //rather crach the robot program than run improper health check + if(!method.trySetAccessible()) { + val msg = "$ioName.${method.name}: inaccessible, crashing to prevent health check running without setup" + logger.error { msg } + throw java.lang.IllegalStateException(msg) + } + + //methods annotated with @BeforeHealthCheck should have no params and return a boolean + if(method.parameterCount != 0) { + val msg = "$ioName.${method.name}: should have no parameters" + logger.error { msg } + throw java.lang.IllegalArgumentException(msg) + } + + if(method.returnType != Boolean::class.java) { + val msg = "$ioName.${method.name}: should return a boolean" + logger.error { msg } + throw java.lang.IllegalArgumentException(msg) + } + + return LifecycleIOHealthCheck(io, method) + } +} + private const val AZIMUTH_LEADER_ID = 0 private const val DRIVE_LEADER_ID = 10 @@ -158,13 +303,14 @@ class SwerveDriveHealthCheckBuilder(private val subsystem: Subsystem, private va private val swerveDrive = field.get(subsystem) as? SwerveDrive ?: throw IllegalArgumentException("$subsystem: field '${field.name}' is not a SwerveDrive") - fun build(): List { - val talonHealthChecks = mutableListOf() + fun build(): List { + val talonHealthChecks = mutableListOf() var azimuthLeader: BaseTalon? = null - var driveLeader: BaseTalon? = null + var driveLeader: TalonFX? = null + var driveLeaderId: Int ?= null val azimuthFollowers = mutableListOf() - val driveFollowers = mutableListOf() + val driveFollowers = mutableListOf() // extract talons from swerve drive modules and // create TalonTimedHealthChecks for leader azimuth and drive talons @@ -172,7 +318,7 @@ class SwerveDriveHealthCheckBuilder(private val subsystem: Subsystem, private va // follow associated leaders swerveDrive.swerveModules.forEach { val module = - it as? V5TalonSwerveModule ?: throw IllegalArgumentException("$subsystem: $it is not TalonSwerveModule") + it as? V6TalonSwerveModule ?: throw IllegalArgumentException("$subsystem: $it is not TalonSwerveModule") val azimuth = module.azimuthTalon val drive = module.driveTalon @@ -185,10 +331,11 @@ class SwerveDriveHealthCheckBuilder(private val subsystem: Subsystem, private va } if (drive.deviceID == DRIVE_LEADER_ID) { - talonHealthChecks.add(TalonTimedHealthCheckBuilder(drive).build()) + talonHealthChecks.add(P6TalonTimedHealthCheckBuilder(drive).build()) driveLeader = drive + driveLeaderId = drive.deviceID } else { - talonHealthChecks.add(TalonFollowerHealthCheck(drive, DRIVE_LEADER_ID)) + talonHealthChecks.add(P6TalonFollowerHealthCheck(drive, DRIVE_LEADER_ID)) driveFollowers.add(drive) } } @@ -197,7 +344,57 @@ class SwerveDriveHealthCheckBuilder(private val subsystem: Subsystem, private va requireNotNull(driveLeader) { "$subsystem: swerve drive talon with id $DRIVE_LEADER_ID not found" } azimuthFollowers.forEach { it.follow(azimuthLeader) } - driveFollowers.forEach { it.follow(driveLeader) } + driveFollowers.forEach { it.setControl(Follower(driveLeaderId ?: DRIVE_LEADER_ID,false) )} + + return talonHealthChecks + } +} + +class SwerveDriveHealthCheckIOBuilder(private val io: Checkable, private val field: Field) { + private val swerveDrive = field.get(io) as? SwerveDrive ?: throw IllegalArgumentException("$io: field '${field.name}' is not a SwerveDrive") + + fun build(): List { + val talonHealthChecks = mutableListOf() + + var azimuthLeader: BaseTalon? = null + var driveLeader: TalonFX? = null + var driveLeaderId: Int ?= null + val azimuthFollowers = mutableListOf() + val driveFollowers = mutableListOf() + + // extract talons from swerve drive modules and + // create TalonTimedHealthChecks for leader azimuth and drive talons + // create TalonFollowerHealthCheck for remainder + // follow associated leaders + swerveDrive.swerveModules.forEach { + val module = + it as? V6TalonSwerveModule ?: throw IllegalArgumentException("$io: $it is not TalonSwerveModule") + val azimuth = module.azimuthTalon + val drive = module.driveTalon + + if (azimuth.deviceID == AZIMUTH_LEADER_ID) { + talonHealthChecks.add(TalonTimedHealthCheckBuilder(azimuth).build()) + azimuthLeader = azimuth + } else { + talonHealthChecks.add(TalonFollowerHealthCheck(azimuth, AZIMUTH_LEADER_ID)) + azimuthFollowers.add(azimuth) + } + + if (drive.deviceID == DRIVE_LEADER_ID) { + talonHealthChecks.add(P6TalonTimedHealthCheckBuilder(drive).build()) + driveLeader = drive + driveLeaderId = drive.deviceID + } else { + talonHealthChecks.add(P6TalonFollowerHealthCheck(drive, DRIVE_LEADER_ID)) + driveFollowers.add(drive) + } + } + + requireNotNull(azimuthLeader) { "$io: swerve azimuth talon with id $AZIMUTH_LEADER_ID not found" } + requireNotNull(driveLeader) { "$io: swerve drive talon with id $DRIVE_LEADER_ID not found" } + + azimuthFollowers.forEach { it.follow(azimuthLeader) } + driveFollowers.forEach { it.setControl(Follower(driveLeaderId ?: DRIVE_LEADER_ID,false) )} return talonHealthChecks } @@ -227,7 +424,7 @@ class TalonHealthCheckBuilder(private val subsystem: Subsystem, private val fiel if (positionAnnotation != null) { val builder = TalonPositionHealthCheckBuilder(talon) builder.percentOutput = positionAnnotation.percentOutput - builder.encoderChange = positionAnnotation.encoderChange + builder.encoderChange = positionAnnotation.encoderChange.toInt() return builder.build() } @@ -238,6 +435,101 @@ class TalonHealthCheckBuilder(private val subsystem: Subsystem, private val fiel } } +class TalonHealthCheckIOBuilder(private val io: Checkable, private val field: Field) { + val talon = field.get(io) as? BaseTalon ?: throw IllegalArgumentException("$io: field '${field.name}' is not a subclass of BaseTalon") + + fun build(): TalonHealthCheck { + val timedAnnotation = field.getAnnotation(Timed::class.java) + val positionAnnotation = field.getAnnotation(Position::class.java) + val followAnnotation = field.getAnnotation(Follow::class.java) + + if(arrayListOf(timedAnnotation, positionAnnotation, followAnnotation).filterNotNull().count() > 1) throw IllegalArgumentException("$io: only one of @Timed, @Position, or @Follow may be specified.") + + if(timedAnnotation != null) { + val builder = TalonTimedHealthCheckBuilder(talon) + builder.percentOutput = timedAnnotation.percentOutput + builder.duration = timedAnnotation.duration + return builder.build() + } + + if(positionAnnotation != null) { + val builder = TalonPositionHealthCheckBuilder(talon) + builder.percentOutput = positionAnnotation.percentOutput + builder.encoderChange = positionAnnotation.encoderChange.toInt() + return builder.build() + } + + if(followAnnotation != null) return TalonFollowerHealthCheck(talon, followAnnotation.leader) + + //default to timed if not specified + return TalonTimedHealthCheckBuilder(talon).build() + } +} + +class P6TalonHealthCheckBuilder(private val subsystem: Subsystem, private val field: Field) { + val talonFx = field.get(subsystem) as? TalonFX?: throw java.lang.IllegalArgumentException("$subsystem: field '${field.name}' is not subclass of TalonFX") + + fun build(): P6TalonHealthCheck { + val timedAnnotation = field.getAnnotation(Timed::class.java) + val positionAnnotation = field.getAnnotation(Position::class.java) + val followAnnotation = field.getAnnotation(Follow::class.java) + + if(arrayListOf(timedAnnotation, positionAnnotation, followAnnotation).filterNotNull().count() > 1) + throw IllegalArgumentException("$subsystem: only one of @Timed, @Position, or @Follow may be specified.") + + if(timedAnnotation != null) { + val builder = P6TalonTimedHealthCheckBuilder(talonFx) + builder.percentOutput = timedAnnotation.percentOutput + builder.duration = timedAnnotation.duration + return builder.build() + } + + if(positionAnnotation != null) { + val builder = P6TalonPositionHealthCheckBuilder(talonFx) + builder.percentOutput = positionAnnotation.percentOutput + builder.encoderChange = positionAnnotation.encoderChange + return builder.build() + } + + if(followAnnotation != null) return P6TalonFollowerHealthCheck(talonFx, followAnnotation.leader) + + //Default ot timed if not specified + return P6TalonTimedHealthCheckBuilder(talonFx).build() + } +} + +class P6TalonHealthCheckIOBuilder(private val io: Checkable, private val field: Field) { + val talonFx = field.get(io) as? TalonFX?: throw java.lang.IllegalArgumentException("$io: field '${field.name}' is not subclass of TalonFX") + + fun build(): P6TalonHealthCheck { + val timedAnnotation = field.getAnnotation(Timed::class.java) + val positionAnnotation = field.getAnnotation(Position::class.java) + val followAnnotation = field.getAnnotation(Follow::class.java) + + if(arrayListOf(timedAnnotation, positionAnnotation, followAnnotation).filterNotNull().count() > 1) + throw IllegalArgumentException("$io: only one of @Timed, @Position, or @Follow may be specified.") + + if(timedAnnotation != null) { + val builder = P6TalonTimedHealthCheckBuilder(talonFx) + builder.percentOutput = timedAnnotation.percentOutput + builder.duration = timedAnnotation.duration + return builder.build() + } + + if(positionAnnotation != null) { + val builder = P6TalonPositionHealthCheckBuilder(talonFx) + builder.percentOutput = positionAnnotation.percentOutput + builder.encoderChange = positionAnnotation.encoderChange + return builder.build() + } + + if(followAnnotation != null) return P6TalonFollowerHealthCheck(talonFx, followAnnotation.leader) + + //Default to timed if not specified + return P6TalonTimedHealthCheckBuilder(talonFx).build() + } +} + class TalonTimedHealthCheckBuilder(val talon: BaseTalon) { var percentOutput = doubleArrayOf(0.2, 1.0, -0.2, -1.0) @@ -258,6 +550,25 @@ class TalonTimedHealthCheckBuilder(val talon: BaseTalon) { } } +class P6TalonTimedHealthCheckBuilder(val talonFx: TalonFX) { + var percentOutput = doubleArrayOf(0.2, 1.0, -0.2, -1.0) + var duration = 2.0 + var limits: DoubleArray? = null + + fun build(): P6TalonHealthCheck { + val cases = percentOutput.let { + var previousCase: P6TalonTimedHealthCheckCase? = null + it.map { pctOut -> + val case = P6TalonTimedHealthCheckCase(previousCase, talonFx, pctOut, (duration * 1e6).toLong()) + previousCase = case + case + } + } + + return P6TalonTimedHealthCheck(talonFx, cases, percentOutput) + } +} + class TalonPositionHealthCheckBuilder(val talon: BaseTalon) { var percentOutput = doubleArrayOf(0.25, -0.25) @@ -279,3 +590,24 @@ class TalonPositionHealthCheckBuilder(val talon: BaseTalon) { return TalonPositionHealthCheck(talon, cases, percentOutput) } } + +class P6TalonPositionHealthCheckBuilder(var talonFx: TalonFX) { + var percentOutput = doubleArrayOf(0.25, -0.25) + var encoderChange = 0.0 + var limits: DoubleArray? = null + + fun build(): P6TalonHealthCheck { + if(encoderChange == 0.0) logger.warn { "Talon ${talonFx.deviceID}: position health check encoderChange is zero" } + + val cases = percentOutput.let { + var previousCase: P6TalonPositionHealthCheckCase? = null + it.map { pctOut -> + val case = P6TalonPositionHealthCheckCase(previousCase, talonFx, pctOut, encoderChange) + previousCase = case + case + } + } + + return P6TalonPositionHealthCheck(talonFx, cases, percentOutput) + } +} diff --git a/src/main/kotlin/org/strykeforce/healthcheck/internal/HealthCheckData.kt b/src/main/kotlin/org/strykeforce/healthcheck/internal/HealthCheckData.kt index 1a07b34..2ac09ce 100644 --- a/src/main/kotlin/org/strykeforce/healthcheck/internal/HealthCheckData.kt +++ b/src/main/kotlin/org/strykeforce/healthcheck/internal/HealthCheckData.kt @@ -1,6 +1,7 @@ package org.strykeforce.healthcheck.internal import com.ctre.phoenix.motorcontrol.can.BaseTalon +import com.ctre.phoenix6.hardware.TalonFX data class DiagnosticLimits( val currentMin: Double = 0.0, val currentMax: Double = 0.0, val speedMin: Double = 0.0, val speedMax: Double = 0.0 @@ -50,6 +51,49 @@ class TalonHealthCheckData(val case: Int, private val talon: BaseTalon) { } +class P6TalonHealthCheckData(val case: Int, private val talonFx: TalonFX) { + val deviceId = talonFx.deviceID + val timestamp : MutableList = mutableListOf() + val voltage: MutableList = mutableListOf() + val position: MutableList = mutableListOf() + val speed: MutableList = mutableListOf() + val supplyCurrent: MutableList = mutableListOf() + val statorCurrent: MutableList = mutableListOf() + + fun measure(ts: Long) { + timestamp.add(ts) + voltage.add(talonFx.motorVoltage.valueAsDouble) + position.add(talonFx.position.valueAsDouble) + speed.add(talonFx.velocity.valueAsDouble) + supplyCurrent.add(talonFx.supplyCurrent.valueAsDouble) + statorCurrent.add(talonFx.statorCurrent.valueAsDouble) + } + + fun reset() { + timestamp.clear() + voltage.clear() + position.clear() + speed.clear() + supplyCurrent.clear() + statorCurrent.clear() + } + + val id + get() = talonFx.deviceID + + val averageVoltage + get() = voltage.average() + + val averageSpeed + get() = speed.average() + + val averageSupplyCurrent + get() = supplyCurrent.average() + + val averageStatorCurrent + get() = statorCurrent.average() +} + internal fun DoubleArray.limitsFor(iteration: Int): DiagnosticLimits { val index = iteration * 4 if (size > index + 3) return DiagnosticLimits( diff --git a/src/main/kotlin/org/strykeforce/healthcheck/internal/HealthChecks.kt b/src/main/kotlin/org/strykeforce/healthcheck/internal/HealthChecks.kt index 510c088..1c80c07 100644 --- a/src/main/kotlin/org/strykeforce/healthcheck/internal/HealthChecks.kt +++ b/src/main/kotlin/org/strykeforce/healthcheck/internal/HealthChecks.kt @@ -2,10 +2,14 @@ package org.strykeforce.healthcheck.internal import com.ctre.phoenix.motorcontrol.ControlMode import com.ctre.phoenix.motorcontrol.can.BaseTalon +import com.ctre.phoenix6.controls.DutyCycleOut +import com.ctre.phoenix6.hardware.TalonFX import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj2.command.Subsystem import edu.wpi.first.wpilibj2.command.SubsystemBase +import kotlinx.html.OL import mu.KotlinLogging +import org.strykeforce.healthcheck.Checkable import java.lang.reflect.Method import java.util.* import kotlin.math.abs @@ -62,6 +66,12 @@ class RobotHealthCheck(name: String, healthChecks: List) : } } +class RobotIOHealthCheck(name: String, healthChecks: List): HealthCheckCollection(name, healthChecks) { + override fun accept(visitor: HealthCheckVisitor) { + visitor.visit(this) + } +} + class SubsystemHealthCheck(name: String, healthChecks: List) : HealthCheckCollection(name, healthChecks) { override var isFinished = false @@ -70,6 +80,14 @@ class SubsystemHealthCheck(name: String, healthChecks: List) : } } +class IOHealthCheck(name: String, healthChecks: List): HealthCheckCollection(name, healthChecks) { + override var isFinished = false + + override fun accept(visitor: HealthCheckVisitor) { + visitor.visit(this) + } +} + abstract class TalonHealthCheck(val talon: BaseTalon, healthChecks: List) : HealthCheckCollection("Talon ${talon.deviceID}", healthChecks) { override fun accept(visitor: HealthCheckVisitor) { @@ -77,6 +95,13 @@ abstract class TalonHealthCheck(val talon: BaseTalon, healthChecks: List) : + HealthCheckCollection("TalonFX ${talonFx.deviceID}", healthChecks) { + override fun accept(visitor: HealthCheckVisitor) { + visitor.visit(this) + } + } + enum class State { INITIALIZING, REVERSING, @@ -93,14 +118,28 @@ class TalonTimedHealthCheck( val percentOutput: DoubleArray, ) : TalonHealthCheck(talon, healthChecks) +class P6TalonTimedHealthCheck( + talonFx: TalonFX, + healthChecks: List, + val percentOutput: DoubleArray, +): P6TalonHealthCheck(talonFx, healthChecks) + class TalonPositionHealthCheck( talon: BaseTalon, healthChecks: List, val percentOutput: DoubleArray, ) : TalonHealthCheck(talon, healthChecks) +class P6TalonPositionHealthCheck( + talonFx: TalonFX, + healthChecks: List, + val percentOutput: DoubleArray, +): P6TalonHealthCheck(talonFx, healthChecks) + class TalonFollowerHealthCheck(talon: BaseTalon, val leaderId: Int) : TalonHealthCheck(talon, listOf()) +class P6TalonFollowerHealthCheck(talonFX: TalonFX, val leaderId: Int) : P6TalonHealthCheck(talonFX, listOf()) + class LifecycleHealthCheck(private val subsystem: Subsystem, private val method: Method) : HealthCheck { override val name = subsystem.let { @@ -122,6 +161,25 @@ class LifecycleHealthCheck(private val subsystem: Subsystem, private val method: } } +class LifecycleIOHealthCheck(private val io: Checkable, private val method: Method) : HealthCheck { + override val name = io.let { + val ioName = (it as? Checkable)?.getName() ?: it.toString() + "$ioName.${method.name}" + } + + override var isFinished = false + + override fun accept(visitor: HealthCheckVisitor) = visitor.visit(this) + + override fun initialize() { + isFinished = false + } + + override fun execute() { + isFinished = method.invoke(io) as Boolean + } +} + var caseId = 0 class ResetCaseNum() { @@ -211,6 +269,84 @@ abstract class TalonHealthCheckCase( } } +abstract class P6TalonHealthCheckCase( + val talonFx: TalonFX, + val isReversing: Boolean, + val type: String, + val output: Double, + val duration: Long +) : HealthCheck { + val case = caseId++ + var uuid: UUID = UUID.randomUUID() + + override var isFinished = false + + private var state = State.INITIALIZING + + private var start = 0L + + val data: MutableList = mutableListOf(P6TalonHealthCheckData(case, talonFx)) + + fun addFollowerTalon(talonFx: TalonFX) { + data.add(P6TalonHealthCheckData(case, talonFx)) + } + + override fun accept(visitor: HealthCheckVisitor) = visitor.visit(this) + + override fun initialize() { + uuid = UUID.randomUUID() + data.forEach(P6TalonHealthCheckData::reset) + state = State.INITIALIZING + isFinished = false + start = 0 + } + + abstract fun isRunning(elapsed: Long) : Boolean + + abstract fun setTalon(talonFx: TalonFX) + + fun measure(timestamp: Long) = data.forEach { it.measure(timestamp) } + + override fun execute() { + val time = RobotController.getFPGATime() + + when(state) { + State.INITIALIZING -> { + talonFx.setControl(DutyCycleOut(0.0)) + start = time + state = if(isReversing) State.REVERSING else State.STARTING + } + + State.REVERSING -> { + val elapsed = time - start + state = if(elapsed > REVERSING_DURATION) State.STARTING else State.REVERSING + } + + State.STARTING -> { + setTalon(talonFx) + start = time + state = State.RUNNING + val elapsed = time - start + measure(elapsed) + } + + State.RUNNING -> { + val elapsed = time - start + if(isRunning(elapsed)) { + state = State.STOPPING + return + } + measure(elapsed) + } + + State.STOPPING -> { + talonFx.setControl(DutyCycleOut(0.0)) + isFinished = true + } + } + } +} + class TalonTimedHealthCheckCase( previousCase: TalonTimedHealthCheckCase?, talon: BaseTalon, @@ -246,6 +382,30 @@ class TalonTimedHealthCheckCase( } } +class P6TalonTimedHealthCheckCase( + previousCase: P6TalonTimedHealthCheckCase?, + talonFx :TalonFX, + val percentOutput: Double, + duration: Long +) : P6TalonHealthCheckCase(talonFx, (previousCase?.percentOutput ?: 0.0) * percentOutput < 0.0, "time", percentOutput, duration) { + override val name = "P6TalonTimedHealthCheckCase: ${percentOutput * 100} percent output" + + override fun isRunning(elapsed: Long) = elapsed > duration + + override fun setTalon(talonFx: TalonFX) { + talonFx.setControl(DutyCycleOut(percentOutput)) + } + + override fun toString(): String { + return "P6TalonTimedHealthCheckCase(" + + "percentOutput=$percentOutput, " + + "duration=$duration, " + + "isReversing=$isReversing" + + ")" + } + +} + class TalonPositionHealthCheckCase( previousCase: TalonPositionHealthCheckCase?, @@ -292,3 +452,42 @@ class TalonPositionHealthCheckCase( ")" } } + +class P6TalonPositionHealthCheckCase( + previousCase: P6TalonPositionHealthCheckCase?, + talonFx: TalonFX, + val percentOutput: Double, + private val encoderChange: Double +): P6TalonHealthCheckCase( + talonFx, + (previousCase?.percentOutput ?: 0.0) * percentOutput < 0.0, + "position", + percentOutput, + encoderChange.toLong() +) { + override val name = "P6TalonPositionHeathCheckCase: ${percentOutput * 100} percent output" + + private var encoderStart: Double = 0.0 + + override fun initialize() { + super.initialize() + encoderStart = talonFx.position.value + } + + override fun isRunning(elapsed: Long): Boolean { + val encoderCurrent = talonFx.position.value + return abs(encoderCurrent - encoderStart) >= encoderChange + } + + override fun setTalon(talonFx: TalonFX) { + talonFx.setControl(DutyCycleOut(percentOutput)) + } + + override fun toString(): String { + return "P6TalonPositionHealthCheckCase(" + + "percentOutput=$percentOutput, " + + "encoderChange=$encoderChange, " + + "isReversing=$isReversing" + + ")" + } +} diff --git a/src/main/kotlin/org/strykeforce/healthcheck/internal/ReportServer.kt b/src/main/kotlin/org/strykeforce/healthcheck/internal/ReportServer.kt index 1c26996..39f4414 100644 --- a/src/main/kotlin/org/strykeforce/healthcheck/internal/ReportServer.kt +++ b/src/main/kotlin/org/strykeforce/healthcheck/internal/ReportServer.kt @@ -51,6 +51,48 @@ class ReportServer(private val healthCheck: RobotHealthCheck) { } +class ReportServerIO(private val healthCheck: RobotIOHealthCheck) { + private val httpServer = HttpServer.create(InetSocketAddress(2767), 0).apply { + createContext("/run") { + logger.info { "${it.requestMethod} ${it.requestURI}" } + HealthCheckCommand.BUTTON.setPressed(true) + + it.responseHeaders.let { headers -> + headers["Content-Type"] = "text/plain; charset=utf-8" + } + + it.sendResponseHeaders(200, 0) + + it.responseBody.writer().use { out -> + out.write("BUTTON pressed") + } + } + + createContext("/data") { httpExchange -> + + httpExchange.responseHeaders.let { headers -> + headers["Content-Type"] = "application/json" + } + httpExchange.sendResponseHeaders(200, 0) + + httpExchange.responseBody.use { + try { + val jsonVisitor = JsonVisitor(it) + jsonVisitor.visit(healthCheck) + } catch (e: Exception) { + logger.error(e) { "error creating healthcheck JSON report" } + } + } + + } + + start() + } + + fun stop() = httpServer.stop(0) + +} + class JsonVisitor(outputStream: OutputStream) : HealthCheckVisitor { private val writer = outputStream.bufferedWriter() @@ -87,17 +129,43 @@ class JsonVisitor(outputStream: OutputStream) : HealthCheckVisitor { writer.flush() } + override fun visit(healthCheck: RobotIOHealthCheck) { + writer.write("{\"data\":[") + healthCheck.healthChecks.forEach { it.accept(this) } + + meta.values.forEach { it.deleteCharAt(it.lastIndex) } + + writer.write("],\"meta\":{") + for ((i, key) in meta.keys.withIndex()) { + writer.write("\"$key\":{${meta[key]}}") + if (i < meta.size - 1) writer.write(",") + } + writer.write("}}") + writer.flush() + } + override fun visit(healthCheck: SubsystemHealthCheck) { name = healthCheck.name healthCheck.healthChecks.forEach { it.accept(this) } } + override fun visit(healthCheck: IOHealthCheck) { + name = healthCheck.name + healthCheck.healthChecks.forEach { it.accept(this) } + } + override fun visit(healthCheck: LifecycleHealthCheck) = Unit + override fun visit(healthCheck: LifecycleIOHealthCheck) = Unit + override fun visit(healthCheck: TalonHealthCheck) { healthCheck.healthChecks.forEach { it.accept(this) } } + override fun visit(healthCheck: P6TalonHealthCheck) { + healthCheck.healthChecks.forEach { it.accept(this) } + } + override fun visit(healthCheck: TalonHealthCheckCase) { meta.getValue("case").append("\"${metaIndex}\":${healthCheck.case},") meta.getValue("case_uuid").append("\"${metaIndex}\":\"${healthCheck.uuid}\",") @@ -148,4 +216,54 @@ class JsonVisitor(outputStream: OutputStream) : HealthCheckVisitor { writer.write("}") } + override fun visit(healthCheck: P6TalonHealthCheckCase) { + meta.getValue("case").append("\"${metaIndex}\":${healthCheck.case},") + meta.getValue("case_uuid").append("\"${metaIndex}\":\"${healthCheck.uuid}\",") + meta.getValue("name").append("\"${metaIndex}\":\"$name\",") + meta.getValue("talon").append("\"${metaIndex}\":${healthCheck.talonFx.deviceID},") + meta.getValue("type").append("\"${metaIndex}\":\"${healthCheck.type}\",") + meta.getValue("output").append("\"${metaIndex}\":${healthCheck.output},") + meta.getValue("duration").append("\"${metaIndex}\":${healthCheck.duration},") + metaIndex++ + + val data = mapOf( + "msec_elapsed" to StringBuilder(), + "talon" to StringBuilder(), + "case" to StringBuilder(), + "voltage" to StringBuilder(), + "position" to StringBuilder(), + "speed" to StringBuilder(), + "supply_current" to StringBuilder(), + "stator_current" to StringBuilder(), + ) + + healthCheck.data.forEach { healthCheckData -> + healthCheckData.timestamp.forEachIndexed { i, v -> + data.getValue("msec_elapsed").append("\"${index + i}\":$v,") + data.getValue("talon").append("\"${index + i}\":${healthCheckData.deviceId},") + data.getValue("case").append("\"${index + i}\":${healthCheckData.case},") + data.getValue("voltage").append("\"${index + i}\":${healthCheckData.voltage[i]},") + data.getValue("position").append("\"${index + i}\":${healthCheckData.position[i]},") + data.getValue("speed").append("\"${index + i}\":${healthCheckData.speed[i]},") + data.getValue("supply_current").append("\"${index + i}\":${healthCheckData.supplyCurrent[i]},") + data.getValue("stator_current").append("\"${index + i}\":${healthCheckData.statorCurrent[i]},") + } + index += healthCheckData.timestamp.size + } + + data.values.forEach { if (it.lastIndex > 0) it.deleteCharAt(it.lastIndex) } + if (isFirst) { + writer.write("{") + isFirst = false + } else { + writer.write(",{") + } + + for ((i, key) in data.keys.withIndex()) { + writer.write("\"$key\":{${data[key]}}") + if (i < data.size - 1) writer.write(",") + } + writer.write("}") + } + } \ No newline at end of file diff --git a/src/main/kotlin/org/strykeforce/healthcheck/internal/Visitors.kt b/src/main/kotlin/org/strykeforce/healthcheck/internal/Visitors.kt index 52cc4fa..820e4c4 100644 --- a/src/main/kotlin/org/strykeforce/healthcheck/internal/Visitors.kt +++ b/src/main/kotlin/org/strykeforce/healthcheck/internal/Visitors.kt @@ -4,11 +4,18 @@ import mu.KotlinLogging interface HealthCheckVisitor { fun visit(healthCheck: RobotHealthCheck) + fun visit(healthCheck: RobotIOHealthCheck) fun visit(healthCheck: SubsystemHealthCheck) + fun visit(healthCheck: IOHealthCheck) fun visit(healthCheck: TalonHealthCheck) + + fun visit(healthCheck: P6TalonHealthCheck) fun visit(healthCheck: TalonHealthCheckCase) + fun visit(healthCheck: P6TalonHealthCheckCase) + fun visit(healthCheck: LifecycleHealthCheck) + fun visit(healthCheck: LifecycleIOHealthCheck) } class DumpVisitor : HealthCheckVisitor { @@ -26,20 +33,44 @@ class DumpVisitor : HealthCheckVisitor { logger.info { buffer } } + override fun visit(healthCheck: RobotIOHealthCheck) { + buffer.appendLine() + buffer.appendLine() + buffer.appendLine(healthCheck.name) + healthCheck.healthChecks.forEach{ it.accept(this)} + buffer.appendLine() + buffer.appendLine() + logger.info { buffer } + } + override fun visit(healthCheck: SubsystemHealthCheck) { buffer.appendLine(" ${healthCheck.name}") healthCheck.healthChecks.forEach { it.accept(this) } } + override fun visit(healthCheck: IOHealthCheck) { + buffer.appendLine(" ${healthCheck.name}") + healthCheck.healthChecks.forEach{ it.accept(this)} + } + override fun visit(healthCheck: LifecycleHealthCheck) { buffer.appendLine(" BeforeHealthCheck(${healthCheck.name})") } + override fun visit(healthCheck: LifecycleIOHealthCheck) { + buffer.appendLine(" BeforeHealthCheck(${healthCheck.name})") + } + override fun visit(healthCheck: TalonHealthCheck) { buffer.appendLine(" TalonHealthCheck(${healthCheck.name})") healthCheck.healthChecks.forEach { it.accept(this) } } + override fun visit(healthCheck: P6TalonHealthCheck) { + buffer.appendLine(" P6TalonHealthCheck(${healthCheck.name})") + healthCheck.healthChecks.forEach { it.accept(this) } + } + override fun visit(healthCheck: TalonHealthCheckCase) { buffer.appendLine(" $healthCheck: ${healthCheck.data.size} measurements") healthCheck.data.forEach { @@ -49,6 +80,13 @@ class DumpVisitor : HealthCheckVisitor { buffer.appendLine(" talon ${it.id} avg. stator current = ${it.averageStatorCurrent.format()} amps") } } + + override fun visit(healthCheck: P6TalonHealthCheckCase) { + buffer.appendLine(" $healthCheck: ${healthCheck.data.size} measurements") + healthCheck.data.forEach { + + } + } } private fun Double.format() = "%.2f".format(this) \ No newline at end of file diff --git a/src/test/java/org/strykeforce/healthcheck/AnnotationsTest.kt b/src/test/java/org/strykeforce/healthcheck/AnnotationsTest.kt index 73b2160..05665dd 100644 --- a/src/test/java/org/strykeforce/healthcheck/AnnotationsTest.kt +++ b/src/test/java/org/strykeforce/healthcheck/AnnotationsTest.kt @@ -11,7 +11,7 @@ class AnnotationsTest { val fieldTwo = "Two" @HealthCheck - @Position(percentOutput = [0.5], encoderChange = 1000) + @Position(percentOutput = [0.5], encoderChange = 1000.0) val fieldThree = "Three" @HealthCheck @@ -20,7 +20,7 @@ class AnnotationsTest { @HealthCheck @Timed(percentOutput = [0.5], duration = 1.0) - @Position(percentOutput = [0.5], encoderChange = 1000) + @Position(percentOutput = [0.5], encoderChange = 1000.0) val fieldFive = "Five" diff --git a/src/test/java/org/strykeforce/healthcheck/TestSubsystem.kt b/src/test/java/org/strykeforce/healthcheck/TestSubsystem.kt index 3f42851..2de8e10 100644 --- a/src/test/java/org/strykeforce/healthcheck/TestSubsystem.kt +++ b/src/test/java/org/strykeforce/healthcheck/TestSubsystem.kt @@ -16,7 +16,7 @@ internal class TestSubsystem : Subsystem { // position tests - no current or speed limits @HealthCheck - @Position(percentOutput = [0.25, -0.25], encoderChange = 20000) + @Position(percentOutput = [0.25, -0.25], encoderChange = 20000.0) private val talonThree = mock() // timed tests - specify limits: currentMin, currentMax, speedMin, speedMax @@ -27,7 +27,7 @@ internal class TestSubsystem : Subsystem { // position test - specify limits @HealthCheck - @Position(percentOutput = [0.25], encoderChange = 20000) + @Position(percentOutput = [0.25], encoderChange = 20000.0) @Limits([0.75, 2.0, 1500.0, 3500.0]) private val talonFive = mock() diff --git a/src/test/java/org/strykeforce/swerve/TalonSwerveModuleTest.java b/src/test/java/org/strykeforce/swerve/TalonSwerveModuleTest.java index d73835e..5516a02 100644 --- a/src/test/java/org/strykeforce/swerve/TalonSwerveModuleTest.java +++ b/src/test/java/org/strykeforce/swerve/TalonSwerveModuleTest.java @@ -20,6 +20,7 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.Timestamp; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.VelocityDutyCycle; import edu.wpi.first.math.geometry.Rotation2d; @@ -1215,12 +1216,18 @@ class TestShouldGetV6ModulePosition { private StatusSignal positionStatusSig; + private Timestamp timestamp; + + private StatusSignal velocityStatusSig; + @BeforeEach void setTup() { azimuthTalon = mock(TalonSRX.class); driveTalon = mock(com.ctre.phoenix6.hardware.TalonFX.class); when(driveTalon.setPosition(0)).thenReturn(StatusCode.OK); positionStatusSig = mock(StatusSignal.class); + timestamp = mock(Timestamp.class); + velocityStatusSig = mock(StatusSignal.class); } @Test @@ -1237,6 +1244,10 @@ void V6PositionWithDefaultEncoderCounts() { .build(); when(driveTalon.getPosition()).thenReturn(positionStatusSig); when(positionStatusSig.getValue()).thenReturn(10.0); + when(positionStatusSig.getTimestamp()).thenReturn(timestamp); + when(timestamp.getLatency()).thenReturn(0.0); + when(driveTalon.getVelocity()).thenReturn(velocityStatusSig); + when(velocityStatusSig.getValue()).thenReturn(0.0); SwerveModulePosition position = module.getPosition(); assertEquals(0.365733744755412, position.distanceMeters, 1e-9); } @@ -1258,6 +1269,11 @@ void V6PositionWithEncoderCounts( .build(); when(driveTalon.getPosition()).thenReturn(positionStatusSig); when(positionStatusSig.getValue()).thenReturn(driveTalonPosition); + + when(positionStatusSig.getTimestamp()).thenReturn(timestamp); + when(timestamp.getLatency()).thenReturn(0.0); + when(driveTalon.getVelocity()).thenReturn(velocityStatusSig); + when(velocityStatusSig.getValue()).thenReturn(0.0); SwerveModulePosition position = module.getPosition(); assertEquals(expectedMeters, position.distanceMeters, 1e-9); }