Added delay to config loop. Updated setInverted to have redundant checks.

Signed-off-by: thenetworkgrinch <[email protected]>

Author: thenetworkgrinch

src/main/java/swervelib/motors/SparkFlexSwerve.java

@@ -12,6 +12,7 @@
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkAnalogSensor;
 import com.revrobotics.SparkPIDController;
+import edu.wpi.first.wpilibj.Timer;
 import java.util.function.Supplier;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 import swervelib.parser.PIDFConfig;
@@ -108,6 +109,7 @@ private void configureSparkFlex(Supplier<REVLibError> config)
       {
         return;
       }
+      Timer.delay(0.01);
     }
     failureConfiguring.set(true);
   }
@@ -329,7 +331,10 @@ public void setMotorBrake(boolean isBrakeMode)
   @Override
   public void setInverted(boolean inverted)
   {
-    motor.setInverted(inverted);
+    configureSparkFlex(() -> {
+      motor.setInverted(inverted);
+      return motor.getLastError();
+    });
   }
 
   /**

src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java

@@ -11,6 +11,7 @@
 import com.revrobotics.SparkMaxAlternateEncoder;
 import com.revrobotics.SparkPIDController;
 import com.revrobotics.SparkRelativeEncoder.Type;
+import edu.wpi.first.wpilibj.Timer;
 import java.util.function.Supplier;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 import swervelib.parser.PIDFConfig;
@@ -145,6 +146,7 @@ private void configureSparkMax(Supplier<REVLibError> config)
       {
         return;
       }
+      Timer.delay(0.01);
     }
     failureConfiguringAlert.set(true);
   }
@@ -351,7 +353,10 @@ public void setMotorBrake(boolean isBrakeMode)
   @Override
   public void setInverted(boolean inverted)
   {
-    motor.setInverted(inverted);
+    configureSparkMax(() -> {
+      motor.setInverted(inverted);
+      return motor.getLastError();
+    });
   }
 
   /**

src/main/java/swervelib/motors/SparkMaxSwerve.java

@@ -12,6 +12,7 @@
 import com.revrobotics.SparkAnalogSensor;
 import com.revrobotics.SparkPIDController;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
 import java.util.function.Supplier;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 import swervelib.parser.PIDFConfig;
@@ -26,31 +27,31 @@ public class SparkMaxSwerve extends SwerveMotor
   /**
    * {@link CANSparkMax} Instance.
    */
-  private final CANSparkMax motor;
+  private final CANSparkMax           motor;
   /**
    * Integrated encoder.
    */
-  public  RelativeEncoder       encoder;
+  public        RelativeEncoder       encoder;
   /**
    * Absolute encoder attached to the SparkMax (if exists)
    */
-  public  SwerveAbsoluteEncoder absoluteEncoder;
+  public        SwerveAbsoluteEncoder absoluteEncoder;
   /**
    * Closed-loop PID controller.
    */
-  public  SparkPIDController    pid;
+  public        SparkPIDController    pid;
   /**
    * Factory default already occurred.
    */
-  private boolean               factoryDefaultOccurred = false;
+  private       boolean               factoryDefaultOccurred = false;
   /**
    * Supplier for the velocity of the motor controller.
    */
-  private Supplier<Double>      velocity;
+  private       Supplier<Double>      velocity;
   /**
    * Supplier for the position of the motor controller.
    */
-  private Supplier<Double>      position;
+  private       Supplier<Double>      position;
 
   /**
    * Initialize the swerve motor.
@@ -100,6 +101,7 @@ private void configureSparkMax(Supplier<REVLibError> config)
       {
         return;
       }
+      Timer.delay(0.01);
     }
     DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
   }
@@ -357,7 +359,10 @@ public void setMotorBrake(boolean isBrakeMode)
   @Override
   public void setInverted(boolean inverted)
   {
-    motor.setInverted(inverted);
+    configureSparkMax(() -> {
+      motor.setInverted(inverted);
+      return motor.getLastError();
+    });
   }
 
   /**