From 154bee371a08c7229093ba5300b4b4abf8b3a4ee Mon Sep 17 00:00:00 2001
From: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
Date: Mon, 13 Feb 2023 23:19:13 -0600
Subject: [PATCH] Added lock mode

---
 swervelib/SwerveDrive.java | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index d6acf63..3dad556 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -140,6 +140,21 @@ public class SwerveDrive
     setModuleStates(swerveModuleStates, isOpenLoop);
   }
 
+  /**
+   * Lock the swerve drive to prevent it from moving.
+   */
+  public void lock()
+  {
+    double[]             angles        = new double[]{45, -45, -45, 45};
+    SwerveModuleState2[] moduleState2s = new SwerveModuleState2[swerveModules.length];
+    for (int i = 0; i < moduleState2s.length; i++)
+    {
+      swerveModules[i].lastAngle = angles[i % 4]; // Override jittering to ensure module is set to angle.
+      moduleState2s[i] = new SwerveModuleState2(0, Rotation2d.fromDegrees(angles[i % 4]), 0);
+    }
+    setModuleStates(moduleState2s, false);
+  }
+
   /**
    * Set the module states (azimuth and velocity) directly.  Used primarily for auto pathing.
    *