Revert "Simplify MAXSwerve autolock"

This reverts commit 466ba08.
lasarobotics · Jan 22, 2024 · c8c1f54 · c8c1f54
1 parent 2f51b71
commit c8c1f54
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 7 deletions.
diff --git a/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java b/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java
@@ -4,6 +4,9 @@
 
 package org.lasarobotics.drive;
 
+import java.time.Duration;
+import java.time.Instant;
+
 import org.lasarobotics.hardware.revrobotics.Spark;
 import org.lasarobotics.hardware.revrobotics.Spark.MotorKind;
 import org.lasarobotics.hardware.revrobotics.SparkPIDConfig;
@@ -26,7 +29,6 @@
 import edu.wpi.first.units.Time;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.Velocity;
-import edu.wpi.first.wpilibj.Timer;
 
 /** REV MAXSwerve module */
 public class MAXSwerveModule implements AutoCloseable {
@@ -122,7 +124,7 @@ private GearRatio(double value) {
   private boolean m_autoLock;
 
   private TractionControlController m_tractionControlController;
-  private Timer m_autoLockTimer;
+  private Instant m_autoLockTimer;
 
   /**
    * Create an instance of a MAXSwerveModule
@@ -151,7 +153,7 @@ public MAXSwerveModule(Hardware swerveHardware, ModuleLocation location, GearRat
     this.m_autoLockTime = MathUtil.clamp(autoLockTime.in(Units.Milliseconds), 0.0, MAX_AUTO_LOCK_TIME * 1000);
     this.m_previousRotatePosition = LOCK_POSITION;
     this.m_tractionControlController =  new TractionControlController(Units.MetersPerSecond.of(DRIVE_MAX_LINEAR_SPEED), slipRatio);
-    this.m_autoLockTimer = new Timer();
+    this.m_autoLockTimer = Instant.now();
 
     // Set drive encoder conversion factor
     m_driveConversionFactor = DRIVE_WHEEL_DIAMETER_METERS * Math.PI / m_driveGearRatio.value;
@@ -291,13 +293,13 @@ public void set(SwerveModuleState state) {
     if (m_autoLock && state.speedMetersPerSecond < EPSILON) {
       state.speedMetersPerSecond = 0.0;
       // Time's up, lock now...
-      if (m_autoLockTimer.hasElapsed(m_autoLockTime))
+      if (Duration.between(m_autoLockTimer, Instant.now()).toMillis() > m_autoLockTime)
         state.angle = LOCK_POSITION.minus(m_location.offset);
       // Waiting to lock...
       else state.angle = m_previousRotatePosition.minus(m_location.offset);
     } else {
       // Not locking this loop, restart timer...
-      m_autoLockTimer.restart();
+      m_autoLockTimer = Instant.now();
     }
 
     // Apply chassis angular offset to the requested state.

diff --git a/src/test/java/org/lasarobotics/drive/MAXSwerveModuleTest.java b/src/test/java/org/lasarobotics/drive/MAXSwerveModuleTest.java
@@ -35,7 +35,7 @@
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Time;
 import edu.wpi.first.units.Units;
-import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.wpilibj.Timer;
 
 @TestMethodOrder(MethodOrderer.OrderAnnotation.class)
 public class MAXSwerveModuleTest {
@@ -174,7 +174,7 @@ public void autoLock() {
     when(m_rRearRotateMotor.getInputs()).thenReturn(sparkInputs);
 
     // Advance sim time
-    WPIUtilJNI.setMockTime((long)AUTO_LOCK_TIME.in(Units.Seconds));
+    Timer.delay(AUTO_LOCK_TIME.in(Units.Seconds));
 
     // Try to set module state
     SwerveModuleState state = new SwerveModuleState(0.0, Rotation2d.fromRadians(+Math.PI));