Merge pull request #109 from strykeforce/2024-fixes

2024.0.3 updates

Author: mwitcpalek

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

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;

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)

src/main/kotlin/org/strykeforce/healthcheck/Checkable.kt

@@ -0,0 +1,8 @@
+package org.strykeforce.healthcheck
+
+interface Checkable {
+
+    fun getName(): String
+
+
+}

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<Subsystem>, 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
 }