1414public final class MotorErrors {
1515
1616 private static final HashMap <Integer , CANSparkMax > temperatureSparks = new HashMap <>();
17+ private static final HashMap <Integer , Integer > sparkTemperatureLimits = new HashMap <>();
1718 private static final ArrayList <Integer > overheatedSparks = new ArrayList <>();
1819 private static final HashMap <CANSparkMax , Short > flags = new HashMap <>();
1920 private static final HashMap <CANSparkMax , Short > stickyFlags = new HashMap <>();
@@ -29,7 +30,7 @@ public static void reportError(ErrorCode error) {
2930 public static void reportError (REVLibError error ) {
3031 reportError ("REV Robotics" , error , REVLibError .kOk );
3132 }
32-
33+
3334 public static void reportErrors (ErrorCode ... errors ) {
3435 for (ErrorCode error : errors ) {
3536 reportError (error );
@@ -52,8 +53,7 @@ private static <T extends Enum<T>> void reportError(String vendor, T error, T ok
5253 System .err .println (Arrays .toString (stack ));
5354 }
5455
55-
56- public static void checkSparkMaxErrors (CANSparkMax spark ) {
56+ public static void checkSparkMaxErrors (CANSparkMax spark ) {
5757 //Purposely obivously impersonal to differentiate from actual computer generated errors
5858 short faults = spark .getFaults ();
5959 short stickyFaults = spark .getStickyFaults ();
@@ -99,17 +99,22 @@ public static CANSparkMax createDummySparkMax() {
9999 return DummySparkMaxAnswer .DUMMY_SPARK_MAX ;
100100 }
101101
102- public static void reportSparkMaxTemp (CANSparkMax spark ) {
102+ public static void reportSparkMaxTemp (CANSparkMax spark , TemperatureLimit temperatureLimit ) {
103+ reportSparkMaxTemp (spark , temperatureLimit .limit );
104+ }
105+
106+ public static void reportSparkMaxTemp (CANSparkMax spark , int temperatureLimit ) {
103107 int id = spark .getDeviceId ();
104108 temperatureSparks .put (id , spark );
109+ sparkTemperatureLimits .put (id , temperatureLimit );
105110 }
106111
107112 public static void doReportSparkMaxTemp () {
108113 temperatureSparks .forEach ((port , spark ) -> {
109114 double temp = spark .getMotorTemperature ();
110115 SmartDashboard .putNumber ("Port " + port + " Spark Max Temp" , temp );
111116 // Check if temperature exceeds the setpoint or if the contoller has already overheated to prevent other code from resetting the current limit after the controller has cooled
112- if (temp >= 100 || overheatedSparks .contains (port )) {
117+ if (temp >= sparkTemperatureLimits . get ( port ) || overheatedSparks .contains (port )) {
113118 if (!overheatedSparks .contains (port )) {
114119 overheatedSparks .add (port );
115120 System .err .println ("Port " + port + " spark max is operating at " + temp + " degrees Celsius! It will be disabled until the robot code is restarted." );
@@ -121,4 +126,14 @@ public static void doReportSparkMaxTemp() {
121126
122127 private MotorErrors () {}
123128
129+ public static enum TemperatureLimit {
130+ NEO (70 ), NEO_550 (40 );
131+
132+ public final int limit ;
133+
134+ private TemperatureLimit (int limit ) {
135+ this .limit = limit ;
136+ }
137+ }
138+
124139}
0 commit comments