Added back 2nd order kinematics to angle motors and enabled it within…

… the SparkMaxSwerve.setReference function. Signed-off-by: thenetworkgrinch <[email protected]>
GeraldTM · Mar 27, 2023 · fb4556f · fb4556f
1 parent 1dd0795
commit fb4556f
Showing 1 changed file with 16 additions and 5 deletions.
diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java
@@ -293,20 +293,31 @@ public void set(double percentOutput)
   @Override
   public void setReference(double setpoint, double feedforward)
   {
+    boolean possibleBurnOutIssue = true;
     int pidSlot =
         isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
-    if (isDriveMotor)
+    if (possibleBurnOutIssue)
     {
       pid.setReference(
           setpoint,
-          ControlType.kVelocity,
+          isDriveMotor ? ControlType.kVelocity : ControlType.kPosition,
           pidSlot,
           feedforward);
     } else
     {
-      pid.setReference(
-          setpoint,
-          ControlType.kPosition);
+      if (isDriveMotor)
+      {
+        pid.setReference(
+            setpoint,
+            ControlType.kVelocity,
+            pidSlot,
+            feedforward);
+      } else
+      {
+        pid.setReference(
+            setpoint,
+            ControlType.kPosition);
+      }
     }
   }