From e9064e4906563a535b6d0722fad6ee3a84d6cd59 Mon Sep 17 00:00:00 2001 From: Danny Lin <75951607+dannynotsmart@users.noreply.github.com> Date: Sat, 11 May 2024 01:21:58 -0400 Subject: [PATCH 01/14] updated Hydrogen --- README.md | 16 +- build.gradle | 2 +- networktables.json | 4 +- networktables.json.bck | 3 + .../java/org/sciborgs1155/lib/FakePDH.java | 39 ++ .../org/sciborgs1155/lib/FaultLogger.java | 443 ++++++++++++++---- .../org/sciborgs1155/lib/InputStream.java | 61 ++- .../java/org/sciborgs1155/lib/SparkUtils.java | 153 ++++-- .../java/org/sciborgs1155/lib/TalonUtils.java | 101 ++++ .../org/sciborgs1155/lib/TestingUtil.java | 45 +- .../java/org/sciborgs1155/lib/Tuning.java | 74 +++ .../org/sciborgs1155/robot/Constants.java | 116 ++++- .../java/org/sciborgs1155/robot/Robot.java | 122 +++-- .../sciborgs1155/robot/commands/Autos.java | 49 +- .../org/sciborgs1155/robot/drive/Drive.java | 361 +++++++++----- .../robot/drive/DriveConstants.java | 78 +-- .../sciborgs1155/robot/drive/FlexModule.java | 121 ----- .../org/sciborgs1155/robot/drive/GyroIO.java | 52 +- .../sciborgs1155/robot/drive/ModuleIO.java | 14 +- .../sciborgs1155/robot/drive/NavXGyro.java | 32 ++ .../org/sciborgs1155/robot/drive/NoGyro.java | 24 + .../sciborgs1155/robot/drive/NoModule.java | 33 ++ .../sciborgs1155/robot/drive/SimModule.java | 15 +- .../sciborgs1155/robot/drive/SparkModule.java | 143 ++++++ .../robot/drive/SwerveModule.java | 146 ++++-- .../sciborgs1155/robot/drive/TalonModule.java | 25 +- .../org/sciborgs1155/robot/vision/Vision.java | 181 +++++++ .../robot/vision/VisionConstants.java | 64 +++ .../org/sciborgs1155/lib/InputStreamTest.java | 4 +- .../org/sciborgs1155/robot/RobotTest.java | 4 +- vendordeps/PathplannerLib.json | 6 +- vendordeps/Phoenix6.json | 48 +- vendordeps/REVLib.json | 10 +- vendordeps/URCL.json | 12 +- vendordeps/photonlib.json | 33 +- 35 files changed, 1984 insertions(+), 650 deletions(-) create mode 100644 networktables.json.bck create mode 100644 src/main/java/org/sciborgs1155/lib/FakePDH.java create mode 100644 src/main/java/org/sciborgs1155/lib/TalonUtils.java create mode 100644 src/main/java/org/sciborgs1155/lib/Tuning.java delete mode 100644 src/main/java/org/sciborgs1155/robot/drive/FlexModule.java create mode 100644 src/main/java/org/sciborgs1155/robot/drive/NavXGyro.java create mode 100644 src/main/java/org/sciborgs1155/robot/drive/NoGyro.java create mode 100644 src/main/java/org/sciborgs1155/robot/drive/NoModule.java create mode 100644 src/main/java/org/sciborgs1155/robot/drive/SparkModule.java create mode 100644 src/main/java/org/sciborgs1155/robot/vision/Vision.java create mode 100644 src/main/java/org/sciborgs1155/robot/vision/VisionConstants.java diff --git a/README.md b/README.md index e9b0795..3a7141d 100644 --- a/README.md +++ b/README.md @@ -1,15 +1,23 @@ # Hydrogen -The SciBorgs' base repository. - -WIP +The SciBorgs' base repository. It is a current work-in-progress. ## Structure Our robot code is centered around [Robot.java](src/main/java/org/sciborgs1155/robot/Robot.java) +This project currently contains drive, vision, and autos. You are expected to add/modify code supporting existing files and new subsystems when using this template. Some of these files include but are not limited to: +- **[Autos.java](src/main/java/org/sciborgs1155/robot/commands/Autos.java)** Add code for new subsystems in `configureAutos`, such as commands for `NamedCommands` +- **[VisionConstants.java](src/main/java/org/sciborgs1155/robot/vision/VisionConstants.java)** Add new `CameraConfig` objects representing cameras on the robot in the field `CAMERAS`. Also modify any camera configurations or constants if needed. +- **[Constants.java](src/main/java/org/sciborgs1155/robot/Constants.java)** Modify the class `Field` to be updated for each year's game, and any other constants if needed. +- **[Ports.java](src/main/java/org/sciborgs1155/robot/Ports.java)** Modify existing OI and drive ports, as well as adding new ports. +- **[Robot.java](src/main/java/org/sciborgs1155/robot/Robot.java)** A lot needs to be added/modified in this file, self-explanatory. + ## Dependencies - [WPILib](https://docs.wpilib.org/) - [Spotless](https://github.com/diffplug/spotless/blob/main/plugin-gradle/README.md) - [Monologue](https://github.com/shueja/Monologue) - [REVLib](https://docs.revrobotics.com/sparkmax/software-resources/spark-max-api-information) -- [PathPlannerLib](https://github.com/mjansen4857/pathplanner/wiki) +- [PathPlannerLib](https://pathplanner.dev/home.html) - [PhotonLib](https://docs.photonvision.org/en/latest/docs/programming/photonlib/adding-vendordep.html) +- [Phoenix6](https://v6.docs.ctr-electronics.com/en/stable/) +- [URCL](https://github.com/Mechanical-Advantage/URCL) +- [NavX](https://pdocs.kauailabs.com/navx-mxp/software/roborio-libraries/java/) diff --git a/build.gradle b/build.gradle index 9eb3d2c..76471f2 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" id 'com.diffplug.spotless' version '6.23.3' } diff --git a/networktables.json b/networktables.json index fe51488..41b42e6 100644 --- a/networktables.json +++ b/networktables.json @@ -1 +1,3 @@ -[] +[ + +] diff --git a/networktables.json.bck b/networktables.json.bck new file mode 100644 index 0000000..41b42e6 --- /dev/null +++ b/networktables.json.bck @@ -0,0 +1,3 @@ +[ + +] diff --git a/src/main/java/org/sciborgs1155/lib/FakePDH.java b/src/main/java/org/sciborgs1155/lib/FakePDH.java new file mode 100644 index 0000000..b3a5899 --- /dev/null +++ b/src/main/java/org/sciborgs1155/lib/FakePDH.java @@ -0,0 +1,39 @@ +package org.sciborgs1155.lib; + +import com.revrobotics.CANSparkBase; +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; + +/** + * Our actual PDH can't connect to CAN, so this is a workaround to (poorly) estimate our total used + * current based on the stator currents of all our motors with the formula: + * + *

supply current = battery voltage * applied output * stator current + */ +public class FakePDH { + private static final List sparks = new ArrayList<>(); + private static double statorCurrent; + private static double supplyCurrent; + + public static void register(CANSparkBase spark) { + sparks.add(spark); + } + + public static double update() { + double voltage = RobotController.getBatteryVoltage(); + double output = sparks.stream().mapToDouble(CANSparkBase::getAppliedOutput).sum(); + statorCurrent = + sparks.stream().mapToDouble(CANSparkBase::getOutputCurrent).map(Math::abs).sum(); + supplyCurrent = voltage * output * statorCurrent; + return supplyCurrent; + } + + public static double supplyCurrent() { + return supplyCurrent; + } + + public static double statorCurrent() { + return statorCurrent; + } +} diff --git a/src/main/java/org/sciborgs1155/lib/FaultLogger.java b/src/main/java/org/sciborgs1155/lib/FaultLogger.java index 4028180..9a26825 100644 --- a/src/main/java/org/sciborgs1155/lib/FaultLogger.java +++ b/src/main/java/org/sciborgs1155/lib/FaultLogger.java @@ -1,55 +1,46 @@ package org.sciborgs1155.lib; +import com.ctre.phoenix6.hardware.TalonFX; +import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkBase.FaultID; import com.revrobotics.REVLibError; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringArrayPublisher; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj.PowerDistribution; import java.util.ArrayList; import java.util.HashSet; import java.util.List; -import java.util.Objects; import java.util.Optional; import java.util.Set; import java.util.function.BooleanSupplier; -import java.util.function.Function; import java.util.function.Supplier; -import java.util.stream.Collectors; +import org.photonvision.PhotonCamera; +/** + * FaultLogger allows for faults to be logged and displayed. + * + * 

+ * FaultLogger.register(spark); // registers a spark, periodically checking for hardware faults
+ * spark.set(0.5);
+ * FaultLogger.check(spark); // checks that the previous set call did not encounter an error.
+ *
+ */ public final class FaultLogger { - /** Adds an alert widget to SmartDashboard;. */ - public static void setupLogging() { - SmartDashboard.putData( - "Faults", - builder -> { - builder.setSmartDashboardType("Alerts"); - builder.addStringArrayProperty("errors", () -> getStrings(FaultType.ERROR), null); - builder.addStringArrayProperty("warnings", () -> getStrings(FaultType.WARNING), null); - builder.addStringArrayProperty("infos", () -> getStrings(FaultType.INFO), null); - }); - } - /** An individual fault, containing necessary information. */ - public static record Fault(String description, FaultType type, double timestamp) { - public Fault(String description, FaultType type) { - this(description, type, Timer.getFPGATimestamp()); - } - + public static record Fault(String name, String description, FaultType type) { @Override - public boolean equals(Object other) { - if (other instanceof Fault f) { - return f.hashCode() == hashCode(); - } - return false; + public String toString() { + return name + ": " + description; } + } - @Override - public int hashCode() { - return Objects.hash(description, type); - } + @FunctionalInterface + public static interface FaultReporter { + void report(); } /** @@ -62,128 +53,386 @@ public static enum FaultType { ERROR, } - private static final List>> faultSuppliers = new ArrayList<>(); - private static final Set faults = new HashSet<>(); + /** A class to represent an alerts widget on NetworkTables */ + public static class Alerts { + private final StringArrayPublisher errors; + private final StringArrayPublisher warnings; + private final StringArrayPublisher infos; + + public Alerts(NetworkTable base, String name) { + NetworkTable table = base.getSubTable(name); + table.getStringTopic(".type").publish().set("Alerts"); + errors = table.getStringArrayTopic("errors").publish(); + warnings = table.getStringArrayTopic("warnings").publish(); + infos = table.getStringArrayTopic("infos").publish(); + } + + public void set(Set faults) { + errors.set(filteredStrings(faults, FaultType.ERROR)); + warnings.set(filteredStrings(faults, FaultType.WARNING)); + infos.set(filteredStrings(faults, FaultType.INFO)); + } + } + + // DATA + private static final List faultReporters = new ArrayList<>(); + private static final Set newFaults = new HashSet<>(); + private static final Set activeFaults = new HashSet<>(); + private static final Set totalFaults = new HashSet<>(); + + // NETWORK TABLES + private static final NetworkTable base = NetworkTableInstance.getDefault().getTable("Faults"); + private static final Alerts activeAlerts = new Alerts(base, "Active Faults"); + private static final Alerts totalAlerts = new Alerts(base, "Total Faults"); /** Polls registered fallibles. This method should be called periodically. */ public static void update() { - faultSuppliers.stream() - .map(s -> s.get()) - .flatMap(Optional::stream) - .collect(Collectors.toCollection(() -> faults)); + activeFaults.clear(); + + faultReporters.forEach(FaultReporter::report); + activeFaults.addAll(newFaults); + newFaults.clear(); + + totalFaults.addAll(activeFaults); + + activeAlerts.set(activeFaults); + totalAlerts.set(totalFaults); } - /** - * Returns a list of all current faults. - * - * @return A list of all current faults. - */ - public static Set getFaults() { - return faults; + /** Clears total faults. */ + public static void clear() { + totalFaults.clear(); + activeFaults.clear(); + newFaults.clear(); + } + + /** Clears fault suppliers. */ + public static void unregisterAll() { + faultReporters.clear(); } /** - * Returns a list of current faults based on the provided FaultType. + * Returns the set of all current faults. * - * @param type The type of faults to return. - * @return A list of faults of the specified FaultType. + * @return The set of all current faults. */ - public static List getFaults(FaultType type) { - return getFaults().stream().filter(a -> a.type() == type).toList(); + public static Set activeFaults() { + return activeFaults; } /** - * Returns a trigger for whether a failure has been reported. + * Returns the set of all total faults. * - * @param type The type of fault to filter for. - * @return A trigger based on the presence of faults. + * @return The set of all total faults. */ - public static Trigger failing(FaultType type) { - return new Trigger(() -> !getFaults(type).isEmpty()); + public static Set totalFaults() { + return totalFaults; } /** - * Configures a command to be ran whenever a failure is reported. + * Reports a fault. * - * @param command A function that creates a command from a set of faults. - * @return The created trigger. + * @param fault The fault to report. */ - public static Trigger onFailing(Function, Command> command) { - return failing(FaultType.ERROR).onTrue(command.apply(getFaults())); + public static void report(Fault fault) { + newFaults.add(fault); + switch (fault.type) { + case ERROR -> DriverStation.reportError(fault.toString(), false); + case WARNING -> DriverStation.reportWarning(fault.toString(), false); + case INFO -> System.out.println(fault.toString()); + } } /** - * Returns an array of descriptions of all faults that match the specified type. + * Reports a fault. * - * @param type The type to filter for. - * @return An array of description strings. + * @param name The name of the fault. + * @param description The description of the fault. + * @param type The type of the fault. */ - public static String[] getStrings(FaultType type) { - return getFaults().stream() - .filter(a -> a.type() == type) - .map(Fault::description) - .toArray(String[]::new); + public static void report(String name, String description, FaultType type) { + report(new Fault(name, description, type)); } /** - * Registers a new fallible supplier. + * Registers a new fault supplier. * * @param supplier A supplier of an optional fault. */ public static void register(Supplier> supplier) { - faultSuppliers.add(supplier); + faultReporters.add(() -> supplier.get().ifPresent(FaultLogger::report)); } /** - * Registers a new fallible supplier. + * Registers a new fault supplier. * * @param condition Whether a failure is occuring. * @param description The failure's description. * @param type The type of failure. */ - public static void register(BooleanSupplier condition, String description, FaultType type) { - faultSuppliers.add( - () -> - condition.getAsBoolean() - ? Optional.of(new Fault(description, type)) - : Optional.empty()); + public static void register( + BooleanSupplier condition, String name, String description, FaultType type) { + faultReporters.add( + () -> { + if (condition.getAsBoolean()) { + report(name, description, type); + } + }); } /** - * Registers fallible suppliers for a CAN-based Spark motor controller. + * Registers fault suppliers for a CAN-based Spark motor controller. * * @param spark The Spark Max or Spark Flex to manage. */ public static void register(CANSparkBase spark) { - int id = spark.getDeviceId(); - - register( + faultReporters.add( () -> { - REVLibError err = spark.getLastError(); - return err == REVLibError.kOk - ? Optional.empty() - : Optional.of( - new Fault( - String.format("Spark [%d]: Error: %s", id, err.name()), FaultType.ERROR)); + for (FaultID fault : FaultID.values()) { + if (spark.getFault(fault)) { + report(SparkUtils.name(spark), fault.name(), FaultType.ERROR); + } + } }); - - for (FaultID fault : FaultID.values()) { - register( - () -> spark.getFault(fault), - String.format("Spark [%d]: Fault: %s", id, fault.name()), - FaultType.WARNING); - } + register( + () -> spark.getMotorTemperature() > 100, + SparkUtils.name(spark), + "motor above 100°C", + FaultType.WARNING); + // TODO actually fix PDH (this is cursed) + FakePDH.register(spark); } /** - * Registers fallible suppliers for a duty cycle encoder. + * Registers fault suppliers for a duty cycle encoder. * * @param encoder The duty cycle encoder to manage. */ public static void register(DutyCycleEncoder encoder) { register( () -> !encoder.isConnected(), - String.format("DutyCycleEncoder [%d]: Disconnected"), + "Duty Cycle Encoder [" + encoder.getSourceChannel() + "]", + "disconnected", + FaultType.ERROR); + } + + /** + * Registers fault suppliers for a NavX. + * + * @param ahrs The NavX to manage. + */ + public static void register(AHRS ahrs) { + register(() -> !ahrs.isConnected(), "NavX", "disconnected", FaultType.ERROR); + } + + /** + * Registers fault suppliers for a power distribution hub/panel. + * + * @param powerDistribution The power distribution to manage. + */ + public static void register(PowerDistribution powerDistribution) { + // Field[] fields = PowerDistributionFaults.class.getFields(); + // faultReporters.add( + // () -> { + // try { + // PowerDistributionFaults faults = powerDistribution.getFaults(); + // for (Field fault : fields) { + // if (fault.getBoolean(faults)) { + // report("Power Distribution", fault.getName(), FaultType.ERROR); + // } + // } + // } catch (Exception e) { + // } + // }); + } + + /** + * Registers fault suppliers for a camera. + * + * @param camera The camera to manage. + */ + public static void register(PhotonCamera camera) { + register( + () -> !camera.isConnected(), + "Photon Camera [" + camera.getName() + "]", + "disconnected", + FaultType.ERROR); + } + + /** + * Registers fault suppliers for a talon. + * + * @param talon The talon to manage. + */ + public static void register(TalonFX talon) { + // TODO: Remove all the unnecessary faults + register( + (BooleanSupplier) talon.getFault_Hardware(), + "Talon ID: " + talon.getDeviceID(), + "Hardware fault occurred", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_ProcTemp(), + "Talon ID: " + talon.getDeviceID(), + "Processor temperature exceeded limit", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_DeviceTemp(), + "Talon ID: " + talon.getDeviceID(), + "Device temperature exceeded limit", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_Undervoltage(), + "Talon ID: " + talon.getDeviceID(), + "Device supply voltage dropped to near brownout levels", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_BootDuringEnable(), + "Talon ID: " + talon.getDeviceID(), + "Device boot while detecting the enable signal", FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_UnlicensedFeatureInUse(), + "Talon ID: " + talon.getDeviceID(), + "An unlicensed feature is in use, device may not behave as expected.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_BridgeBrownout(), + "Talon ID: " + talon.getDeviceID(), + "Bridge was disabled most likely due to supply voltage dropping too low.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_RemoteSensorReset(), + "Talon ID: " + talon.getDeviceID(), + "The remote sensor has reset.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_MissingDifferentialFX(), + "Talon ID: " + talon.getDeviceID(), + "The remote Talon FX used for differential control is not present on CAN Bus.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_RemoteSensorPosOverflow(), + "Talon ID: " + talon.getDeviceID(), + "The remote sensor position has overflowed.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_OverSupplyV(), + "Talon ID: " + talon.getDeviceID(), + "Supply Voltage has exceeded the maximum voltage rating of device.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_UnstableSupplyV(), + "Talon ID: " + talon.getDeviceID(), + "Supply Voltage is unstable.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_ReverseHardLimit(), + "Talon ID: " + talon.getDeviceID(), + "Reverse limit switch has been asserted. Output is set to neutral.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_ForwardHardLimit(), + "Talon ID: " + talon.getDeviceID(), + "Forward limit switch has been asserted. Output is set to neutral.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_ReverseSoftLimit(), + "Talon ID: " + talon.getDeviceID(), + "Reverse soft limit has been asserted. Output is set to neutral.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_ForwardSoftLimit(), + "Talon ID: " + talon.getDeviceID(), + "Forward soft limit has been asserted. Output is set to neutral.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_RemoteSensorDataInvalid(), + "Talon ID: " + talon.getDeviceID(), + "The remote sensor's data is no longer trusted.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_FusedSensorOutOfSync(), + "Talon ID: " + talon.getDeviceID(), + "The remote sensor used for fusion has fallen out of sync to the local sensor.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_StatorCurrLimit(), + "Talon ID: " + talon.getDeviceID(), + "Stator current limit occured.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_SupplyCurrLimit(), + "Talon ID: " + talon.getDeviceID(), + "Supply current limit occured.", + FaultType.ERROR); + + register( + (BooleanSupplier) talon.getFault_UsingFusedCANcoderWhileUnlicensed(), + "Talon ID: " + talon.getDeviceID(), + "Using Fused CANcoder feature while unlicensed. Device has fallen back to remote CANcoder.", + FaultType.ERROR); + } + + /** + * Reports REVLibErrors from a spark. + * + *

This should be called immediately after any call to the spark. + * + * @param spark The spark to report REVLibErrors from. + * @return If the spark is working without errors. + */ + public static boolean check(CANSparkBase spark) { + REVLibError error = spark.getLastError(); + return check(spark, error); + } + + /** + * Reports REVLibErrors from a spark. + * + *

This should be called immediately after any call to the spark. + * + * @param spark The spark to report REVLibErrors from. + * @param error Any REVLibErrors that may be returned from a method for a spark. + * @return If the spark is working without errors. + */ + public static boolean check(CANSparkBase spark, REVLibError error) { + if (error != REVLibError.kOk) { + report(SparkUtils.name(spark), error.name(), FaultType.ERROR); + return false; + } + return true; + } + + /** + * Returns an array of descriptions of all faults that match the specified type. + * + * @param type The type to filter for. + * @return An array of description strings. + */ + private static String[] filteredStrings(Set faults, FaultType type) { + return faults.stream() + .filter(a -> a.type() == type) + .map(Fault::toString) + .toArray(String[]::new); } } diff --git a/src/main/java/org/sciborgs1155/lib/InputStream.java b/src/main/java/org/sciborgs1155/lib/InputStream.java index 4e5c649..ede6b51 100644 --- a/src/main/java/org/sciborgs1155/lib/InputStream.java +++ b/src/main/java/org/sciborgs1155/lib/InputStream.java @@ -1,7 +1,10 @@ package org.sciborgs1155.lib; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTableInstance; import java.util.function.DoubleSupplier; import java.util.function.DoubleUnaryOperator; @@ -23,6 +26,14 @@ public static InputStream of(DoubleSupplier base) { return base::getAsDouble; } + public static InputStream hypot(InputStream x, InputStream y) { + return () -> Math.hypot(x.get(), y.get()); + } + + public static InputStream atan(InputStream x, InputStream y) { + return () -> Math.atan2(x.get(), y.get()); + } + /** * Shorthand to return a double value. * @@ -71,6 +82,26 @@ public default InputStream negate() { return scale(-1); } + /** + * Offsets the stream by a factor. + * + * @param factor A supplier of offset values. + * @return An offset stream. + */ + public default InputStream add(DoubleSupplier offset) { + return map(x -> x + offset.getAsDouble()); + } + + /** + * Offsets the stream by a factor. + * + * @param factor An offset. + * @return An offset stream. + */ + public default InputStream add(double factor) { + return add(() -> factor); + } + /** * Raises the stream outputs to an exponent. * @@ -91,6 +122,16 @@ public default InputStream signedPow(double exponent) { return map(x -> Math.copySign(Math.pow(x, exponent), x)); } + /** + * Filters the stream's outputs by the provided {@link LinearFilter}. + * + * @param filter The linear filter to use. + * @return A filtered stream. + */ + public default InputStream filter(LinearFilter filter) { + return map(filter::calculate); + } + /** * Deadbands the stream outputs by a minimum bound and scales them from 0 to a maximum bound. * @@ -99,7 +140,7 @@ public default InputStream signedPow(double exponent) { * @return A deadbanded stream. */ public default InputStream deadband(double deadband, double max) { - return map(x -> MathUtil.applyDeadband(x, deadband, Double.MAX_VALUE)); + return map(x -> MathUtil.applyDeadband(x, deadband, max)); } /** @@ -122,4 +163,22 @@ public default InputStream rateLimit(double rate) { var limiter = new SlewRateLimiter(rate); return map(x -> limiter.calculate(x)); } + + /** + * Logs the output of this stream to networktables every time it is polled. + * + *

A new stream is returned that is identical to this stream, but publishes its output to + * networktables every time it is polled. + * + * @param key The NetworkTables key to publish to. + * @return A stream with the same output as this one. + */ + public default InputStream log(String key) { + DoublePublisher pub = NetworkTableInstance.getDefault().getDoubleTopic(key).publish(); + return () -> { + double val = this.get(); + pub.set(val); + return val; + }; + } } diff --git a/src/main/java/org/sciborgs1155/lib/SparkUtils.java b/src/main/java/org/sciborgs1155/lib/SparkUtils.java index af845d6..dff4e30 100644 --- a/src/main/java/org/sciborgs1155/lib/SparkUtils.java +++ b/src/main/java/org/sciborgs1155/lib/SparkUtils.java @@ -1,32 +1,58 @@ package org.sciborgs1155.lib; -import static edu.wpi.first.units.Units.Rotations; - import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import edu.wpi.first.units.Angle; -import edu.wpi.first.units.Time; -import edu.wpi.first.units.Units; +import com.revrobotics.REVLibError; +import com.revrobotics.SparkRelativeEncoder.Type; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; import java.util.Set; +import java.util.function.DoubleSupplier; /** Utility class for configuration of Spark motor controllers */ public class SparkUtils { - public static final int FRAME_STRATEGY_DISABLED = 65535; - public static final int FRAME_STRATEGY_SLOW = 500; - public static final int FRAME_STRATEGY_FAST = 15; + private static final List runnables = new ArrayList<>(); + + public static void addChecker(Runnable runnable) { + runnables.add(runnable); + } + + public static List getRunnables() { + return runnables; + } + + // REV's docs have the size of a signed value of 65535ms for the max period + // https://docs.revrobotics.com/brushless/spark-max/control-interfaces#periodic-status-frames + // The actual max is half of this (32767ms) + // https://www.chiefdelphi.com/t/revlibs-documented-maximum-status-frame-period-limit-is-wrong/458845, https://www.chiefdelphi.com/t/extreme-can-utilization-but-parameters-set-ok/456613/6 + public static final int FRAME_STRATEGY_DISABLED = 32767; + public static final int FRAME_STRATEGY_SLOW = 400; + public static final int FRAME_STRATEGY_MEDIUM = 100; + public static final int FRAME_STRATEGY_FAST = 20; + public static final int FRAME_STRATEGY_VERY_FAST = 10; - public static final Angle ANGLE_UNIT = Units.Rotations; - public static final Time TIME_UNIT = Units.Minutes; - public static final Angle THROUGHBORE_PPR = - Units.derive(Rotations).splitInto(2048).named("Pulses Per Revolution").symbol("PPR").make(); + public static final int THROUGHBORE_CPR = 8192; + + public static final int MAX_ATTEMPTS = 3; + + /** + * Formats the name of a spark with its CAN ID. + * + * @param spark The spark to find the name of. + * @return The name of a spark. + */ + public static String name(CANSparkBase spark) { + return "Spark [" + spark.getDeviceId() + "]"; + } /** Represents a type of sensor that can be plugged into the spark */ public static enum Sensor { INTEGRATED, ANALOG, - QUADRATURE, - DUTY_CYCLE; + ALTERNATE, + ABSOLUTE; } /** Represents a type of data that can be sent from the spark */ @@ -34,7 +60,9 @@ public static enum Data { POSITION, VELOCITY, CURRENT, - VOLTAGE; + TEMPERATURE, + INPUT_VOLTAGE, + APPLIED_OUTPUT; } /** @@ -49,54 +77,88 @@ public static enum Data { * @see Data * @see https://docs.revrobotics.com/brushless/spark-max/control-interfaces */ - public static void configureFrameStrategy( + public static REVLibError configureFrameStrategy( CANSparkBase spark, Set data, Set sensors, boolean withFollower) { - int status0 = 10; // output, faults - int status1 = FRAME_STRATEGY_SLOW; // velocity, temperature, input voltage, current - int status2 = FRAME_STRATEGY_SLOW; // position + int status0 = FRAME_STRATEGY_MEDIUM; // output, faults + int status1 = FRAME_STRATEGY_SLOW; + // integrated velocity, temperature, input voltage, current | default 20 + int status2 = FRAME_STRATEGY_SLOW; // integrated position | default 20 int status3 = FRAME_STRATEGY_DISABLED; // analog encoder | default 50 int status4 = FRAME_STRATEGY_DISABLED; // alternate quadrature encoder | default 20 int status5 = FRAME_STRATEGY_DISABLED; // duty cycle position | default 200 int status6 = FRAME_STRATEGY_DISABLED; // duty cycle velocity | default 200 + int status7 = FRAME_STRATEGY_DISABLED; + // status frame 7 is cursed, the only mention i found of it in rev's docs is at + // https://docs.revrobotics.com/brushless/spark-flex/revlib/spark-flex-firmware-changelog#breaking-changes + // if it's only IAccum, there's literally no reason to enable the frame + + Optional supplier1 = Optional.empty(); + Optional supplier2 = Optional.empty(); + Optional supplier3 = Optional.empty(); + Optional supplier4 = Optional.empty(); + Optional supplier5 = Optional.empty(); + Optional supplier6 = Optional.empty(); + + if (withFollower || data.contains(Data.APPLIED_OUTPUT)) { + status0 = FRAME_STRATEGY_VERY_FAST; + } - if (data.contains(Data.VELOCITY) - || data.contains(Data.VOLTAGE) - || data.contains(Data.CURRENT)) { + if (sensors.contains(Sensor.INTEGRATED) && data.contains(Data.VELOCITY) + || data.contains(Data.INPUT_VOLTAGE) + || data.contains(Data.CURRENT) + || data.contains(Data.TEMPERATURE)) { status1 = FRAME_STRATEGY_FAST; + supplier1 = Optional.of(spark::getOutputCurrent); } - if (data.contains(Data.POSITION)) { + if (sensors.contains(Sensor.INTEGRATED) && data.contains(Data.POSITION)) { status2 = FRAME_STRATEGY_FAST; + supplier2 = Optional.of(() -> spark.getEncoder().getPosition()); } - if (sensors.contains(Sensor.ANALOG)) { + if (sensors.contains(Sensor.ANALOG) + && (data.contains(Data.VELOCITY) || data.contains(Data.POSITION))) { status3 = FRAME_STRATEGY_FAST; + supplier3 = Optional.of(() -> spark.getEncoder().getVelocity()); } - if (sensors.contains(Sensor.QUADRATURE)) { + if (sensors.contains(Sensor.ALTERNATE) + && (data.contains(Data.VELOCITY) || data.contains(Data.POSITION))) { status4 = FRAME_STRATEGY_FAST; + supplier4 = + Optional.of(() -> spark.getEncoder(Type.kQuadrature, THROUGHBORE_CPR).getPosition()); } - if (sensors.contains(Sensor.DUTY_CYCLE)) { + if (sensors.contains(Sensor.ABSOLUTE)) { if (data.contains(Data.POSITION)) { status5 = FRAME_STRATEGY_FAST; + supplier5 = Optional.of(() -> spark.getAbsoluteEncoder().getPosition()); } if (data.contains(Data.VELOCITY)) { status6 = FRAME_STRATEGY_FAST; + supplier6 = Optional.of(() -> spark.getAbsoluteEncoder().getVelocity()); } } - if (!withFollower) { - status0 = FRAME_STRATEGY_SLOW; + int[] frames = {status0, status1, status2, status3, status4, status5, status6, status7}; + List> suppliers = + List.of( + Optional.empty(), + supplier1, + supplier2, + supplier3, + supplier4, + supplier5, + supplier6, + Optional.empty()); + REVLibError error = REVLibError.kOk; + for (int i = 0; i < frames.length; i++) { + REVLibError e = spark.setPeriodicFramePeriod(PeriodicFrame.fromId(i), frames[i]); + if (e != REVLibError.kOk) { + error = e; + } } - - spark.setPeriodicFramePeriod(PeriodicFrame.kStatus0, status0); - spark.setPeriodicFramePeriod(PeriodicFrame.kStatus1, status1); - spark.setPeriodicFramePeriod(PeriodicFrame.kStatus2, status2); - spark.setPeriodicFramePeriod(PeriodicFrame.kStatus3, status3); - spark.setPeriodicFramePeriod(PeriodicFrame.kStatus4, status4); - spark.setPeriodicFramePeriod(PeriodicFrame.kStatus5, status5); - spark.setPeriodicFramePeriod(PeriodicFrame.kStatus6, status6); + return error; } /** @@ -105,7 +167,22 @@ public static void configureFrameStrategy( * * @param spark The follower spark. */ - public static void configureFollowerFrameStrategy(CANSparkBase spark) { - configureFrameStrategy(spark, Set.of(), Set.of(), false); + public static REVLibError configureNothingFrameStrategy(CANSparkBase spark) { + return configureFrameStrategy(spark, Set.of(), Set.of(), false); + } + + /** + * Wraps the value of a call into an optional depending on the spark's indicated last error. + * + * @param The type of value. + * @param spark The spark to check for errors. + * @param value The value to wrap. + * @return An optional that may contain the value. + */ + public static Optional wrapCall(CANSparkBase spark, T value) { + if (FaultLogger.check(spark)) { + return Optional.of(value); + } + return Optional.empty(); } } diff --git a/src/main/java/org/sciborgs1155/lib/TalonUtils.java b/src/main/java/org/sciborgs1155/lib/TalonUtils.java new file mode 100644 index 0000000..518fefc --- /dev/null +++ b/src/main/java/org/sciborgs1155/lib/TalonUtils.java @@ -0,0 +1,101 @@ +package org.sciborgs1155.lib; + +import com.ctre.phoenix6.Orchestra; +import com.ctre.phoenix6.configs.AudioConfigs; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.DriverStation; +import java.util.ArrayList; + +public class TalonUtils { + private static final Orchestra orchestra = new Orchestra(); + private static final ArrayList talons = new ArrayList<>(4); + private static boolean fileLoaded = false; + + /** + * Adds motor to the orchestra. + * + * @param talon The motor to add. + */ + public static void addMotor(TalonFX talon) { + talons.add(talon); + } + + /** + * Configure all motors to play a selected Chirp (CHRP) file in the deploy directory. Should be + * called once after addition of all Talons to TalonUtils. + * + *

Use {@code loadOrchestraFile()} after configuration to change the played file. + * + * @param fileName The path of the file to play. + * @return Whether loading the file was successful. + */ + public static boolean configureOrchestra(String fileName) { + AudioConfigs audioCfg = new AudioConfigs().withAllowMusicDurDisable(true); + for (TalonFX talon : talons) { + talon.getConfigurator().apply(audioCfg); + orchestra.addInstrument(talon); + } + return loadOrchestraFile(fileName); + } + + /** + * Load the selected CHRP file located in the deploy directory. + * + * @param fileName The name of the file to play. + * @return Whether loading the file was successful. + */ + public static boolean loadOrchestraFile(String fileName) { + fileLoaded = orchestra.loadMusic(fileName).isOK(); + if (!fileLoaded) { + fileNotFound(); + } + + return fileLoaded; + } + + /** + * Begin playback of the loaded file. + * + * @return Whether the operation was successful. + */ + public static boolean play() { + if (fileLoaded) { + return orchestra.play().isOK(); + } + fileNotFound(); + return false; + } + + /** + * Stop and restart playback of the loaded file. + * + * @return Whether the operation was successful. + */ + public static boolean stop() { + if (fileLoaded) { + return orchestra.stop().isOK(); + } + fileNotFound(); + return false; + } + + /** + * Pause playback of the loaded file. + * + * @return Whether the operation was successful. + */ + public static boolean pause() { + if (fileLoaded) { + return orchestra.pause().isOK(); + } + fileNotFound(); + return false; + } + + private static void fileNotFound() { + fileLoaded = false; + DriverStation.reportError( + "CHRP file not loaded. Check that it is in the deploy directory & includes file extension.", + true); + } +} diff --git a/src/main/java/org/sciborgs1155/lib/TestingUtil.java b/src/main/java/org/sciborgs1155/lib/TestingUtil.java index 5ea8094..e3d877e 100644 --- a/src/main/java/org/sciborgs1155/lib/TestingUtil.java +++ b/src/main/java/org/sciborgs1155/lib/TestingUtil.java @@ -1,9 +1,10 @@ package org.sciborgs1155.lib; import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Subsystem; +import org.sciborgs1155.lib.FaultLogger.FaultType; public class TestingUtil { /** @@ -28,7 +29,7 @@ public static void fastForward() { * @param command The command to run. */ public static void run(Command command) { - command.ignoringDisable(true).schedule(); + command.schedule(); CommandScheduler.getInstance().run(); } @@ -39,23 +40,43 @@ public static void run(Command command) { * @param runs The number of times CommandScheduler is run. */ public static void run(Command command, int runs) { - command.ignoringDisable(true).schedule(); + command.schedule(); fastForward(runs); } + /** Sets up DS and initializes HAL with default values and asserts that it doesn't fail. */ + public static void setupTests() { + assert HAL.initialize(500, 0); + DriverStationSim.setEnabled(true); + DriverStationSim.setTest(true); + DriverStationSim.notifyNewData(); + } + /** - * Closes subsystem and unregisters it from CommandScheduler. + * Resets CommandScheduler and closes all subsystems. Please call in an @AfterEach method! * - * @param subsystem The subsystem to unregister. + * @param subsystems All subsystems that need to be closed */ - public static void closeSubsystem( - TestableSubsystem subsystem) throws Exception { - CommandScheduler.getInstance().unregisterSubsystem(subsystem); - subsystem.close(); + public static void reset(AutoCloseable... subsystems) throws Exception { + CommandScheduler.getInstance().unregisterAllSubsystems(); + CommandScheduler.getInstance().cancelAll(); + for (AutoCloseable subsystem : subsystems) { + subsystem.close(); + } } - /** Initializes HAL with default values and asserts that it doesn't fail. */ - public static void setupHAL() { - assert HAL.initialize(500, 0); + public static void assertEqualsReport( + String faultName, double expected, double actual, double delta) { + assertReport( + Math.abs(expected - actual) <= delta, + faultName, + "expected: " + expected + "; actual: " + actual); + } + + public static void assertReport(boolean condition, String faultName, String description) { + FaultLogger.report( + faultName, + (condition ? "success! " : "") + description, + condition ? FaultType.INFO : FaultType.WARNING); } } diff --git a/src/main/java/org/sciborgs1155/lib/Tuning.java b/src/main/java/org/sciborgs1155/lib/Tuning.java new file mode 100644 index 0000000..8b820c2 --- /dev/null +++ b/src/main/java/org/sciborgs1155/lib/Tuning.java @@ -0,0 +1,74 @@ +package org.sciborgs1155.lib; + +import edu.wpi.first.networktables.BooleanEntry; +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.IntegerEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringEntry; + +/** + * Tuning creates an entry with a specified topic and configurable value in Network Tables. + * + * 

+ * Tuning.entry("/[FOLDER_NAME]/[TOPIC_NAME]", [CONFIGURABLE_VALUE]); //this creates a new configurable datatype value corresponding to the path given
+ *
+ */ +public final class Tuning { + + /** + * Logs a DoubleEntry on Network Tables. + * + * @param path The file path, "/[FOLDER_NAME]/[TOPIC_NAME]", ie: "/Robot/exampleTopicName". + * @param value The configurable double that you would like to correspond to the topic. + * @return The DoubleEntry - contains all methods of DoublePublisher, DoubleSubscriber. + */ + public static DoubleEntry entry(String path, double value) { + + DoubleEntry entry = NetworkTableInstance.getDefault().getDoubleTopic(path).getEntry(value); + entry.set(value); + + return entry; + } + + /** + * Logs a IntegerEntry on Network Tables. + * + * @param path The file path, "/[FOLDER_NAME]/[TOPIC_NAME]", ie: "/Robot/exampleTopicName". + * @param value The configurable (int)long that you would like to correspond to the topic. + * @return The IntegerEntry - contains all methods of IntegerPublisher, IntegerSubscriber. + */ + public static IntegerEntry entry(String path, long value) { + IntegerEntry entry = NetworkTableInstance.getDefault().getIntegerTopic(path).getEntry(value); + entry.set(value); + + return entry; + } + + /** + * Logs a StringEntry on Network Tables. + * + * @param path The file path, "/[FOLDER_NAME]/[TOPIC_NAME]", ie: "/Robot/exampleTopicName". + * @param value The configurable String that you would like to correspond to the topic. + * @return The StringEntry - contains all methods of StringPublisher, StringSubscriber. + */ + public static StringEntry entry(String path, String value) { + StringEntry entry = NetworkTableInstance.getDefault().getStringTopic(path).getEntry(value); + entry.set(value); + + return entry; + } + + /** + * Logs a BooleanEntry on Network Tables. + * + * @param path The file path, "/[FOLDER_NAME]/[TOPIC_NAME]", ie: "/Robot/exampleTopicName". + * @param value The configurable boolean that you would like to correspond to the topic. + * @return The BooleanEntry - contains all methods of BooleanPublisher, BooleanSubscriber. + */ + public static BooleanEntry entry(String path, boolean value) { + BooleanEntry entry = NetworkTableInstance.getDefault().getBooleanTopic(path).getEntry(value); + entry.set(value); + + return entry; + } +} diff --git a/src/main/java/org/sciborgs1155/robot/Constants.java b/src/main/java/org/sciborgs1155/robot/Constants.java index 709e0b5..86e2c56 100644 --- a/src/main/java/org/sciborgs1155/robot/Constants.java +++ b/src/main/java/org/sciborgs1155/robot/Constants.java @@ -2,8 +2,19 @@ import static edu.wpi.first.units.Units.*; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Time; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.List; import org.sciborgs1155.robot.drive.DriveConstants; /** @@ -16,11 +27,110 @@ * @see Units */ public class Constants { + /** Returns the robot's alliance. */ + public static Alliance alliance() { + return DriverStation.getAlliance().orElse(Alliance.Blue); + } + + /** Returns the rotation of the robot's alliance with respect to the origin. */ + public static Rotation2d allianceRotation() { + return Rotation2d.fromRotations(alliance() == Alliance.Blue ? 0 : 0.5); + } + public static final Measure