From 6d6e3c1692be47416b5a59a18b8690d6015d02e5 Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Sun, 5 Nov 2023 22:07:36 -0600 Subject: [PATCH] Code cleanup --- .../lasarobotics/battery/BatteryTracker.java | 2 ++ .../org/lasarobotics/hardware/Analog.java | 2 ++ .../org/lasarobotics/hardware/DMAService.java | 7 ++-- .../lasarobotics/hardware/LimitSwitch.java | 2 ++ .../java/org/lasarobotics/hardware/NavX2.java | 2 ++ .../org/lasarobotics/hardware/SparkMax.java | 32 +++++++++++++++++-- .../org/lasarobotics/led/LEDSubsystem.java | 13 +++----- .../lasarobotics/utils/SparkPIDConfig.java | 8 ++--- .../lasarobotics/utils/TalonPIDConfig.java | 14 ++++---- 9 files changed, 57 insertions(+), 25 deletions(-) diff --git a/src/main/java/org/lasarobotics/battery/BatteryTracker.java b/src/main/java/org/lasarobotics/battery/BatteryTracker.java index df5043ed..29c634dd 100644 --- a/src/main/java/org/lasarobotics/battery/BatteryTracker.java +++ b/src/main/java/org/lasarobotics/battery/BatteryTracker.java @@ -26,6 +26,8 @@ public Hardware(BatteryScanner scanner) { /** * Create battery tracker + *

+ * Uses a Waveshare barcode scanner (https://a.co/d/ij6mFif) * @param batteryHardware Hardware devices required by battery tracker */ public BatteryTracker(Hardware batteryHardware) { diff --git a/src/main/java/org/lasarobotics/hardware/Analog.java b/src/main/java/org/lasarobotics/hardware/Analog.java index f9377ce5..a0b1862d 100644 --- a/src/main/java/org/lasarobotics/hardware/Analog.java +++ b/src/main/java/org/lasarobotics/hardware/Analog.java @@ -52,6 +52,7 @@ private void updateInputs() { /** * Call this method periodically */ + @Override public void periodic() { updateInputs(); Logger.getInstance().processInputs(m_id.name, m_inputs); @@ -61,6 +62,7 @@ public void periodic() { * Get latest sensor input data * @return Latest sensor data */ + @Override public AnalogInputsAutoLogged getInputs() { return m_inputs; } diff --git a/src/main/java/org/lasarobotics/hardware/DMAService.java b/src/main/java/org/lasarobotics/hardware/DMAService.java index ed8b9c9b..9cf1ccde 100644 --- a/src/main/java/org/lasarobotics/hardware/DMAService.java +++ b/src/main/java/org/lasarobotics/hardware/DMAService.java @@ -1,3 +1,7 @@ +// Copyright (c) LASA Robotics and other contributors +// Open Source Software; you can modify and/or share it under the terms of +// the MIT license file in the root directory of this project + package org.lasarobotics.hardware; import org.lasarobotics.utils.GlobalConstants; @@ -5,9 +9,6 @@ import edu.wpi.first.wpilibj.DMA; import edu.wpi.first.wpilibj.DMASample; -// Copyright (c) LASA Robotics and other contributors -// Open Source Software; you can modify and/or share it under the terms of -// the MIT license file in the root directory of this project. import edu.wpi.first.wpilibj.DMASample.DMAReadStatus; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/org/lasarobotics/hardware/LimitSwitch.java b/src/main/java/org/lasarobotics/hardware/LimitSwitch.java index 38e8481a..d39e870b 100644 --- a/src/main/java/org/lasarobotics/hardware/LimitSwitch.java +++ b/src/main/java/org/lasarobotics/hardware/LimitSwitch.java @@ -72,6 +72,7 @@ private void updateInputs() { /** * Call this method periodically */ + @Override public void periodic() { updateInputs(); Logger.getInstance().processInputs(m_id.name, m_inputs); @@ -81,6 +82,7 @@ public void periodic() { * Get latest sensor input data * @return Latest sensor data */ + @Override public LimitSwitchInputsAutoLogged getInputs() { return m_inputs; } diff --git a/src/main/java/org/lasarobotics/hardware/NavX2.java b/src/main/java/org/lasarobotics/hardware/NavX2.java index e937a514..15c288ce 100644 --- a/src/main/java/org/lasarobotics/hardware/NavX2.java +++ b/src/main/java/org/lasarobotics/hardware/NavX2.java @@ -146,6 +146,7 @@ private void updateInputs() { /** * Call this method periodically */ + @Override public void periodic() { updateInputs(); Logger.getInstance().processInputs(m_name, m_inputs); @@ -155,6 +156,7 @@ public void periodic() { * Get latest sensor input data * @return Latest NavX data */ + @Override public NavX2InputsAutoLogged getInputs() { return m_inputs; } diff --git a/src/main/java/org/lasarobotics/hardware/SparkMax.java b/src/main/java/org/lasarobotics/hardware/SparkMax.java index ca71e80b..cbf7a2a0 100644 --- a/src/main/java/org/lasarobotics/hardware/SparkMax.java +++ b/src/main/java/org/lasarobotics/hardware/SparkMax.java @@ -199,6 +199,32 @@ private double getAbsoluteEncoderVelocity() { return getAbsoluteEncoder().getVelocity(); } + /** + * Returns an object for interfacing with the forward limit switch connected to the appropriate + * pins on the data port. + * + *

This call will disable support for the alternate encoder. + * + * @param switchType Whether the limit switch is normally open or normally closed. + * @return An object for interfacing with the forward limit switch. + */ + private SparkMaxLimitSwitch getForwardLimitSwitch() { + return m_spark.getForwardLimitSwitch(m_limitSwitchType); + } + + /** + * Returns an object for interfacing with the reverse limit switch connected to the appropriate + * pins on the data port. + * + *

This call will disable support for the alternate encoder. + * + * @param switchType Whether the limit switch is normally open or normally closed. + * @return An object for interfacing with the reverse limit switch. + */ + private SparkMaxLimitSwitch getReverseLimitSwitch() { + return m_spark.getReverseLimitSwitch(m_limitSwitchType); + } + /** * Update sensor input readings */ @@ -209,8 +235,8 @@ private void updateInputs() { m_inputs.analogVelocity = getAnalogVelocity(); m_inputs.absoluteEncoderPosition = getAbsoluteEncoderPosition(); m_inputs.absoluteEncoderVelocity = getAbsoluteEncoderVelocity(); - m_inputs.forwardLimitSwitch = m_spark.getForwardLimitSwitch(m_limitSwitchType).isPressed(); - m_inputs.reverseLimitSwitch = m_spark.getReverseLimitSwitch(m_limitSwitchType).isPressed(); + m_inputs.forwardLimitSwitch = getForwardLimitSwitch().isPressed(); + m_inputs.reverseLimitSwitch = getReverseLimitSwitch().isPressed(); } /** @@ -236,6 +262,7 @@ public void addToSimulation(DCMotor motor) { /** * Call this method periodically */ + @Override public void periodic() { updateInputs(); Logger.getInstance().processInputs(m_id.name, m_inputs); @@ -247,6 +274,7 @@ public void periodic() { * Get latest sensor input data * @return Latest sensor data */ + @Override public SparkMaxInputsAutoLogged getInputs() { return m_inputs; } diff --git a/src/main/java/org/lasarobotics/led/LEDSubsystem.java b/src/main/java/org/lasarobotics/led/LEDSubsystem.java index cbd84e04..49aa9361 100644 --- a/src/main/java/org/lasarobotics/led/LEDSubsystem.java +++ b/src/main/java/org/lasarobotics/led/LEDSubsystem.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LEDSubsystem extends SubsystemBase implements AutoCloseable { - private static boolean m_override = false; private static Pattern m_overridePattern = Pattern.TEAM_COLOR_SOLID; private static LEDSubsystem m_subsystem; @@ -35,14 +34,9 @@ private void setLEDs() { for (LEDStrip ledStrip : m_ledStrips) ledStrip.update(); } - private void overrideLEDs() { - for (LEDStrip ledStrip : m_ledStrips) ledStrip.set(m_overridePattern, Section.FULL); - } - @Override public void periodic() { // This method will be called once per scheduler run - if (m_override) overrideLEDs(); setLEDs(); } @@ -51,13 +45,14 @@ public void add(LEDStrip... ledStrips) { } public void startOverride(Pattern pattern) { - m_override = true; m_overridePattern = pattern; - for (LEDStrip ledStrip : m_ledStrips) ledStrip.startOverride(); + for (LEDStrip ledStrip : m_ledStrips) { + ledStrip.startOverride(); + ledStrip.set(m_overridePattern, Section.FULL); + } } public void endOverride() { - m_override = false; m_overridePattern = Pattern.TEAM_COLOR_SOLID; for (LEDStrip ledStrip : m_ledStrips) ledStrip.endOverride(); } diff --git a/src/main/java/org/lasarobotics/utils/SparkPIDConfig.java b/src/main/java/org/lasarobotics/utils/SparkPIDConfig.java index 55c63ea9..faf0f5af 100644 --- a/src/main/java/org/lasarobotics/utils/SparkPIDConfig.java +++ b/src/main/java/org/lasarobotics/utils/SparkPIDConfig.java @@ -16,7 +16,7 @@ public class SparkPIDConfig { private static final double MAX_VOLTAGE = 12.0; private static final int PID_SLOT = 0; - + private boolean m_enableSoftLimits = true; private boolean m_sensorPhase = false; @@ -83,7 +83,7 @@ public SparkPIDConfig(PIDConstants pidf, boolean sensorPhase, boolean invertMoto * @param forwardLimitSwitch Enable forward limit switch * @param reverseLimitSwitch Enable reverse limit switch */ - public void initializeSparkPID(CANSparkMax spark, MotorFeedbackSensor feedbackSensor, + public void initializeSparkPID(CANSparkMax spark, MotorFeedbackSensor feedbackSensor, boolean forwardLimitSwitch, boolean reverseLimitSwitch) { // Reset Spark to default spark.restoreFactoryDefaults(); @@ -96,7 +96,7 @@ public void initializeSparkPID(CANSparkMax spark, MotorFeedbackSensor feedbackSe pidController.setFeedbackDevice(feedbackSensor); feedbackSensor.setInverted(m_sensorPhase); } catch (IllegalArgumentException e) {} - + // Configure forward and reverse soft limits if (m_enableSoftLimits) { spark.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, (float) m_upperLimit); @@ -141,7 +141,7 @@ public void initializeSparkPID(CANSparkMax spark, MotorFeedbackSensor feedbackSe /** * Initializes Spark PID *

- * Calls {@link SparkPIDConfig#initializeSparkPID(CANSparkMax, MotorFeedbackSensor, boolean, boolean)} with no limit switches + * Calls {@link SparkPIDConfig#initializeSparkPID(CANSparkMax, MotorFeedbackSensor, boolean, boolean)} with no limit switches * @param spark Spark motor controller to apply settings to * @param feedbackSensor Feedback device to use for Spark PID */ diff --git a/src/main/java/org/lasarobotics/utils/TalonPIDConfig.java b/src/main/java/org/lasarobotics/utils/TalonPIDConfig.java index 41148833..9d5bec68 100644 --- a/src/main/java/org/lasarobotics/utils/TalonPIDConfig.java +++ b/src/main/java/org/lasarobotics/utils/TalonPIDConfig.java @@ -11,7 +11,7 @@ import edu.wpi.first.math.MathUtil; -/** +/** * Automates the configuration of Talon PID and MotionMagic parameters */ public class TalonPIDConfig { @@ -93,7 +93,7 @@ public TalonPIDConfig(PIDConstants pidf, boolean sensorPhase, boolean invertMoto this.m_lowerLimit = lowerLimit; this.m_upperLimit = upperLimit; this.m_enableSoftLimits = enableSoftLimits; - + this.m_velocityRPM = velocity; this.m_accelerationRPMPerSec = acceleration; this.m_motionSmoothing = MathUtil.clamp(motionSmoothing, MIN_MOTION_SMOOTHING, MAX_MOTION_SMOOTHING); @@ -108,14 +108,14 @@ public TalonPIDConfig(PIDConstants pidf, boolean sensorPhase, boolean invertMoto * @param forwardLimitSwitch Enable forward limit switch * @param reverseLimitSwitch Enable reverse limit switch */ - public void initializeTalonPID(BaseTalon talon, FeedbackDevice feedbackDevice, + public void initializeTalonPID(BaseTalon talon, FeedbackDevice feedbackDevice, boolean forwardLimitSwitch, boolean reverseLimitSwitch) { // Reset Talon to default talon.configFactoryDefault(); // Configure feedback sensor talon.configSelectedFeedbackSensor(feedbackDevice); - + // Configure forward and reverse soft limits if (m_enableSoftLimits) { talon.configForwardSoftLimitThreshold(m_upperLimit); @@ -153,17 +153,17 @@ public void initializeTalonPID(BaseTalon talon, FeedbackDevice feedbackDevice, talon.enableVoltageCompensation(true); // Configure MotionMagic values - if (m_motionMagic) { + if (m_motionMagic) { talon.configMotionCruiseVelocity(rpmToTicksPer100ms(m_velocityRPM)); talon.configMotionAcceleration(rpmToTicksPer100ms(m_accelerationRPMPerSec)); talon.configMotionSCurveStrength(m_motionSmoothing); - } + } } /** * Initializes Talon PID and MotionMagic parameters *

- * Calls {@link TalonPIDConfig#initializeTalonPID(BaseTalon, FeedbackDevice, boolean, boolean)} with no limit switches + * Calls {@link TalonPIDConfig#initializeTalonPID(BaseTalon, FeedbackDevice, boolean, boolean)} with no limit switches * @param talon Talon motor controller to apply settings to * @param feedbackDevice Feedback device to use for Talon PID */