Add sim support for more devices

src/main/java/org/carlmontrobotics/lib199/MotorConfig.java

@@ -5,6 +5,13 @@ public class MotorConfig {
     public static final MotorConfig NEO = new MotorConfig(70, 40);
     public static final MotorConfig NEO_550 = new MotorConfig(40, 20);
 
+    // The temp limit of 100C for the Vortex is based on the fact that its temp sensors are mounted directly on the
+    // windings (which is not the case for the Neo or Neo550, causing them to have very delayed temp readings) and the 
+    // fact that the winding enamel will melt at 140C. 
+    // See: https://www.chiefdelphi.com/t/rev-robotics-spark-flex-and-neo-vortex/442595/349?u=brettle
+    // As a result I think 100C should be safe. I wouldn't increase it past 120. --Dean
+    public static final MotorConfig NEO_VORTEX = new MotorConfig(100, 60);
+
     public final int temperatureLimitCelsius, currentLimitAmps;
 
     public MotorConfig(int temperatureLimitCelsius, int currentLimitAmps) {

src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java

@@ -14,9 +14,12 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkBase.ExternalFollower;
 import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkLowLevel;
 import com.revrobotics.SparkPIDController;
 
+import org.carlmontrobotics.lib199.sim.MockSparkFlex;
 import org.carlmontrobotics.lib199.sim.MockSparkMax;
 import org.carlmontrobotics.lib199.sim.MockTalonSRX;
 import org.carlmontrobotics.lib199.sim.MockVictorSPX;
@@ -120,24 +123,41 @@ public static CANSparkMax createSparkMax(int id, MotorConfig config) {
       spark = MockSparkMax.createMockSparkMax(id, CANSparkLowLevel.MotorType.kBrushless);
     }
 
-    MotorErrors.reportSparkMaxTemp(spark, config.temperatureLimitCelsius);
+    configureSpark(spark, config);
+
+    return spark;
+  }
+
+  public static CANSparkFlex createSparkFlex(int id, MotorConfig config) {
+    CANSparkFlex spark;
+    if (RobotBase.isReal()) {
+      spark = new CANSparkFlex(id, CANSparkLowLevel.MotorType.kBrushless);
+    } else {
+      spark = MockSparkFlex.createMockSparkFlex(id, CANSparkLowLevel.MotorType.kBrushless);
+    }
+
+    configureSpark(spark, config);
+
+    return spark;
+  }
+
+  private static void configureSpark(CANSparkBase spark, MotorConfig config) {
+    MotorErrors.reportSparkTemp(spark, config.temperatureLimitCelsius);
 
     MotorErrors.reportError(spark.restoreFactoryDefaults());
     //MotorErrors.reportError(spark.follow(ExternalFollower.kFollowerDisabled, 0));
     MotorErrors.reportError(spark.setIdleMode(IdleMode.kBrake));
     MotorErrors.reportError(spark.enableVoltageCompensation(12));
     MotorErrors.reportError(spark.setSmartCurrentLimit(config.currentLimitAmps));
 
-    MotorErrors.checkSparkMaxErrors(spark);
+    MotorErrors.checkSparkErrors(spark);
 
     SparkPIDController controller = spark.getPIDController();
     MotorErrors.reportError(controller.setOutputRange(-1, 1));
     MotorErrors.reportError(controller.setP(0));
     MotorErrors.reportError(controller.setI(0));
     MotorErrors.reportError(controller.setD(0));
     MotorErrors.reportError(controller.setFF(0));
-
-    return spark;
   }
 
   /**
@@ -183,4 +203,4 @@ public static UsbCamera[] configureCameras(int numCameras) {
     for(int i = 0; i < numCameras; i++) cameras[i] = configureCamera();
     return cameras;
   }
-}
+}

src/main/java/org/carlmontrobotics/lib199/MotorErrors.java

@@ -4,6 +4,8 @@
 import java.util.concurrent.ConcurrentHashMap;
 
 import com.ctre.phoenix.ErrorCode;
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkBase.FaultID;
 import com.revrobotics.REVLibError;
@@ -12,17 +14,17 @@
 
 public final class MotorErrors {
 
-    private static final ConcurrentHashMap<Integer, CANSparkMax> temperatureSparks = new ConcurrentHashMap<>();
+    private static final ConcurrentHashMap<Integer, CANSparkBase> temperatureSparks = new ConcurrentHashMap<>();
     private static final ConcurrentHashMap<Integer, Integer> sparkTemperatureLimits = new ConcurrentHashMap<>();
     private static final ConcurrentHashMap<Integer, Integer> overheatedSparks = new ConcurrentHashMap<>();
-    private static final ConcurrentHashMap<CANSparkMax, Short> flags = new ConcurrentHashMap<>();
-    private static final ConcurrentHashMap<CANSparkMax, Short> stickyFlags = new ConcurrentHashMap<>();
+    private static final ConcurrentHashMap<CANSparkBase, Short> flags = new ConcurrentHashMap<>();
+    private static final ConcurrentHashMap<CANSparkBase, Short> stickyFlags = new ConcurrentHashMap<>();
 
     public static final int kOverheatTripCount = 5;
 
     static {
-        Lib199Subsystem.registerAsyncPeriodic(MotorErrors::doReportSparkMaxTemp);
-        Lib199Subsystem.registerAsyncPeriodic(MotorErrors::printSparkMaxErrorMessages);
+        Lib199Subsystem.registerAsyncPeriodic(MotorErrors::doReportSparkTemp);
+        Lib199Subsystem.registerAsyncPeriodic(MotorErrors::printSparkErrorMessages);
     }
 
     public static void reportError(ErrorCode error) {
@@ -55,25 +57,30 @@ private static <T extends Enum<T>> void reportError(String vendor, T error, T ok
         System.err.println(Arrays.toString(stack));
     }
 
-    public static void checkSparkMaxErrors(CANSparkMax spark) {
+    public static void checkSparkErrors(CANSparkBase spark) {
         //Purposely obviously impersonal to differentiate from actual computer generated errors
         short faults = spark.getFaults();
         short stickyFaults = spark.getStickyFaults();
         short prevFaults = flags.containsKey(spark) ? flags.get(spark) : 0;
         short prevStickyFaults = stickyFlags.containsKey(spark) ? stickyFlags.get(spark) : 0;
 
         if (spark.getFaults() != 0 && prevFaults != faults) {
-        System.err.println("Whoops, big oopsie : fault error(s) with spark max id : " + spark.getDeviceId() + ": [ " + formatFaults(spark) + "], ooF!");
+        System.err.println("Whoops, big oopsie : fault error(s) with spark id : " + spark.getDeviceId() + ": [ " + formatFaults(spark) + "], ooF!");
         }
         if (spark.getStickyFaults() != 0 && prevStickyFaults != stickyFaults) {
-        System.err.println("Bruh, you did an Error : sticky fault(s) error with spark max id : " + spark.getDeviceId() + ": " + formatStickyFaults(spark) + ", Ouch!");
+        System.err.println("Bruh, you did an Error : sticky fault(s) error with spark id : " + spark.getDeviceId() + ": " + formatStickyFaults(spark) + ", Ouch!");
         }
         spark.clearFaults();
         flags.put(spark, faults);
         stickyFlags.put(spark, stickyFaults);
     }
 
-    private static String formatFaults(CANSparkMax spark) {
+    @Deprecated
+    public static void checkSparkMaxErrors(CANSparkMax spark) {
+        checkSparkErrors((CANSparkBase)spark);
+    }
+
+    private static String formatFaults(CANSparkBase spark) {
         String out = "";
         for(FaultID fault: FaultID.values()) {
             if(spark.getFault(fault)) {
@@ -83,7 +90,7 @@ private static String formatFaults(CANSparkMax spark) {
         return out;
     }
 
-    private static String formatStickyFaults(CANSparkMax spark) {
+    private static String formatStickyFaults(CANSparkBase spark) {
         String out = "";
         for(FaultID fault: FaultID.values()) {
             if(spark.getStickyFault(fault)) {
@@ -93,8 +100,13 @@ private static String formatStickyFaults(CANSparkMax spark) {
         return out;
     }
 
+    @Deprecated
     public static void printSparkMaxErrorMessages() {
-        flags.keySet().forEach((spark) -> checkSparkMaxErrors(spark));
+        printSparkErrorMessages();
+    }
+
+    public static void printSparkErrorMessages() {
+        flags.keySet().forEach(MotorErrors::checkSparkErrors);
     }
 
     public static CANSparkMax createDummySparkMax() {
@@ -105,26 +117,42 @@ public static CANSparkMax createDummySparkMax() {
     public static void reportSparkMaxTemp(CANSparkMax spark, TemperatureLimit temperatureLimit) {
         reportSparkMaxTemp(spark, temperatureLimit.limit);
     }
-  
+
     public static boolean isSparkMaxOverheated(CANSparkMax spark){
       int id = spark.getDeviceId();
       int motorMaxTemp = sparkTemperatureLimits.get(id);
       return ( spark.getMotorTemperature() >= motorMaxTemp );
     }
 
+    @Deprecated
     public static void reportSparkMaxTemp(CANSparkMax spark, int temperatureLimit) {
+        reportSparkTemp((CANSparkBase) spark, temperatureLimit);
+    }
+
+    public static void reportSparkTemp(CANSparkBase spark, int temperatureLimit) {
         int id = spark.getDeviceId();
         temperatureSparks.put(id, spark);
         sparkTemperatureLimits.put(id, temperatureLimit);
         overheatedSparks.put(id, 0);
     }
 
+    @Deprecated
     public static void doReportSparkMaxTemp() {
+        doReportSparkTemp();
+    }
+
+    public static void doReportSparkTemp() {
         temperatureSparks.forEach((port, spark) -> {
             double temp = spark.getMotorTemperature();
             double limit = sparkTemperatureLimits.get(port);
             int numTrips = overheatedSparks.get(port);
-            SmartDashboard.putNumber("Port " + port + " Spark Max Temp", temp);
+            String sparkType = "of unknown type";
+            if (spark instanceof CANSparkMax) {
+                sparkType = "Max";
+            } else if (spark instanceof CANSparkFlex) {
+                sparkType = "Flex";
+            }
+            SmartDashboard.putNumber(String.format("Port %d Spark %s Temp", port, sparkType), temp);
 
             if(numTrips < kOverheatTripCount) {
                 if(temp > limit) {
@@ -140,7 +168,7 @@ public static void doReportSparkMaxTemp() {
                     // Set trip count to kOverheatTripCount + 1 to flag that an error message has already been printed
                     // This prevents the error message from being re-printed every time the periodic method is run
                     overheatedSparks.put(port, kOverheatTripCount + 1);
-                    System.err.println("Port " + port + " spark max is operating at " + temp + " degrees Celsius! It will be disabled until the robot code is restarted.");
+                    System.err.println("Port " + port + " spark is operating at " + temp + " degrees Celsius! It will be disabled until the robot code is restarted.");
                 }
                 spark.setSmartCurrentLimit(1);
             }
@@ -160,4 +188,4 @@ private TemperatureLimit(int limit) {
         }
     }
 
-}
+}

src/main/java/org/carlmontrobotics/lib199/SensorFactory.java

@@ -1,9 +1,10 @@
 package org.carlmontrobotics.lib199;
 
 import org.carlmontrobotics.lib199.sim.MockedCANCoder;
+import org.carlmontrobotics.lib199.sim.MockedPlayingWithFusionTimeOfFlight;
 
 import com.ctre.phoenix6.hardware.CANcoder;
-
+import com.playingwithfusion.TimeOfFlight;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
@@ -59,4 +60,13 @@ public static UsbCamera[] configureCameras(int numCameras) {
         return cameras;
     }
 
+    public static TimeOfFlight createPlayingWithFusionTimeOfFlight(int portNumber) {
+        TimeOfFlight tof;
+        if (RobotBase.isReal()) {
+            tof = new TimeOfFlight(portNumber);
+        } else {
+            tof = MockedPlayingWithFusionTimeOfFlight.createMock(portNumber);
+        }
+        return tof;
+    }
 }