Skip to content

Commit 5ca4c11

Browse files
committed
add different temperature limits for NEOs and NEO 550s
1 parent 421f5c8 commit 5ca4c11

4 files changed

Lines changed: 29 additions & 9 deletions

File tree

src/main/java/frc/robot/lib/MotorControllerFactory.java

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
import com.revrobotics.SparkMaxPIDController;
1818

1919
import edu.wpi.first.wpilibj.RobotBase;
20+
import frc.robot.lib.MotorErrors.TemperatureLimit;
2021
import frc.robot.lib.sim.MockSparkMax;
2122
import frc.robot.lib.sim.MockTalonSRX;
2223
import frc.robot.lib.sim.MockVictorSPX;
@@ -71,7 +72,11 @@ public static WPI_TalonSRX createTalon(int id) {
7172

7273
//checks for spark max errors
7374

74-
public static CANSparkMax createSparkMax(int id) {
75+
public static CANSparkMax createSparkMax(int id, TemperatureLimit temperatureLimit) {
76+
return createSparkMax(id, temperatureLimit.limit);
77+
}
78+
79+
public static CANSparkMax createSparkMax(int id, int temperatureLimit) {
7580
CANSparkMax spark;
7681
if (RobotBase.isReal()) {
7782
spark = new CachedSparkMax(id, CANSparkMaxLowLevel.MotorType.kBrushless);
@@ -84,7 +89,7 @@ public static CANSparkMax createSparkMax(int id) {
8489
spark = MockSparkMax.createMockSparkMax(id, CANSparkMaxLowLevel.MotorType.kBrushless);
8590
}
8691

87-
MotorErrors.reportSparkMaxTemp(spark);
92+
MotorErrors.reportSparkMaxTemp(spark, temperatureLimit);
8893

8994
MotorErrors.reportError(spark.restoreFactoryDefaults());
9095
MotorErrors.reportError(spark.follow(ExternalFollower.kFollowerDisabled, 0));

src/main/java/frc/robot/lib/MotorErrors.java

Lines changed: 20 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
public 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
}

src/test/java/frc/robot/lib/MotorControllerFactoryTest.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@ public void testCreateNoErrors() throws Exception {
2222
// Call close to free PWM ports
2323
((AutoCloseable)MotorControllerFactory.createTalon(0)).close();
2424
((AutoCloseable)MotorControllerFactory.createVictor(1)).close();
25-
MotorControllerFactory.createSparkMax(2);
25+
MotorControllerFactory.createSparkMax(2, 40);
2626
assertEquals(0, errStream.toByteArray().length);
2727
}
2828

src/test/java/frc/robot/lib/MotorErrorsTest.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -162,7 +162,7 @@ public void testReportSparkMaxTemp() {
162162

163163
private void doTestReportSparkMaxTemp(int id) {
164164
TemperatureSparkMax spark = (TemperatureSparkMax)Mocks.createMock(CANSparkMax.class, new TemperatureSparkMax.Instance(id), TemperatureSparkMax.class);
165-
MotorErrors.reportSparkMaxTemp((CANSparkMax)spark);
165+
MotorErrors.reportSparkMaxTemp((CANSparkMax)spark, 40);
166166
spark.setSmartCurrentLimit(50);
167167
spark.setTemperature(20);
168168
CommandScheduler.getInstance().run();

0 commit comments

Comments
 (0)