From 3b0ab24f119a688b243cbda03dc3a8c9d34c6fe8 Mon Sep 17 00:00:00 2001 From: pacoito123 Date: Sun, 18 Aug 2019 10:14:14 -0500 Subject: [PATCH] Some more changes and code optimizations. [SuperComponents] - SuperComponents now check for duplicate entries before init. - ComponentInitExceptions are now reported to the DriverStation. - Removed a bunch of unnecessary code in SuperTalon and SuperVictor. --- .../org/usfirst/lib6647/oi/JController.java | 6 +- .../supercomponents/SuperCompressor.java | 12 +- .../supercomponents/SuperDigitalInput.java | 11 +- .../supercomponents/SuperDoubleSolenoid.java | 12 +- .../supercomponents/SuperEncoder.java | 14 +- .../subsystem/supercomponents/SuperPDP.java | 10 +- .../supercomponents/SuperSolenoid.java | 15 +- .../subsystem/supercomponents/SuperTalon.java | 145 ++++++------------ .../supercomponents/SuperUltrasonic.java | 10 +- .../supercomponents/SuperVictor.java | 87 ++++------- 10 files changed, 132 insertions(+), 190 deletions(-) diff --git a/src/main/java/org/usfirst/lib6647/oi/JController.java b/src/main/java/org/usfirst/lib6647/oi/JController.java index ed16bac..d5766cf 100644 --- a/src/main/java/org/usfirst/lib6647/oi/JController.java +++ b/src/main/java/org/usfirst/lib6647/oi/JController.java @@ -183,7 +183,7 @@ public boolean get() { } /** - * Method for getting an axisButton input for any value, with 0.15 as tolerance + * Method for getting an axisButton input for any value, with 0.30 as tolerance * (so as to avoid accidental input due to improper {@link JController} * calibration). * @@ -201,7 +201,7 @@ public boolean get() { } /** - * Method for getting a negative axisButton input, with 0.15 as tolerance (to + * Method for getting a negative axisButton input, with -0.30 as tolerance (to * avoid accidental input due to improper {@link JController} calibration). * * @param controller @@ -218,7 +218,7 @@ public boolean get() { } /** - * Method for getting a positive axisButton input, with 0.15 as tolerance (to + * Method for getting a positive axisButton input, with 0.30 as tolerance (to * avoid accidental input due to improper {@link JController} calibration). * * @param controller diff --git a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperCompressor.java b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperCompressor.java index be54b2b..5448247 100644 --- a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperCompressor.java +++ b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperCompressor.java @@ -9,6 +9,7 @@ import org.usfirst.lib6647.util.ComponentInitException; import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.DriverStation; /** * Interface to allow {@link Compressor} initialization via JSON. Subsystems @@ -35,7 +36,8 @@ default void initCompressors(JsonNode robotMap, String subsystemName) { // Spliterate through each of the elements in the JsonNode. robotMap.get("compressors").spliterator().forEachRemaining(json -> { try { - if (json.hasNonNull("name") && json.hasNonNull("module")) { + if (json.hasNonNull("name") && !compressors.containsKey(json.get("name").asText()) + && json.hasNonNull("module")) { // Read values from JsonNode. int module = json.get("module").asInt(-1); @@ -53,12 +55,14 @@ default void initCompressors(JsonNode robotMap, String subsystemName) { // Put object in HashMap with its declared name as key after initialization and // configuration. - compressors.put(json.get("name").toString(), compressor); + compressors.put(json.get("name").asText(), compressor); } else - throw new ComponentInitException(String.format( - "[!] UNDECLARED OR EMPTY COMPRESSOR ENTRY IN SUBSYSTEM '%s'", subsystemName.toUpperCase())); + throw new ComponentInitException( + String.format("[!] UNDECLARED, DUPLICATE, OR EMPTY COMPRESSOR ENTRY IN SUBSYSTEM '%s'", + subsystemName.toUpperCase())); } catch (ComponentInitException e) { System.out.println(e.getMessage()); + DriverStation.reportError(e.getMessage(), false); } }); } diff --git a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperDigitalInput.java b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperDigitalInput.java index 13c5874..c54fb22 100644 --- a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperDigitalInput.java +++ b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperDigitalInput.java @@ -9,6 +9,7 @@ import org.usfirst.lib6647.util.ComponentInitException; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DriverStation; /** * Interface to allow {@link DigitalInput} initialization via JSON. Subsystems @@ -36,7 +37,8 @@ default void initDigitalInputs(JsonNode robotMap, String subsystemName) { // Spliterate through each of the elements in the JsonNode. robotMap.get("digitalInputs").spliterator().forEachRemaining(json -> { try { - if (json.hasNonNull("name") && json.hasNonNull("channel")) { + if (json.hasNonNull("name") && !digitalInputs.containsKey(json.get("name").asText()) + && json.hasNonNull("channel")) { // Read values from JsonNode. int channel = json.get("channel").asInt(-1); @@ -44,7 +46,7 @@ default void initDigitalInputs(JsonNode robotMap, String subsystemName) { if (channel < 0) throw new ComponentInitException(String.format( "[!] INVALID OR EMPTY CHANNEL VALUE FOR DIGITALINPUT '%1$s' IN SUBSYSTEM '%2$s'", - json.get("name").toString(), subsystemName)); + json.get("name").asText(), subsystemName)); // Create DigitalInput object. DigitalInput digitalInput = new DigitalInput(json.get("channel").asInt()); @@ -54,13 +56,14 @@ default void initDigitalInputs(JsonNode robotMap, String subsystemName) { // Put object in HashMap with its declared name as key after initialization and // configuration. - digitalInputs.put(json.get("name").toString(), digitalInput); + digitalInputs.put(json.get("name").asText(), digitalInput); } else throw new ComponentInitException( - String.format("[!] UNDECLARED OR EMPTY DIGITALINPUT ENTRY IN SUBSYSTEM '%s'", + String.format("[!] UNDECLARED, DUPLICATE, OR EMPTY DIGITALINPUT ENTRY IN SUBSYSTEM '%s'", subsystemName.toUpperCase())); } catch (ComponentInitException e) { System.out.println(e.getMessage()); + DriverStation.reportError(e.getMessage(), false); } }); } diff --git a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperDoubleSolenoid.java b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperDoubleSolenoid.java index 4bd5675..fac7e7e 100644 --- a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperDoubleSolenoid.java +++ b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperDoubleSolenoid.java @@ -9,6 +9,8 @@ import org.usfirst.lib6647.subsystem.hypercomponents.HyperDoubleSolenoid; import org.usfirst.lib6647.util.ComponentInitException; +import edu.wpi.first.wpilibj.DriverStation; + /** * Interface to allow {@link HyperDoubleSolenoid} initialization via JSON. * Subsystems declared need to extend {@link SuperSubsystem} or @@ -37,7 +39,8 @@ default void initDoubleSolenoids(JsonNode robotMap, String subsystemName) { // Spliterate through each of the elements in the JsonNode. robotMap.get("doubleSolenoids").spliterator().forEachRemaining(json -> { try { - if (json.hasNonNull("name") && json.hasNonNull("forwardChannel") && json.hasNonNull("reverseChannel")) { + if (json.hasNonNull("name") && !doubleSolenoids.containsKey(json.get("name").asText()) + && json.hasNonNull("forwardChannel") && json.hasNonNull("reverseChannel")) { // Read values from JsonNode. int forwardChannel = json.get("forwardChannel").asInt(-1), reverseChannel = json.get("reverseChannel").asInt(-1); @@ -46,7 +49,7 @@ default void initDoubleSolenoids(JsonNode robotMap, String subsystemName) { if (forwardChannel < 0 || reverseChannel < 0) throw new ComponentInitException(String.format( "[!] INVALID OR EMPTY CHANNEL VALUE(S) FOR DOUBLESOLENOID '%1$s' IN SUBSYSTEM '%2$s'", - json.get("name").toString(), subsystemName)); + json.get("name").asText(), subsystemName)); // Create HyperDoubleSolenoid object. HyperDoubleSolenoid doubleSolenoid = new HyperDoubleSolenoid(json.get("forwardChannel").asInt(), @@ -58,13 +61,14 @@ default void initDoubleSolenoids(JsonNode robotMap, String subsystemName) { // Put object in HashMap with its declared name as key after initialization and // configuration. - doubleSolenoids.put(json.get("name").toString(), doubleSolenoid); + doubleSolenoids.put(json.get("name").asText(), doubleSolenoid); } else throw new ComponentInitException( - String.format("[!] UNDECLARED OR EMPTY DOUBLESOLENOID ENTRY IN SUBSYSTEM '%s'", + String.format("[!] UNDECLARED, DUPLICATE, OR EMPTY DOUBLESOLENOID ENTRY IN SUBSYSTEM '%s'", subsystemName.toUpperCase())); } catch (ComponentInitException e) { System.out.println(e.getMessage()); + DriverStation.reportError(e.getMessage(), false); } }); } diff --git a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperEncoder.java b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperEncoder.java index 68febf5..99e0901 100644 --- a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperEncoder.java +++ b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperEncoder.java @@ -9,6 +9,7 @@ import org.usfirst.lib6647.util.ComponentInitException; import org.usfirst.lib6647.util.MotorUtils; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Encoder; /** @@ -36,8 +37,9 @@ default void initEncoders(JsonNode robotMap, String subsystemName) { // Spliterate through each of the elements in the JsonNode. robotMap.get("encoders").spliterator().forEachRemaining(json -> { try { - if (json.hasNonNull("name") && json.hasNonNull("channelA") && json.hasNonNull("channelB") - && json.hasNonNull("reverse") && json.hasNonNull("encodingType")) { + if (json.hasNonNull("name") && !encoders.containsKey(json.get("name").asText()) + && json.hasNonNull("channelA") && json.hasNonNull("channelB") && json.hasNonNull("reverse") + && json.hasNonNull("encodingType")) { // Read values from JsonNode. int channelA = json.get("channelA").asInt(-1), channelB = json.get("channelB").asInt(-1); @@ -45,7 +47,7 @@ default void initEncoders(JsonNode robotMap, String subsystemName) { if (channelA < 0 || channelB < 0) throw new ComponentInitException( String.format("[!] INVALID OR EMPTY VALUE(S) FOR ENCODER '%1$s' IN SUBSYSTEM '%2$s'", - json.get("name").toString(), subsystemName)); + json.get("name").asText(), subsystemName)); // Create Encoder object. Encoder encoder = new Encoder(json.get("channelA").asInt(), json.get("channelB").asInt(), @@ -59,10 +61,12 @@ default void initEncoders(JsonNode robotMap, String subsystemName) { // configuration. encoders.put(json.get("name").asText(), encoder); } else - throw new ComponentInitException(String.format( - "[!] UNDECLARED OR EMPTY ENCODER ENTRY IN SUBSYSTEM '%s'", subsystemName.toUpperCase())); + throw new ComponentInitException( + String.format("[!] UNDECLARED, DUPLICATE, OR EMPTY ENCODER ENTRY IN SUBSYSTEM '%s'", + subsystemName.toUpperCase())); } catch (ComponentInitException e) { System.out.println(e.getMessage()); + DriverStation.reportError(e.getMessage(), false); } }); } diff --git a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperPDP.java b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperPDP.java index 7b0c87d..e1af09f 100644 --- a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperPDP.java +++ b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperPDP.java @@ -8,6 +8,7 @@ import org.usfirst.lib6647.subsystem.SuperSubsystem; import org.usfirst.lib6647.util.ComponentInitException; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistributionPanel; /** @@ -37,7 +38,8 @@ default void initPDPs(JsonNode robotMap, String subsystemName) { // Spliterate through each of the elements in the JsonNode. robotMap.get("PDPs").spliterator().forEachRemaining(json -> { try { - if (json.hasNonNull("name") && json.hasNonNull("module")) { + if (json.hasNonNull("name") && !PDPs.containsKey(json.get("name").asText()) + && json.hasNonNull("module")) { // Read values from JsonNode. int module = json.get("module").asInt(-1); @@ -57,10 +59,12 @@ default void initPDPs(JsonNode robotMap, String subsystemName) { // configuration. PDPs.put(json.get("name").asText(), pdp); } else - throw new ComponentInitException(String.format( - "[!] UNDECLARED OR EMPTY PDP ENTRY IN SUBSYSTEM '%s'", subsystemName.toUpperCase())); + throw new ComponentInitException( + String.format("[!] UNDECLARED, DUPLICATE, OR EMPTY PDP ENTRY IN SUBSYSTEM '%s'", + subsystemName.toUpperCase())); } catch (ComponentInitException e) { System.out.println(e.getMessage()); + DriverStation.reportError(e.getMessage(), false); } }); } diff --git a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperSolenoid.java b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperSolenoid.java index 66b0e83..6591e11 100644 --- a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperSolenoid.java +++ b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperSolenoid.java @@ -9,6 +9,8 @@ import org.usfirst.lib6647.subsystem.hypercomponents.HyperSolenoid; import org.usfirst.lib6647.util.ComponentInitException; +import edu.wpi.first.wpilibj.DriverStation; + /** * Interface to allow {@link HyperSolenoid} initialization via JSON. Subsystems * declared need to extend {@link SuperSubsystem} or {@link PIDSuperSubsystem} @@ -35,7 +37,8 @@ default void initSolenoids(JsonNode robotMap, String subsystemName) { // Spliterate through each of the elements in the JsonNode. robotMap.get("solenoids").spliterator().forEachRemaining(json -> { try { - if (json.hasNonNull("name") && json.hasNonNull("channel")) { + if (json.hasNonNull("name") && !solenoids.containsKey(json.get("name").asText()) + && json.hasNonNull("channel")) { // Read values from JsonNode. int channel = json.get("channel").asInt(-1); @@ -57,10 +60,12 @@ default void initSolenoids(JsonNode robotMap, String subsystemName) { // configuration. solenoids.put(json.get("name").asText(), solenoid); } else - throw new ComponentInitException(String.format( - "[!] UNDECLARED OR EMPTY SOLENOID ENTRY IN SUBSYSTEM '%s'", subsystemName.toUpperCase())); + throw new ComponentInitException( + String.format("[!] UNDECLARED, DUPLICATE, OR EMPTY SOLENOID ENTRY IN SUBSYSTEM '%s'", + subsystemName.toUpperCase())); } catch (ComponentInitException e) { System.out.println(e.getMessage()); + DriverStation.reportError(e.getMessage(), false); } }); } @@ -73,10 +78,6 @@ default void initSolenoids(JsonNode robotMap, String subsystemName) { * @throws ComponentInitException if {@link JsonNode} key is defined, but empty. */ private void setInitialValue(JsonNode json, HyperSolenoid solenoid) throws ComponentInitException { - if (json.get("initialValue").asText().isEmpty()) - throw new ComponentInitException(String.format("[!] EMPTY INITIAL VALUE FOR SOLENOID '%s'.", - json.get("name").asText().toUpperCase())); - solenoid.set(json.get("initialValue").asBoolean()); } diff --git a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperTalon.java b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperTalon.java index e7b053e..6c75453 100644 --- a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperTalon.java +++ b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperTalon.java @@ -11,6 +11,8 @@ import org.usfirst.lib6647.util.ComponentInitException; import org.usfirst.lib6647.util.MotorUtils; +import edu.wpi.first.wpilibj.DriverStation; + /** * Interface to allow {@link HyperTalon} initialization via JSON. Subsystems * declared need to extend {@link SuperSubsystem} or {@link PIDSuperSubsystem} @@ -36,7 +38,8 @@ default void initTalons(JsonNode robotMap, String subsystemName) { // Spliterate through each of the elements in the JsonNode. robotMap.get("talons").spliterator().forEachRemaining(json -> { try { - if (json.hasNonNull("name") && json.hasNonNull("port")) { + if (json.hasNonNull("name") && !talons.containsKey(json.get("name").asText()) + && json.hasNonNull("port")) { // Read values from JsonNode. int port = json.get("port").asInt(-1); @@ -77,12 +80,14 @@ default void initTalons(JsonNode robotMap, String subsystemName) { // Put object in HashMap with its declared name as key after initialization and // configuration. - talons.put(json.get("name").toString(), talon); + talons.put(json.get("name").asText(), talon); } else - throw new ComponentInitException(String.format( - "[!] UNDECLARED OR EMPTY TALON ENTRY IN SUBSYSTEM '%s'", subsystemName.toUpperCase())); + throw new ComponentInitException( + String.format("[!] UNDECLARED, DUPLICATE, OR EMPTY TALON ENTRY IN SUBSYSTEM '%s'", + subsystemName.toUpperCase())); } catch (ComponentInitException e) { System.out.println(e.getMessage()); + DriverStation.reportError(e.getMessage(), false); } }); } @@ -94,20 +99,10 @@ default void initTalons(JsonNode robotMap, String subsystemName) { * * @param {@link JsonNode} * @param {@link HyperTalon} - * @throws ComponentInitException if {@link JsonNode} key is defined, but empty - * or not a number. */ - private void setLimiter(JsonNode json, HyperTalon talon) throws ComponentInitException { - try { - double limiter = json.get("limiter").asDouble(); - talon.setLimiter(limiter < 0.0 ? 0.0 : limiter > 1.0 ? 1.0 : limiter); - } catch (NullPointerException e) { - throw new ComponentInitException( - String.format("[!] EMPTY LIMITER VALUE FOR TALON '%s'.", json.get("name").asText().toUpperCase())); - } catch (NumberFormatException e) { - throw new ComponentInitException(String.format("[!] INVALID LIMITER VALUE FOR TALON '%s'.", - json.get("name").asText().toUpperCase())); - } + private void setLimiter(JsonNode json, HyperTalon talon) { + double limiter = json.get("limiter").asDouble(); + talon.setLimiter(limiter < 0.0 ? 0.0 : limiter > 1.0 ? 1.0 : limiter); } /** @@ -138,13 +133,8 @@ private void setNeutralMode(JsonNode json, HyperTalon talon) throws ComponentIni * * @param {@link JsonNode} * @param {@link HyperTalon} - * @throws ComponentInitException if {@link JsonNode} key is defined, but empty. */ - private void setInverted(JsonNode json, HyperTalon talon) throws ComponentInitException { - if (json.get("inverted").asText().isEmpty()) - throw new ComponentInitException( - String.format("[!] EMPTY INVERTED VALUE FOR TALON '%s'.", json.get("name").asText().toUpperCase())); - + private void setInverted(JsonNode json, HyperTalon talon) { talon.setInverted(json.get("inverted").asBoolean()); } @@ -153,25 +143,15 @@ private void setInverted(JsonNode json, HyperTalon talon) throws ComponentInitEx * * @param {@link JsonNode} * @param {@link HyperTalon} - * @throws ComponentInitException if {@link JsonNode} key is not found, or its - * subkeys are invalid or empty. */ - private void setClosedloopRamp(JsonNode json, HyperTalon talon) throws ComponentInitException { - try { - JsonNode closed = json.get("loopRamp").get("closed"); - - if (closed.hasNonNull("timeoutMs")) - talon.configClosedloopRamp(closed.get("secondsFromNeutralToFull").asDouble(), - closed.get("timeoutMs").asInt()); - else - talon.configClosedloopRamp(closed.get("secondsFromNeutralToFull").asDouble()); - } catch (NullPointerException e) { - throw new ComponentInitException(String.format("[!] EMPTY CLOSEDLOOP RAMP VALUE(S) FOR TALON '%s'.", - json.get("name").asText().toUpperCase())); - } catch (NumberFormatException e) { - throw new ComponentInitException(String.format("[!] INVALID CLOSEDLOOP RAMP VALUE(S) FOR TALON '%s'.", - json.get("name").asText().toUpperCase())); - } + private void setClosedloopRamp(JsonNode json, HyperTalon talon) { + JsonNode closed = json.get("loopRamp").get("closed"); + + if (closed.hasNonNull("timeoutMs")) + talon.configClosedloopRamp(closed.get("secondsFromNeutralToFull").asDouble(), + closed.get("timeoutMs").asInt()); + else + talon.configClosedloopRamp(closed.get("secondsFromNeutralToFull").asDouble()); } /** @@ -179,25 +159,14 @@ private void setClosedloopRamp(JsonNode json, HyperTalon talon) throws Component * * @param {@link JsonNode} * @param {@link HyperTalon} - * @throws ComponentInitException if {@link JsonNode} key is not found, or its - * subkeys are invalid or empty. */ - private void setOpenloopRamp(JsonNode json, HyperTalon talon) throws ComponentInitException { - try { - JsonNode open = json.get("loopRamp").get("open"); - - if (open.hasNonNull("timeoutMs")) - talon.configOpenloopRamp(open.get("secondsFromNeutralToFull").asDouble(), - open.get("timeoutMs").asInt()); - else - talon.configOpenloopRamp(open.get("secondsFromNeutralToFull").asDouble()); - } catch (NullPointerException e) { - throw new ComponentInitException(String.format("[!] EMPTY OPENLOOP RAMP VALUE(S) FOR TALON '%s'.", - json.get("name").asText().toUpperCase())); - } catch (NumberFormatException e) { - throw new ComponentInitException(String.format("[!] INVALID OPENLOOP RAMP VALUE(S) FOR TALON '%s'.", - json.get("name").asText().toUpperCase())); - } + private void setOpenloopRamp(JsonNode json, HyperTalon talon) { + JsonNode open = json.get("loopRamp").get("open"); + + if (open.hasNonNull("timeoutMs")) + talon.configOpenloopRamp(open.get("secondsFromNeutralToFull").asDouble(), open.get("timeoutMs").asInt()); + else + talon.configOpenloopRamp(open.get("secondsFromNeutralToFull").asDouble()); } /** @@ -206,28 +175,18 @@ private void setOpenloopRamp(JsonNode json, HyperTalon talon) throws ComponentIn * * @param {@link JsonNode} * @param {@link HyperTalon} - * @throws ComponentInitException if {@link JsonNode} key is not found, or its - * subkeys are invalid or empty. */ - private void setSensors(JsonNode json, HyperTalon talon) throws ComponentInitException { - try { - JsonNode sensor = json.get("sensor"); - JsonNode feedback = sensor.get("feedback"); - - talon.configSelectedFeedbackSensor(getFeedbackDevice(feedback.get("feedbackDevice").asText()), - feedback.get("pidIdx").asInt(), feedback.get("timeoutMs").asInt()); - - talon.setSensorPhase(sensor.get("phase").asBoolean()); - - talon.setSelectedSensorPosition(sensor.get("sensorPos").asInt(), sensor.get("pidIdx").asInt(), - sensor.get("timeoutMs").asInt()); - } catch (NullPointerException e) { - throw new ComponentInitException(String.format("[!] EMPTY SENSOR VALUE(S) FOR TALON '%s'.", - json.get("name").asText().toUpperCase())); - } catch (NumberFormatException e) { - throw new ComponentInitException(String.format("[!] INVALID SENSOR VALUE(S) FOR TALON '%s'.", - json.get("name").asText().toUpperCase())); - } + private void setSensors(JsonNode json, HyperTalon talon) { + JsonNode sensor = json.get("sensor"); + JsonNode feedback = sensor.get("feedback"); + + talon.configSelectedFeedbackSensor(getFeedbackDevice(feedback.get("feedbackDevice").asText()), + feedback.get("pidIdx").asInt(), feedback.get("timeoutMs").asInt()); + + talon.setSensorPhase(sensor.get("phase").asBoolean()); + + talon.setSelectedSensorPosition(sensor.get("sensorPos").asInt(), sensor.get("pidIdx").asInt(), + sensor.get("timeoutMs").asInt()); } /** @@ -235,25 +194,15 @@ private void setSensors(JsonNode json, HyperTalon talon) throws ComponentInitExc * * @param {@link JsonNode} * @param {@link HyperTalon} - * @throws ComponentInitException if {@link JsonNode} key is not found, or its - * subkeys are invalid or empty. */ - private void setPIDValues(JsonNode json, HyperTalon talon) throws ComponentInitException { - try { - JsonNode pid = json.get("pid"); - int slotIdx = pid.get("slotIdx").asInt(); - - talon.config_kP(slotIdx, pid.get("p").asDouble()); - talon.config_kI(slotIdx, pid.get("i").asDouble()); - talon.config_kD(slotIdx, pid.get("d").asDouble()); - talon.config_kF(slotIdx, pid.get("f").asDouble()); - } catch (NullPointerException e) { - throw new ComponentInitException( - String.format("[!] EMPTY PID VALUE(S) FOR TALON '%s'.", json.get("name").asText().toUpperCase())); - } catch (NumberFormatException e) { - throw new ComponentInitException( - String.format("[!] INVALID PID VALUE(S) FOR TALON '%s'.", json.get("name").asText().toUpperCase())); - } + private void setPIDValues(JsonNode json, HyperTalon talon) { + JsonNode pid = json.get("pid"); + int slotIdx = pid.get("slotIdx").asInt(); + + talon.config_kP(slotIdx, pid.get("p").asDouble()); + talon.config_kI(slotIdx, pid.get("i").asDouble()); + talon.config_kD(slotIdx, pid.get("d").asDouble()); + talon.config_kF(slotIdx, pid.get("f").asDouble()); } /** diff --git a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperUltrasonic.java b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperUltrasonic.java index 38c2cce..70e4b22 100644 --- a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperUltrasonic.java +++ b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperUltrasonic.java @@ -8,6 +8,7 @@ import org.usfirst.lib6647.subsystem.SuperSubsystem; import org.usfirst.lib6647.util.ComponentInitException; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Ultrasonic; /** @@ -35,7 +36,8 @@ default void initUltrasonics(JsonNode robotMap, String subsystemName) { // Spliterate through each of the elements in the JsonNode. robotMap.get("ultrasonics").spliterator().forEachRemaining(json -> { try { - if (json.hasNonNull("name") && json.hasNonNull("pingChannel") && json.hasNonNull("echoChannel")) { + if (json.hasNonNull("name") && !ultrasonics.containsKey(json.get("name").asText()) + && json.hasNonNull("pingChannel") && json.hasNonNull("echoChannel")) { // Read values from JsonNode. int pingChannel = json.get("pingChannel").asInt(-1), echoChannel = json.get("echoChannel").asInt(-1); @@ -56,10 +58,12 @@ default void initUltrasonics(JsonNode robotMap, String subsystemName) { // configuration. ultrasonics.put(json.get("name").asText(), ultrasonic); } else - throw new ComponentInitException(String.format( - "[!] UNDECLARED OR EMPTY ULTRASONIC ENTRY IN SUBSYSTEM '%s'", subsystemName.toUpperCase())); + throw new ComponentInitException( + String.format("[!] UNDECLARED, DUPLICATE, OR EMPTY ULTRASONIC ENTRY IN SUBSYSTEM '%s'", + subsystemName.toUpperCase())); } catch (ComponentInitException e) { System.out.println(e.getMessage()); + DriverStation.reportError(e.getMessage(), false); } }); } diff --git a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperVictor.java b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperVictor.java index ac995e2..0319bf8 100644 --- a/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperVictor.java +++ b/src/main/java/org/usfirst/lib6647/subsystem/supercomponents/SuperVictor.java @@ -12,6 +12,8 @@ import org.usfirst.lib6647.util.ComponentInitException; import org.usfirst.lib6647.util.MotorUtils; +import edu.wpi.first.wpilibj.DriverStation; + /** * Interface to allow {@link HyperVictor} initialization via JSON. Subsystems * declared need to extend {@link SuperSubsystem} or {@link PIDSuperSubsystem} @@ -38,7 +40,8 @@ default void initVictors(JsonNode robotMap, String subsystemName) { // Spliterate through each of the elements in the JsonNode. robotMap.get("victors").spliterator().forEachRemaining(json -> { try { - if (json.hasNonNull("name") && json.hasNonNull("port")) { + if (json.hasNonNull("name") && !victors.containsKey(json.get("name").asText()) + && json.hasNonNull("port")) { // Read values from JsonNode. int port = json.get("port").asInt(-1); @@ -52,7 +55,7 @@ default void initVictors(JsonNode robotMap, String subsystemName) { HyperVictor victor = new HyperVictor(json.get("port").asInt()); // Additional initialization configuration. - victor.setName(json.get("name").toString()); + victor.setName(json.get("name").asText()); if (json.hasNonNull("limiter")) setLimiter(json, victor); @@ -75,10 +78,12 @@ default void initVictors(JsonNode robotMap, String subsystemName) { // configuration. victors.put(json.get("name").asText(), victor); } else - throw new ComponentInitException(String.format( - "[!] UNDECLARED OR EMPTY VICTOR ENTRY IN SUBSYSTEM '%s'", subsystemName.toUpperCase())); + throw new ComponentInitException( + String.format("[!] UNDECLARED, DUPLICATE, OR EMPTY VICTOR ENTRY IN SUBSYSTEM '%s'", + subsystemName.toUpperCase())); } catch (ComponentInitException e) { System.out.println(e.getMessage()); + DriverStation.reportError(e.getMessage(), false); } }); } @@ -90,20 +95,10 @@ default void initVictors(JsonNode robotMap, String subsystemName) { * * @param {@link JsonNode} * @param {@link HyperVictor} - * @throws ComponentInitException if {@link JsonNode} key is defined, but empty - * or not a number. */ - private void setLimiter(JsonNode json, HyperVictor victor) throws ComponentInitException { - try { - double limiter = json.get("limiter").asDouble(); - victor.setLimiter(limiter < 0.0 ? 0.0 : limiter > 1.0 ? 1.0 : limiter); - } catch (NullPointerException e) { - throw new ComponentInitException( - String.format("[!] EMPTY LIMITER VALUE FOR VICTOR '%s'.", json.get("name").asText().toUpperCase())); - } catch (NumberFormatException e) { - throw new ComponentInitException(String.format("[!] INVALID LIMITER VALUE FOR VICTOR '%s'.", - json.get("name").asText().toUpperCase())); - } + private void setLimiter(JsonNode json, HyperVictor victor) { + double limiter = json.get("limiter").asDouble(); + victor.setLimiter(limiter < 0.0 ? 0.0 : limiter > 1.0 ? 1.0 : limiter); } /** @@ -111,13 +106,8 @@ private void setLimiter(JsonNode json, HyperVictor victor) throws ComponentInitE * * @param {@link JsonNode} * @param {@link HyperVictor} - * @throws ComponentInitException if {@link JsonNode} key is defined, but empty. */ - private void setInverted(JsonNode json, HyperVictor victor) throws ComponentInitException { - if (json.get("inverted").asText().isEmpty()) - throw new ComponentInitException(String.format("[!] EMPTY INVERTED VALUE FOR VICTOR '%s'.", - json.get("name").asText().toUpperCase())); - + private void setInverted(JsonNode json, HyperVictor victor) { victor.setInverted(json.get("inverted").asBoolean()); } @@ -150,25 +140,15 @@ private void setNeutralMode(JsonNode json, HyperVictor victor) throws ComponentI * * @param {@link JsonNode} * @param {@link HyperVictor} - * @throws ComponentInitException if {@link JsonNode} key is not found, or its - * subkeys are invalid or empty. */ - private void setClosedloopRamp(JsonNode json, HyperVictor victor) throws ComponentInitException { - try { - JsonNode closed = json.get("loopRamp").get("closed"); - - if (closed.hasNonNull("timeoutMs")) - victor.configClosedloopRamp(closed.get("secondsFromNeutralToFull").asDouble(), - closed.get("timeoutMs").asInt()); - else - victor.configClosedloopRamp(closed.get("secondsFromNeutralToFull").asDouble()); - } catch (NullPointerException e) { - throw new ComponentInitException(String.format("[!] EMPTY CLOSEDLOOP RAMP VALUE(S) FOR VICTOR '%s'.", - json.get("name").asText().toUpperCase())); - } catch (NumberFormatException e) { - throw new ComponentInitException(String.format("[!] INVALID CLOSEDLOOP RAMP VALUE(S) FOR VICTOR '%s'.", - json.get("name").asText().toUpperCase())); - } + private void setClosedloopRamp(JsonNode json, HyperVictor victor) { + JsonNode closed = json.get("loopRamp").get("closed"); + + if (closed.hasNonNull("timeoutMs")) + victor.configClosedloopRamp(closed.get("secondsFromNeutralToFull").asDouble(), + closed.get("timeoutMs").asInt()); + else + victor.configClosedloopRamp(closed.get("secondsFromNeutralToFull").asDouble()); } /** @@ -176,25 +156,14 @@ private void setClosedloopRamp(JsonNode json, HyperVictor victor) throws Compone * * @param {@link JsonNode} * @param {@link HyperVictor} - * @throws ComponentInitException if {@link JsonNode} key is not found, or its - * subkeys are invalid or empty. */ - private void setOpenloopRamp(JsonNode json, HyperVictor victor) throws ComponentInitException { - try { - JsonNode open = json.get("loopRamp").get("open"); - - if (open.hasNonNull("timeoutMs")) - victor.configOpenloopRamp(open.get("secondsFromNeutralToFull").asDouble(), - open.get("timeoutMs").asInt()); - else - victor.configOpenloopRamp(open.get("secondsFromNeutralToFull").asDouble()); - } catch (NullPointerException e) { - throw new ComponentInitException(String.format("[!] EMPTY OPENLOOP RAMP VALUE(S) FOR VICTOR '%s'.", - json.get("name").asText().toUpperCase())); - } catch (NumberFormatException e) { - throw new ComponentInitException(String.format("[!] INVALID OPENLOOP RAMP VALUE(S) FOR VICTOR '%s'.", - json.get("name").asText().toUpperCase())); - } + private void setOpenloopRamp(JsonNode json, HyperVictor victor) { + JsonNode open = json.get("loopRamp").get("open"); + + if (open.hasNonNull("timeoutMs")) + victor.configOpenloopRamp(open.get("secondsFromNeutralToFull").asDouble(), open.get("timeoutMs").asInt()); + else + victor.configOpenloopRamp(open.get("secondsFromNeutralToFull").asDouble()); } /**