From 26561d347d8b5f1121bdd015e4d629c133abf03c Mon Sep 17 00:00:00 2001 From: BR88C Date: Sat, 9 Nov 2024 12:26:28 -0500 Subject: [PATCH] Update vendordeps and project organization --- .vscode/settings.json | 7 + .wpilib/wpilib_preferences.json | 2 +- src/main/deploy/example.txt | 3 + .../java/com/revrobotics/spark/SparkShim.java | 80 +++ .../spark/config/SignalsConfig.java | 115 ++++ .../team340/lib/controller/Controller.java | 105 --- .../team340/lib/dashboard/GRRDashboard.java | 134 ++-- .../org/team340/lib/dashboard/Tunable.java | 261 -------- .../lib/dashboard/tunables/Tunable.java | 268 ++++++++ .../dashboard/tunables/TunableBoolean.java | 53 ++ .../lib/dashboard/tunables/TunableDouble.java | 53 ++ .../lib/dashboard/tunables/TunableFloat.java | 53 ++ .../dashboard/tunables/TunableInteger.java | 53 ++ .../lib/dashboard/tunables/TunableString.java | 53 ++ .../logging/reduxlib/CanandgyroLogger.java | 3 +- .../lib/logging/reduxlib/CanandmagLogger.java | 3 +- .../revlib/SparkAbsoluteEncoderLogger.java | 2 +- .../revlib/SparkAnalogSensorLogger.java | 21 + .../SparkClosedLoopControllerLogger.java | 19 + .../lib/logging/revlib/SparkFlexLogger.java | 86 +-- .../revlib/SparkLimitSwitchLogger.java | 2 +- .../lib/logging/revlib/SparkMaxLogger.java | 86 +-- .../revlib/SparkPIDControllerLogger.java | 19 - .../revlib/structs/SparkFaultsStruct.java | 53 ++ .../revlib/structs/SparkWarningsStruct.java | 53 ++ ...rwardPerspective.java => Perspective.java} | 18 +- .../org/team340/lib/swerve/SwerveAPI.java | 85 +-- .../org/team340/lib/swerve/SwerveModule.java | 2 +- .../org/team340/lib/swerve/SwerveState.java | 2 +- .../team340/lib/swerve/SwerveTunables.java | 2 +- .../swerve/hardware/SwerveBaseHardware.java | 2 +- .../lib/swerve/hardware/SwerveEncoders.java | 116 ++-- .../lib/swerve/hardware/SwerveIMUs.java | 17 +- .../lib/swerve/hardware/SwerveMotors.java | 215 +++---- .../java/org/team340/lib/util/Alliance.java | 28 +- .../org/team340/lib/util/CommandBuilder.java | 14 +- .../org/team340/lib/util/DummySubsystem.java | 2 +- .../org/team340/lib/util/GRRSubsystem.java | 7 +- src/main/java/org/team340/lib/util/Math2.java | 222 +++---- .../java/org/team340/lib/util/Mutable.java | 3 + .../lib/util/{ctre => }/PhoenixUtil.java | 12 +- .../lib/util/{redux => }/ReduxUtil.java | 12 +- .../java/org/team340/lib/util/RevUtil.java | 59 ++ .../lib/util/rev/RelativeEncoderConfig.java | 117 ---- .../team340/lib/util/rev/RevConfigBase.java | 76 --- .../lib/util/rev/RevConfigRegistry.java | 178 ------ .../util/rev/SparkAbsoluteEncoderConfig.java | 116 ---- .../team340/lib/util/rev/SparkFlexConfig.java | 601 ------------------ .../lib/util/rev/SparkLimitSwitchConfig.java | 53 -- .../team340/lib/util/rev/SparkMaxConfig.java | 597 ----------------- .../util/rev/SparkPIDControllerConfig.java | 465 -------------- .../java/org/team340/robot/Constants.java | 35 +- src/main/java/org/team340/robot/Robot.java | 48 +- .../org/team340/robot/commands/Autos.java | 39 +- .../org/team340/robot/commands/Routines.java | 32 +- .../org/team340/robot/subsystems/Foo.java | 20 + .../org/team340/robot/subsystems/Swerve.java | 76 ++- ...atest.json => Phoenix6-25.0.0-beta-2.json} | 50 +- vendordeps/REVLib.json | 12 +- ...{ReduxLib_2024.json => ReduxLib_2025.json} | 12 +- 60 files changed, 1626 insertions(+), 3306 deletions(-) create mode 100644 src/main/deploy/example.txt create mode 100644 src/main/java/com/revrobotics/spark/SparkShim.java create mode 100644 src/main/java/com/revrobotics/spark/config/SignalsConfig.java delete mode 100644 src/main/java/org/team340/lib/dashboard/Tunable.java create mode 100644 src/main/java/org/team340/lib/dashboard/tunables/Tunable.java create mode 100644 src/main/java/org/team340/lib/dashboard/tunables/TunableBoolean.java create mode 100644 src/main/java/org/team340/lib/dashboard/tunables/TunableDouble.java create mode 100644 src/main/java/org/team340/lib/dashboard/tunables/TunableFloat.java create mode 100644 src/main/java/org/team340/lib/dashboard/tunables/TunableInteger.java create mode 100644 src/main/java/org/team340/lib/dashboard/tunables/TunableString.java create mode 100644 src/main/java/org/team340/lib/logging/revlib/SparkAnalogSensorLogger.java create mode 100644 src/main/java/org/team340/lib/logging/revlib/SparkClosedLoopControllerLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/revlib/SparkPIDControllerLogger.java create mode 100644 src/main/java/org/team340/lib/logging/revlib/structs/SparkFaultsStruct.java create mode 100644 src/main/java/org/team340/lib/logging/revlib/structs/SparkWarningsStruct.java rename src/main/java/org/team340/lib/swerve/{ForwardPerspective.java => Perspective.java} (89%) rename src/main/java/org/team340/lib/util/{ctre => }/PhoenixUtil.java (89%) rename src/main/java/org/team340/lib/util/{redux => }/ReduxUtil.java (97%) create mode 100644 src/main/java/org/team340/lib/util/RevUtil.java delete mode 100644 src/main/java/org/team340/lib/util/rev/RelativeEncoderConfig.java delete mode 100644 src/main/java/org/team340/lib/util/rev/RevConfigBase.java delete mode 100644 src/main/java/org/team340/lib/util/rev/RevConfigRegistry.java delete mode 100644 src/main/java/org/team340/lib/util/rev/SparkAbsoluteEncoderConfig.java delete mode 100644 src/main/java/org/team340/lib/util/rev/SparkFlexConfig.java delete mode 100644 src/main/java/org/team340/lib/util/rev/SparkLimitSwitchConfig.java delete mode 100644 src/main/java/org/team340/lib/util/rev/SparkMaxConfig.java delete mode 100644 src/main/java/org/team340/lib/util/rev/SparkPIDControllerConfig.java create mode 100644 src/main/java/org/team340/robot/subsystems/Foo.java rename vendordeps/{Phoenix6-frc2025-beta-latest.json => Phoenix6-25.0.0-beta-2.json} (90%) rename vendordeps/{ReduxLib_2024.json => ReduxLib_2025.json} (83%) diff --git a/.vscode/settings.json b/.vscode/settings.json index bdd4bf2..f77ea88 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -40,6 +40,7 @@ "Accl", "Accum", "ADIS", + "Backports", "Bezier", "Botpose", "Brushless", @@ -65,7 +66,10 @@ "Discretization", "Discretize", "Discretizing", + "Doppel", "DTheta", + "EEPROM", + "Expdelta", "Falsi", "Feedforward", "Fullscreen", @@ -106,6 +110,9 @@ "Sendables", "Setpoint", "Setpoints", + "Signif", + "Significand", + "Subnormals", "Subuid", "TalonFX", "TalonSRX", diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 860aa38..9b0f9b7 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024", + "projectYear": "2025", "teamNumber": 340 } diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt new file mode 100644 index 0000000..d6ec5cf --- /dev/null +++ b/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function +to get a proper path relative to the deploy directory. diff --git a/src/main/java/com/revrobotics/spark/SparkShim.java b/src/main/java/com/revrobotics/spark/SparkShim.java new file mode 100644 index 0000000..8aeacd8 --- /dev/null +++ b/src/main/java/com/revrobotics/spark/SparkShim.java @@ -0,0 +1,80 @@ +package com.revrobotics.spark; + +import com.revrobotics.REVLibError; +import com.revrobotics.jni.CANSparkJNI; +import com.revrobotics.spark.config.ClosedLoopConfig.ClosedLoopSlot; + +public final class SparkShim { + + private SparkShim() { + throw new AssertionError("This is a utility class!"); + } + + public static long extractHandle(SparkLowLevel spark) { + return spark.sparkHandle; + } + + public static int getFaults(SparkLowLevel spark) { + return CANSparkJNI.c_Spark_GetFaults(spark.sparkHandle); + } + + public static int getStickyFaults(SparkLowLevel spark) { + return CANSparkJNI.c_Spark_GetStickyFaults(spark.sparkHandle); + } + + public static int getWarnings(SparkLowLevel spark) { + return CANSparkJNI.c_Spark_GetWarnings(spark.sparkHandle); + } + + public static int getStickyWarnings(SparkLowLevel spark) { + return CANSparkJNI.c_Spark_GetStickyWarnings(spark.sparkHandle); + } + + public static REVLibError setP(SparkLowLevel spark, ClosedLoopSlot slot, double gain) { + return REVLibError.fromInt( + CANSparkJNI.c_Spark_SetParameterFloat32(spark.sparkHandle, 13 + slot.value * 8, (float) gain) + ); + } + + public static REVLibError setI(SparkLowLevel spark, ClosedLoopSlot slot, double gain) { + return REVLibError.fromInt( + CANSparkJNI.c_Spark_SetParameterFloat32(spark.sparkHandle, 14 + slot.value * 8, (float) gain) + ); + } + + public static REVLibError setD(SparkLowLevel spark, ClosedLoopSlot slot, double gain) { + return REVLibError.fromInt( + CANSparkJNI.c_Spark_SetParameterFloat32(spark.sparkHandle, 15 + slot.value * 8, (float) gain) + ); + } + + public static REVLibError setFF(SparkLowLevel spark, ClosedLoopSlot slot, double gain) { + return REVLibError.fromInt( + CANSparkJNI.c_Spark_SetParameterFloat32(spark.sparkHandle, 16 + slot.value * 8, (float) gain) + ); + } + + public static REVLibError setIZone(SparkLowLevel spark, ClosedLoopSlot slot, double iZone) { + return REVLibError.fromInt( + CANSparkJNI.c_Spark_SetParameterFloat32(spark.sparkHandle, 17 + slot.value * 8, (float) iZone) + ); + } + + public static REVLibError setDFilter(SparkLowLevel spark, ClosedLoopSlot slot, double gain) { + return REVLibError.fromInt( + CANSparkJNI.c_Spark_SetParameterFloat32(spark.sparkHandle, 18 + slot.value * 8, (float) gain) + ); + } + + public static REVLibError setOutputMin(SparkLowLevel spark, ClosedLoopSlot slot, double value) { + return REVLibError.fromInt( + CANSparkJNI.c_Spark_SetParameterFloat32(spark.sparkHandle, 19 + slot.value * 8, (float) value) + ); + } + + public static REVLibError setOutputMax(SparkLowLevel spark, ClosedLoopSlot slot, double value) { + return REVLibError.fromInt( + CANSparkJNI.c_Spark_SetParameterFloat32(spark.sparkHandle, 20 + slot.value * 8, (float) value) + ); + } +} diff --git a/src/main/java/com/revrobotics/spark/config/SignalsConfig.java b/src/main/java/com/revrobotics/spark/config/SignalsConfig.java new file mode 100644 index 0000000..fa5982f --- /dev/null +++ b/src/main/java/com/revrobotics/spark/config/SignalsConfig.java @@ -0,0 +1,115 @@ +package com.revrobotics.spark.config; + +public class SignalsConfig extends BaseConfig { + + /** + * Applies settings from another {@link SignalsConfig} to this one. + * + *

Settings in the provided config will overwrite existing values in this object. Settings not + * specified in the provided config remain unchanged. + * + * @param config The {@link SignalsConfig} to copy settings from + * @return The updated {@link SignalsConfig} for method chaining + */ + public SignalsConfig apply(SignalsConfig config) { + super.apply(config); + return this; + } + + public SignalsConfig status0Period(int periodMs) { + setPeriod(SparkParameter.kStatus0Period, periodMs); + return this; + } + + public SignalsConfig status1Period(int periodMs) { + setPeriod(SparkParameter.kStatus1Period, periodMs); + return this; + } + + public SignalsConfig status1Enabled(boolean enabled) { + setEnabled(SparkParameter.kForceEnableStatus1, enabled); + return this; + } + + public SignalsConfig status2Period(int periodMs) { + setPeriod(SparkParameter.kStatus2Period, periodMs); + return this; + } + + public SignalsConfig status2Enabled(boolean enabled) { + setEnabled(SparkParameter.kForceEnableStatus2, enabled); + return this; + } + + public SignalsConfig status3Period(int periodMs) { + setPeriod(SparkParameter.kStatus3Period, periodMs); + return this; + } + + public SignalsConfig status3Enabled(boolean enabled) { + setEnabled(SparkParameter.kForceEnableStatus3, enabled); + return this; + } + + public SignalsConfig status4Period(int periodMs) { + setPeriod(SparkParameter.kStatus4Period, periodMs); + return this; + } + + public SignalsConfig status4Enabled(boolean enabled) { + setEnabled(SparkParameter.kForceEnableStatus4, enabled); + return this; + } + + public SignalsConfig status5Period(int periodMs) { + setPeriod(SparkParameter.kStatus5Period, periodMs); + return this; + } + + public SignalsConfig status5Enabled(boolean enabled) { + setEnabled(SparkParameter.kForceEnableStatus5, enabled); + return this; + } + + public SignalsConfig status6Period(int periodMs) { + setPeriod(SparkParameter.kStatus6Period, periodMs); + return this; + } + + public SignalsConfig status6Enabled(boolean enabled) { + setEnabled(SparkParameter.kForceEnableStatus6, enabled); + return this; + } + + public SignalsConfig status7Period(int periodMs) { + setPeriod(SparkParameter.kStatus7Period, periodMs); + return this; + } + + public SignalsConfig status7Enabled(boolean enabled) { + setEnabled(SparkParameter.kForceEnableStatus7, enabled); + return this; + } + + private void setPeriod(SparkParameter parameter, int periodMs) { + int id = parameter.value; + Object value = getParameter(id); + if (value == null) { + putParameter(id, periodMs); + } else { + int currentPeriodMs = (int) value; + putParameter(id, Math.min(currentPeriodMs, periodMs)); + } + } + + private void setEnabled(SparkParameter parameter, boolean enabled) { + int id = parameter.value; + Object value = getParameter(id); + if (value == null) { + putParameter(id, enabled ? 1 : 0); + } else { + int currentValue = (int) value; + putParameter(id, (currentValue == 1 || enabled) ? 1 : 0); + } + } +} diff --git a/src/main/java/org/team340/lib/controller/Controller.java b/src/main/java/org/team340/lib/controller/Controller.java index 2b5dc54..61e6005 100644 --- a/src/main/java/org/team340/lib/controller/Controller.java +++ b/src/main/java/org/team340/lib/controller/Controller.java @@ -57,51 +57,6 @@ public Trigger pov(int angle) { return pov(0, angle, loop); } - @Override - public Trigger povUp() { - return pov(0); - } - - @Override - public Trigger povUpRight() { - return pov(45); - } - - @Override - public Trigger povRight() { - return pov(90); - } - - @Override - public Trigger povDownRight() { - return pov(135); - } - - @Override - public Trigger povDown() { - return pov(180); - } - - @Override - public Trigger povDownLeft() { - return pov(225); - } - - @Override - public Trigger povLeft() { - return pov(270); - } - - @Override - public Trigger povUpLeft() { - return pov(315); - } - - @Override - public Trigger povCenter() { - return pov(-1); - } - @Override public Trigger axisLessThan(int axis, double threshold) { return axisLessThan(axis, threshold, loop); @@ -117,66 +72,6 @@ public Trigger axisMagnitudeGreaterThan(int axis, double threshold) { return axisMagnitudeGreaterThan(axis, threshold, loop); } - @Override - public Trigger a() { - return a(loop); - } - - @Override - public Trigger b() { - return b(loop); - } - - @Override - public Trigger x() { - return x(loop); - } - - @Override - public Trigger y() { - return y(loop); - } - - @Override - public Trigger leftBumper() { - return leftBumper(loop); - } - - @Override - public Trigger rightBumper() { - return rightBumper(loop); - } - - @Override - public Trigger back() { - return back(loop); - } - - @Override - public Trigger start() { - return start(loop); - } - - @Override - public Trigger leftStick() { - return leftStick(loop); - } - - @Override - public Trigger rightStick() { - return rightStick(loop); - } - - @Override - public Trigger leftTrigger(double threshold) { - return leftTrigger(threshold, loop); - } - - @Override - public Trigger rightTrigger(double threshold) { - return rightTrigger(threshold, loop); - } - /** * Constructs a Trigger instance around the axis value of the left trigger. * @return A {@link Trigger} instance. diff --git a/src/main/java/org/team340/lib/dashboard/GRRDashboard.java b/src/main/java/org/team340/lib/dashboard/GRRDashboard.java index 47a71bd..dde1841 100644 --- a/src/main/java/org/team340/lib/dashboard/GRRDashboard.java +++ b/src/main/java/org/team340/lib/dashboard/GRRDashboard.java @@ -2,26 +2,19 @@ import choreo.trajectory.SwerveSample; import choreo.trajectory.Trajectory; -import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.math.Pair; -import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringArrayPublisher; +import edu.wpi.first.networktables.RawPublisher; import edu.wpi.first.networktables.StringPublisher; import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import java.math.BigDecimal; -import java.math.RoundingMode; -import java.util.ArrayList; -import java.util.HashMap; +import java.nio.ByteBuffer; import java.util.LinkedHashMap; import java.util.List; import java.util.Map; -import java.util.UUID; -import org.team340.lib.util.Alliance; /** * Utility class for interfacing with GRRDashboard. @@ -29,24 +22,15 @@ public final class GRRDashboard { private GRRDashboard() { - throw new UnsupportedOperationException("This is a utility class!"); + throw new AssertionError("This is a utility class!"); } private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("GRRDashboard"); private static final EventLoop periodic = new EventLoop(); - private static final BooleanSubscriber allianceOverrideActiveSub = nt - .getBooleanTopic("AllianceOverride/active") - .subscribe(false); - private static final BooleanSubscriber allianceOverrideIsBlueSub = nt - .getBooleanTopic("AllianceOverride/isBlue") - .subscribe(false); - - private static final Map> autoOptions = new LinkedHashMap<>(); // { id: [command, json] } - private static final StringArrayPublisher autoOptionsPub = nt.getStringArrayTopic("Autos/options").publish(); + private static final Map> autoOptions = new LinkedHashMap<>(); private static final StringPublisher activeAutoPub = nt.getStringTopic("Autos/active").publish(); private static final StringSubscriber selectedAutoSub; - private static Command selectedAuto = Commands.none(); static { @@ -59,65 +43,53 @@ private GRRDashboard() { * Adds an auto to the dashboard. * @param label The label for the auto. * @param command The auto's command. + * @return The auto's label. */ public static String addAuto(String label, Command command) { - return addAuto(label, command, new Trajectory("", List.of(), List.of(), List.of())); + return addAuto(label, List.of(), command); } /** * Adds an auto to the dashboard. * @param label The label for the auto. + * @param trajectory Trajectory utilized by the auto. * @param command The auto's command. + * @return The auto's label. + */ + public static String addAuto(String label, Trajectory trajectory, Command command) { + return addAuto(label, List.of(trajectory), command); + } + + /** + * Adds an auto to the dashboard. + * @param label The label for the auto. * @param trajectories Trajectories utilized by the auto. + * @param command The auto's command. + * @return The auto's label. */ - @SafeVarargs - public static String addAuto(String label, Command command, Trajectory... trajectories) { - String id = UUID.randomUUID().toString(); - List samples = new ArrayList<>(); - - double time = 0.0; - for (int i = 0; i < trajectories.length; i++) { - for (SwerveSample sample : trajectories[i].sampleArray()) { - samples.add( - new BigDecimal[] { - new BigDecimal(sample.x).setScale(3, RoundingMode.HALF_UP), - new BigDecimal(sample.y).setScale(3, RoundingMode.HALF_UP), - new BigDecimal(sample.heading).setScale(2, RoundingMode.HALF_UP), - new BigDecimal(sample.t + time).setScale(3, RoundingMode.HALF_UP) - } - ); + public static String addAuto(String label, List> trajectories, Command command) { + int size = trajectories.stream().mapToInt(t -> t.sampleArray().length * 16).sum(); + ByteBuffer bb = ByteBuffer.allocate(size + 4); + + double t = 0.0; + for (int i = 0; i < trajectories.size(); i++) { + for (SwerveSample sample : trajectories.get(i).sampleArray()) { + bb.putFloat((float) sample.x); + bb.putFloat((float) sample.y); + bb.putFloat((float) sample.heading); + bb.putFloat((float) (sample.t + t)); } - SwerveSample finalSample = trajectories[i].getFinalSample(); - if (finalSample != null) time += finalSample.t; + var last = trajectories.get(i).getFinalSample(); + if (last != null) t += last.t; } - double t = time; - String json = ""; - try { - json = new ObjectMapper() - .writeValueAsString( - new HashMap<>() { - { - put("id", id); - put("label", label); - put("samples", samples); - put("time", t); - } - } - ); - } catch (Exception e) { - e.printStackTrace(); - json = ""; - } + bb.putFloat((float) t); + var pub = nt.getRawTopic("Autos/options/" + label).publish("raw"); + autoOptions.put(label, Pair.of(command, pub)); + pub.set(bb); - if (json.isEmpty()) { - json = "{ \"id\": \"" + id + "\", \"label\": \"" + label + "\", \"samples\": [], \"time\": 0 }"; - } - - autoOptions.put(id, Pair.of(command, json)); - autoOptionsPub.set(autoOptions.values().stream().map(entry -> entry.getSecond()).toArray(String[]::new)); - return id; + return label; } /** @@ -127,33 +99,29 @@ public static Command getSelectedAuto() { return selectedAuto; } + /** + * Binds an action to the dashboard's update loop. + * @param action The action to bind. + */ + public static void bind(Runnable action) { + periodic.bind(action); + } + /** * Syncs data with the dashboard. Must be called * periodically in order for this class to function. */ public static void update() { - if (allianceOverrideActiveSub.get()) { - Alliance.enableOverride(allianceOverrideIsBlueSub.get()); - } else { - Alliance.disableOverride(); - } - - for (String id : selectedAutoSub.readQueueValues()) { - var entry = autoOptions.get(id); - if (entry != null) { - activeAutoPub.set(id); - selectedAuto = entry.getFirst(); + String[] selections = selectedAutoSub.readQueueValues(); + if (selections.length > 0) { + String selection = selections[selections.length - 1]; + var option = autoOptions.get(selection); + if (option != null) { + activeAutoPub.set(selection); + selectedAuto = option.getFirst(); } } periodic.poll(); } - - /** - * Binds an action to the dashboard's update loop. - * @param action The action to bind. - */ - static void bind(Runnable action) { - periodic.bind(action); - } } diff --git a/src/main/java/org/team340/lib/dashboard/Tunable.java b/src/main/java/org/team340/lib/dashboard/Tunable.java deleted file mode 100644 index 719b1ac..0000000 --- a/src/main/java/org/team340/lib/dashboard/Tunable.java +++ /dev/null @@ -1,261 +0,0 @@ -package org.team340.lib.dashboard; - -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.hardware.TalonFX; -import com.revrobotics.SparkPIDController; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.Logged.Strategy; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.Subscriber; -import java.util.function.Consumer; -import java.util.function.Supplier; - -/** - * The Tunable class is used to construct tunable properties of the robot to be modified - * via NetworkTables, as well as automatically displayed and edited in the dashboard. - */ -@Logged(strategy = Strategy.OPT_IN) -public final class Tunable implements Supplier, AutoCloseable { - - private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("GRRDashboard/Tunables"); - - private final Subscriber sub; - private final Supplier getter; - private long lastSet; - - private Tunable(Subscriber sub, Supplier getter, Consumer onChange) { - this.sub = sub; - this.getter = getter; - - if (onChange != null) { - GRRDashboard.bind(() -> { - long lastChange = sub.getLastChange(); - if (lastChange != lastSet) { - lastSet = lastChange; - onChange.accept(getter.get()); - } - }); - } - } - - /** - * Gets the current value of the tunable. - */ - @Override - public T get() { - return getter.get(); - } - - @Override - public void close() { - sub.close(); - } - - /** - * Creates a tunable boolean. - * @param name The name for the tunable. Must be unique. - * @param defaultValue The default value of the tunable (e.g. a programmed constant). - */ - public static Tunable booleanValue(String name, boolean defaultValue) { - return booleanValue(name, defaultValue, null); - } - - /** - * Creates a tunable boolean. - * @param name The name for the tunable. Must be unique. - * @param defaultValue The default value of the tunable (e.g. a programmed constant). - * @param onChange A consumer that is invoked when the value of the tunable is modified. - */ - public static Tunable booleanValue(String name, boolean defaultValue, Consumer onChange) { - var entry = nt.getBooleanTopic(name).getEntry(defaultValue); - entry.setDefault(defaultValue); - return new Tunable<>(entry, () -> entry.get(), onChange); - } - - /** - * Creates a tunable integer. - * @param name The name for the tunable. Must be unique. - * @param defaultValue The default value of the tunable (e.g. a programmed constant). - */ - public static Tunable intValue(String name, int defaultValue) { - return intValue(name, defaultValue, null); - } - - /** - * Creates a tunable integer. - * @param name The name for the tunable. Must be unique. - * @param defaultValue The default value of the tunable (e.g. a programmed constant). - * @param onChange A consumer that is invoked when the value of the tunable is modified. - */ - public static Tunable intValue(String name, int defaultValue, Consumer onChange) { - var entry = nt.getIntegerTopic(name).getEntry(defaultValue); - entry.setDefault(defaultValue); - return new Tunable<>(entry, () -> (int) entry.get(), onChange); - } - - /** - * Creates a tunable float. - * @param name The name for the tunable. Must be unique. - * @param defaultValue The default value of the tunable (e.g. a programmed constant). - */ - public static Tunable floatValue(String name, float defaultValue) { - return floatValue(name, defaultValue, null); - } - - /** - * Creates a tunable float. - * @param name The name for the tunable. Must be unique. - * @param defaultValue The default value of the tunable (e.g. a programmed constant). - * @param onChange A consumer that is invoked when the value of the tunable is modified. - */ - public static Tunable floatValue(String name, float defaultValue, Consumer onChange) { - var entry = nt.getFloatTopic(name).getEntry(defaultValue); - entry.setDefault(defaultValue); - return new Tunable<>(entry, () -> entry.get(), onChange); - } - - /** - * Creates a tunable double. - * @param name The name for the tunable. Must be unique. - * @param defaultValue The default value of the tunable (e.g. a programmed constant). - */ - public static Tunable doubleValue(String name, double defaultValue) { - return doubleValue(name, defaultValue, null); - } - - /** - * Creates a tunable double. - * @param name The name for the tunable. Must be unique. - * @param defaultValue The default value of the tunable (e.g. a programmed constant). - * @param onChange A consumer that is invoked when the value of the tunable is modified. - */ - public static Tunable doubleValue(String name, double defaultValue, Consumer onChange) { - var entry = nt.getDoubleTopic(name).getEntry(defaultValue); - entry.setDefault(defaultValue); - return new Tunable<>(entry, () -> entry.get(), onChange); - } - - /** - * Creates a tunable string. - * @param name The name for the tunable. Must be unique. - * @param defaultValue The default value of the tunable (e.g. a programmed constant). - */ - public static Tunable stringValue(String name, String defaultValue) { - return stringValue(name, defaultValue, null); - } - - /** - * Creates a tunable string. - * @param name The name for the tunable. Must be unique. - * @param defaultValue The default value of the tunable (e.g. a programmed constant). - * @param onChange A consumer that is invoked when the value of the tunable is modified. - */ - public static Tunable stringValue(String name, String defaultValue, Consumer onChange) { - var entry = nt.getStringTopic(name).getEntry(defaultValue); - entry.setDefault(defaultValue); - return new Tunable<>(entry, () -> entry.get(), onChange); - } - - /** - * Wraps a WPILib {@link PIDController} to be tunable. - * @param name The name for the tunable. Must be unique. - * @param controller The PID controller. - */ - public static void pidController(String name, PIDController controller) { - doubleValue(name + "/kP", controller.getP(), v -> controller.setP(v)); - doubleValue(name + "/kI", controller.getI(), v -> controller.setI(v)); - doubleValue(name + "/kD", controller.getD(), v -> controller.setD(v)); - doubleValue(name + "/iZone", controller.getIZone(), v -> controller.setIZone(v)); - } - - /** - * Wraps a WPILib {@link ProfiledPIDController} to be tunable. - * @param name The name for the tunable. Must be unique. - * @param controller The PID controller. - */ - public static void pidController(String name, ProfiledPIDController controller) { - doubleValue(name + "/kP", controller.getP(), v -> controller.setP(v)); - doubleValue(name + "/kI", controller.getI(), v -> controller.setI(v)); - doubleValue(name + "/kD", controller.getD(), v -> controller.setD(v)); - doubleValue(name + "/iZone", controller.getIZone(), v -> controller.setIZone(v)); - doubleValue(name + "/maxVelocity", controller.getConstraints().maxVelocity, v -> - controller.setConstraints(new TrapezoidProfile.Constraints(v, controller.getConstraints().maxAcceleration)) - ); - doubleValue(name + "/maxAcceleration", controller.getConstraints().maxAcceleration, v -> - controller.setConstraints(new TrapezoidProfile.Constraints(controller.getConstraints().maxVelocity, v)) - ); - } - - /** - * Wraps a {@link SparkPIDController REV Spark PID Controller} to be tunable. - * @param name The name for the tunable. Must be unique. - * @param controller The PID controller. - */ - public static void pidController(String name, SparkPIDController controller) { - pidController(name, controller, 0); - } - - /** - * Wraps a {@link SparkPIDController REV Spark PID Controller} to be tunable. - * @param name The name for the tunable. Must be unique. - * @param controller The PID controller. - * @param slotId The slot of the PID controller to use. - */ - public static void pidController(String name, SparkPIDController controller, int slotId) { - doubleValue(name + "/kP", controller.getP(slotId), v -> controller.setP(v, slotId)); - doubleValue(name + "/kI", controller.getI(slotId), v -> controller.setI(v, slotId)); - doubleValue(name + "/kD", controller.getD(slotId), v -> controller.setD(v, slotId)); - doubleValue(name + "/iZone", controller.getIZone(slotId), v -> controller.setIZone(v, slotId)); - doubleValue(name + "/kFF", controller.getFF(slotId), v -> controller.setFF(slotId)); - } - - /** - * Wraps a {@link TalonFX} PID controller to be tunable. - * @param name The name for the tunable. Must be unique. - * @param controller The PID controller. - */ - public static void pidController(String name, TalonFX controller) { - Slot0Configs config = new Slot0Configs(); - controller.getConfigurator().refresh(config); - - doubleValue(name + "/kP", config.kP, v -> { - controller.getConfigurator().refresh(config); - config.kP = v; - controller.getConfigurator().apply(config); - }); - doubleValue(name + "/kI", config.kI, v -> { - controller.getConfigurator().refresh(config); - config.kI = v; - controller.getConfigurator().apply(config); - }); - doubleValue(name + "/kD", config.kD, v -> { - controller.getConfigurator().refresh(config); - config.kD = v; - controller.getConfigurator().apply(config); - }); - doubleValue(name + "/kS", config.kS, v -> { - controller.getConfigurator().refresh(config); - config.kS = v; - controller.getConfigurator().apply(config); - }); - doubleValue(name + "/kV", config.kV, v -> { - controller.getConfigurator().refresh(config); - config.kV = v; - controller.getConfigurator().apply(config); - }); - doubleValue(name + "/kA", config.kA, v -> { - controller.getConfigurator().refresh(config); - config.kA = v; - controller.getConfigurator().apply(config); - }); - doubleValue(name + "/kG", config.kG, v -> { - controller.getConfigurator().refresh(config); - config.kG = v; - controller.getConfigurator().apply(config); - }); - } -} diff --git a/src/main/java/org/team340/lib/dashboard/tunables/Tunable.java b/src/main/java/org/team340/lib/dashboard/tunables/Tunable.java new file mode 100644 index 0000000..a75bd9e --- /dev/null +++ b/src/main/java/org/team340/lib/dashboard/tunables/Tunable.java @@ -0,0 +1,268 @@ +package org.team340.lib.dashboard.tunables; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkShim; +import com.revrobotics.spark.config.ClosedLoopConfig.ClosedLoopSlot; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.function.BooleanConsumer; +import edu.wpi.first.util.function.FloatConsumer; +import java.util.function.Consumer; +import java.util.function.DoubleConsumer; +import java.util.function.IntConsumer; + +/** + * The Tunable class is used to construct tunable properties of the robot to be modified + * via NetworkTables, as well as automatically displayed and edited in the dashboard. + */ +public final class Tunable { + + private Tunable() { + throw new AssertionError("This is a utility class!"); + } + + static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("GRRDashboard/Tunables"); + static boolean enabled = true; + + /** + * Disables all tunables, excluding tunables marked as always on. Disabling tunables + * is recommended when they will not be in use (i.e. at competition) due to the + * performance implications of fetching values from NetworkTables via the JNI. + */ + public static void disableAll() { + enabled = false; + } + + /** + * Creates a tunable boolean. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + */ + public static TunableBoolean booleanValue(String name, boolean defaultValue) { + return booleanValue(name, defaultValue, null); + } + + /** + * Creates a tunable boolean. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + * @param onChange A consumer that is invoked when the value of the tunable is modified. + */ + public static TunableBoolean booleanValue(String name, boolean defaultValue, BooleanConsumer onChange) { + return new TunableBoolean(name, defaultValue, onChange); + } + + /** + * Creates a tunable integer. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + */ + public static TunableInteger intValue(String name, int defaultValue) { + return intValue(name, defaultValue, null); + } + + /** + * Creates a tunable integer. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + * @param onChange A consumer that is invoked when the value of the tunable is modified. + */ + public static TunableInteger intValue(String name, int defaultValue, IntConsumer onChange) { + return new TunableInteger(name, defaultValue, onChange); + } + + /** + * Creates a tunable float. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + */ + public static TunableFloat floatValue(String name, float defaultValue) { + return floatValue(name, defaultValue, null); + } + + /** + * Creates a tunable float. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + * @param onChange A consumer that is invoked when the value of the tunable is modified. + */ + public static TunableFloat floatValue(String name, float defaultValue, FloatConsumer onChange) { + return new TunableFloat(name, defaultValue, onChange); + } + + /** + * Creates a tunable double. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + */ + public static TunableDouble doubleValue(String name, double defaultValue) { + return doubleValue(name, defaultValue, null); + } + + /** + * Creates a tunable double. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + * @param onChange A consumer that is invoked when the value of the tunable is modified. + */ + public static TunableDouble doubleValue(String name, double defaultValue, DoubleConsumer onChange) { + return new TunableDouble(name, defaultValue, onChange); + } + + /** + * Creates a tunable string. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + */ + public static TunableString stringValue(String name, String defaultValue) { + return stringValue(name, defaultValue, null); + } + + /** + * Creates a tunable string. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + * @param onChange A consumer that is invoked when the value of the tunable is modified. + */ + public static TunableString stringValue(String name, String defaultValue, Consumer onChange) { + return new TunableString(name, defaultValue, onChange); + } + + /** + * Wraps a WPILib {@link PIDController} to be tunable. + * @param name The name for the tunable. Must be unique. + * @param controller The PID controller. + */ + public static void pidController(String name, PIDController controller) { + doubleValue(name + "/kP", controller.getP(), v -> controller.setP(v)); + doubleValue(name + "/kI", controller.getI(), v -> controller.setI(v)); + doubleValue(name + "/kD", controller.getD(), v -> controller.setD(v)); + doubleValue(name + "/iZone", controller.getIZone(), v -> controller.setIZone(v)); + } + + /** + * Wraps a WPILib {@link ProfiledPIDController} to be tunable. + * @param name The name for the tunable. Must be unique. + * @param controller The PID controller. + */ + public static void pidController(String name, ProfiledPIDController controller) { + doubleValue(name + "/kP", controller.getP(), v -> controller.setP(v)); + doubleValue(name + "/kI", controller.getI(), v -> controller.setI(v)); + doubleValue(name + "/kD", controller.getD(), v -> controller.setD(v)); + doubleValue(name + "/iZone", controller.getIZone(), v -> controller.setIZone(v)); + doubleValue(name + "/maxVelocity", controller.getConstraints().maxVelocity, v -> + controller.setConstraints(new TrapezoidProfile.Constraints(v, controller.getConstraints().maxAcceleration)) + ); + doubleValue(name + "/maxAcceleration", controller.getConstraints().maxAcceleration, v -> + controller.setConstraints(new TrapezoidProfile.Constraints(controller.getConstraints().maxVelocity, v)) + ); + } + + /** + * Wraps a Spark Max's {@link SparkClosedLoopController} to be tunable. + * @param name The name for the tunable. Must be unique. + * @param sparkMax The Spark Max to tune the PID controller of. + */ + public static void pidController(String name, SparkMax sparkMax) { + pidController(name, sparkMax, ClosedLoopSlot.kSlot0); + } + + /** + * Wraps a Spark Max's {@link SparkClosedLoopController} to be tunable. + * @param name The name for the tunable. Must be unique. + * @param controller The Spark Max to tune the PID controller of. + * @param slot The slot of the PID controller to use. + */ + public static void pidController(String name, SparkMax sparkMax, ClosedLoopSlot slot) { + var config = sparkMax.configAccessor.closedLoop; + doubleValue(name + "/kP", config.getP(slot), v -> SparkShim.setP(sparkMax, slot, v)); + doubleValue(name + "/kI", config.getI(slot), v -> SparkShim.setI(sparkMax, slot, v)); + doubleValue(name + "/kD", config.getD(slot), v -> SparkShim.setD(sparkMax, slot, v)); + doubleValue(name + "/kFF", config.getFF(slot), v -> SparkShim.setFF(sparkMax, slot, v)); + doubleValue(name + "/iZone", config.getIZone(slot), v -> SparkShim.setIZone(sparkMax, slot, v)); + doubleValue(name + "/dFilter", config.getDFilter(slot), v -> SparkShim.setDFilter(sparkMax, slot, v)); + doubleValue(name + "/minOutput", config.getMinOutput(slot), v -> SparkShim.setOutputMin(sparkMax, slot, v)); + doubleValue(name + "/maxOutput", config.getMaxOutput(slot), v -> SparkShim.setOutputMax(sparkMax, slot, v)); + } + + /** + * Wraps a Spark Flex's {@link SparkClosedLoopController} to be tunable. + * @param name The name for the tunable. Must be unique. + * @param sparkFlex The Spark Flex to tune the PID controller of. + */ + public static void pidController(String name, SparkFlex sparkFlex) { + pidController(name, sparkFlex, ClosedLoopSlot.kSlot0); + } + + /** + * Wraps a Spark Flex's {@link SparkClosedLoopController} to be tunable. + * @param name The name for the tunable. Must be unique. + * @param controller The Spark Flex to tune the PID controller of. + * @param slot The slot of the PID controller to use. + */ + public static void pidController(String name, SparkFlex sparkFlex, ClosedLoopSlot slot) { + var config = sparkFlex.configAccessor.closedLoop; + doubleValue(name + "/kP", config.getP(slot), v -> SparkShim.setP(sparkFlex, slot, v)); + doubleValue(name + "/kI", config.getI(slot), v -> SparkShim.setI(sparkFlex, slot, v)); + doubleValue(name + "/kD", config.getD(slot), v -> SparkShim.setD(sparkFlex, slot, v)); + doubleValue(name + "/kFF", config.getFF(slot), v -> SparkShim.setFF(sparkFlex, slot, v)); + doubleValue(name + "/iZone", config.getIZone(slot), v -> SparkShim.setIZone(sparkFlex, slot, v)); + doubleValue(name + "/dFilter", config.getDFilter(slot), v -> SparkShim.setDFilter(sparkFlex, slot, v)); + doubleValue(name + "/minOutput", config.getMinOutput(slot), v -> SparkShim.setOutputMin(sparkFlex, slot, v)); + doubleValue(name + "/maxOutput", config.getMaxOutput(slot), v -> SparkShim.setOutputMax(sparkFlex, slot, v)); + } + + /** + * Wraps a {@link TalonFX} PID controller to be tunable. + * Note that this only applies to the slot 0 config. + * @param name The name for the tunable. Must be unique. + * @param talonFX The TalonFX to tune the PID controller of. + */ + public static void pidController(String name, TalonFX talonFX) { + Slot0Configs config = new Slot0Configs(); + talonFX.getConfigurator().refresh(config); + + doubleValue(name + "/kP", config.kP, v -> { + talonFX.getConfigurator().refresh(config); + config.kP = v; + talonFX.getConfigurator().apply(config); + }); + doubleValue(name + "/kI", config.kI, v -> { + talonFX.getConfigurator().refresh(config); + config.kI = v; + talonFX.getConfigurator().apply(config); + }); + doubleValue(name + "/kD", config.kD, v -> { + talonFX.getConfigurator().refresh(config); + config.kD = v; + talonFX.getConfigurator().apply(config); + }); + doubleValue(name + "/kS", config.kS, v -> { + talonFX.getConfigurator().refresh(config); + config.kS = v; + talonFX.getConfigurator().apply(config); + }); + doubleValue(name + "/kV", config.kV, v -> { + talonFX.getConfigurator().refresh(config); + config.kV = v; + talonFX.getConfigurator().apply(config); + }); + doubleValue(name + "/kA", config.kA, v -> { + talonFX.getConfigurator().refresh(config); + config.kA = v; + talonFX.getConfigurator().apply(config); + }); + doubleValue(name + "/kG", config.kG, v -> { + talonFX.getConfigurator().refresh(config); + config.kG = v; + talonFX.getConfigurator().apply(config); + }); + } +} diff --git a/src/main/java/org/team340/lib/dashboard/tunables/TunableBoolean.java b/src/main/java/org/team340/lib/dashboard/tunables/TunableBoolean.java new file mode 100644 index 0000000..fb89908 --- /dev/null +++ b/src/main/java/org/team340/lib/dashboard/tunables/TunableBoolean.java @@ -0,0 +1,53 @@ +package org.team340.lib.dashboard.tunables; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.networktables.BooleanEntry; +import edu.wpi.first.util.function.BooleanConsumer; +import org.team340.lib.dashboard.GRRDashboard; + +/** + * A tunable boolean value. Can be modified via NetworkTables. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class TunableBoolean implements AutoCloseable { + + private final boolean defaultValue; + private final BooleanEntry entry; + + private boolean alwaysOn = false; + + TunableBoolean(String name, boolean defaultValue, BooleanConsumer onChange) { + this.defaultValue = defaultValue; + entry = Tunable.nt.getBooleanTopic(name).getEntry(defaultValue); + entry.setDefault(defaultValue); + if (onChange != null) { + GRRDashboard.bind(() -> { + if (Tunable.enabled || alwaysOn) { + boolean[] changes = entry.readQueueValues(); + if (changes.length > 0) onChange.accept(changes[changes.length - 1]); + } + }); + } + } + + /** + * Returns the value of the tunable. + */ + public boolean value() { + return Tunable.enabled || alwaysOn ? entry.get() : defaultValue; + } + + /** + * Marks the tunable to be always on, even if {@link Tunable#disableAll()} is invoked. + */ + public TunableBoolean alwaysOn() { + alwaysOn = true; + return this; + } + + @Override + public void close() { + entry.close(); + } +} diff --git a/src/main/java/org/team340/lib/dashboard/tunables/TunableDouble.java b/src/main/java/org/team340/lib/dashboard/tunables/TunableDouble.java new file mode 100644 index 0000000..5c42eb7 --- /dev/null +++ b/src/main/java/org/team340/lib/dashboard/tunables/TunableDouble.java @@ -0,0 +1,53 @@ +package org.team340.lib.dashboard.tunables; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.networktables.DoubleEntry; +import java.util.function.DoubleConsumer; +import org.team340.lib.dashboard.GRRDashboard; + +/** + * A tunable double value. Can be modified via NetworkTables. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class TunableDouble implements AutoCloseable { + + private final double defaultValue; + private final DoubleEntry entry; + + private boolean alwaysOn = false; + + TunableDouble(String name, double defaultValue, DoubleConsumer onChange) { + this.defaultValue = defaultValue; + entry = Tunable.nt.getDoubleTopic(name).getEntry(defaultValue); + entry.setDefault(defaultValue); + if (onChange != null) { + GRRDashboard.bind(() -> { + if (Tunable.enabled || alwaysOn) { + double[] changes = entry.readQueueValues(); + if (changes.length > 0) onChange.accept(changes[changes.length - 1]); + } + }); + } + } + + /** + * Returns the value of the tunable. + */ + public double value() { + return Tunable.enabled || alwaysOn ? entry.get() : defaultValue; + } + + /** + * Marks the tunable to be always on, even if {@link Tunable#disableAll()} is invoked. + */ + public TunableDouble alwaysOn() { + alwaysOn = true; + return this; + } + + @Override + public void close() { + entry.close(); + } +} diff --git a/src/main/java/org/team340/lib/dashboard/tunables/TunableFloat.java b/src/main/java/org/team340/lib/dashboard/tunables/TunableFloat.java new file mode 100644 index 0000000..af5a95f --- /dev/null +++ b/src/main/java/org/team340/lib/dashboard/tunables/TunableFloat.java @@ -0,0 +1,53 @@ +package org.team340.lib.dashboard.tunables; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.networktables.FloatEntry; +import edu.wpi.first.util.function.FloatConsumer; +import org.team340.lib.dashboard.GRRDashboard; + +/** + * A tunable float value. Can be modified via NetworkTables. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class TunableFloat implements AutoCloseable { + + private final float defaultValue; + private final FloatEntry entry; + + private boolean alwaysOn = false; + + TunableFloat(String name, float defaultValue, FloatConsumer onChange) { + this.defaultValue = defaultValue; + entry = Tunable.nt.getFloatTopic(name).getEntry(defaultValue); + entry.setDefault(defaultValue); + if (onChange != null) { + GRRDashboard.bind(() -> { + if (Tunable.enabled || alwaysOn) { + float[] changes = entry.readQueueValues(); + if (changes.length > 0) onChange.accept(changes[changes.length - 1]); + } + }); + } + } + + /** + * Returns the value of the tunable. + */ + public float value() { + return Tunable.enabled || alwaysOn ? entry.get() : defaultValue; + } + + /** + * Marks the tunable to be always on, even if {@link Tunable#disableAll()} is invoked. + */ + public TunableFloat alwaysOn() { + alwaysOn = true; + return this; + } + + @Override + public void close() { + entry.close(); + } +} diff --git a/src/main/java/org/team340/lib/dashboard/tunables/TunableInteger.java b/src/main/java/org/team340/lib/dashboard/tunables/TunableInteger.java new file mode 100644 index 0000000..3d9f947 --- /dev/null +++ b/src/main/java/org/team340/lib/dashboard/tunables/TunableInteger.java @@ -0,0 +1,53 @@ +package org.team340.lib.dashboard.tunables; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.networktables.IntegerEntry; +import java.util.function.IntConsumer; +import org.team340.lib.dashboard.GRRDashboard; + +/** + * A tunable integer value. Can be modified via NetworkTables. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class TunableInteger implements AutoCloseable { + + private final int defaultValue; + private final IntegerEntry entry; + + private boolean alwaysOn = false; + + TunableInteger(String name, int defaultValue, IntConsumer onChange) { + this.defaultValue = defaultValue; + entry = Tunable.nt.getIntegerTopic(name).getEntry(defaultValue); + entry.setDefault(defaultValue); + if (onChange != null) { + GRRDashboard.bind(() -> { + if (Tunable.enabled || alwaysOn) { + long[] changes = entry.readQueueValues(); + if (changes.length > 0) onChange.accept((int) changes[changes.length - 1]); + } + }); + } + } + + /** + * Returns the value of the tunable. + */ + public int value() { + return Tunable.enabled || alwaysOn ? (int) entry.get() : defaultValue; + } + + /** + * Marks the tunable to be always on, even if {@link Tunable#disableAll()} is invoked. + */ + public TunableInteger alwaysOn() { + alwaysOn = true; + return this; + } + + @Override + public void close() { + entry.close(); + } +} diff --git a/src/main/java/org/team340/lib/dashboard/tunables/TunableString.java b/src/main/java/org/team340/lib/dashboard/tunables/TunableString.java new file mode 100644 index 0000000..04723f0 --- /dev/null +++ b/src/main/java/org/team340/lib/dashboard/tunables/TunableString.java @@ -0,0 +1,53 @@ +package org.team340.lib.dashboard.tunables; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.networktables.StringEntry; +import java.util.function.Consumer; +import org.team340.lib.dashboard.GRRDashboard; + +/** + * A tunable string value. Can be modified via NetworkTables. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class TunableString implements AutoCloseable { + + private final String defaultValue; + private final StringEntry entry; + + private boolean alwaysOn = false; + + TunableString(String name, String defaultValue, Consumer onChange) { + this.defaultValue = defaultValue; + entry = Tunable.nt.getStringTopic(name).getEntry(defaultValue); + entry.setDefault(defaultValue); + if (onChange != null) { + GRRDashboard.bind(() -> { + if (Tunable.enabled || alwaysOn) { + String[] changes = entry.readQueueValues(); + if (changes.length > 0) onChange.accept(changes[changes.length - 1]); + } + }); + } + } + + /** + * Returns the value of the tunable. + */ + public String value() { + return Tunable.enabled || alwaysOn ? entry.get() : defaultValue; + } + + /** + * Marks the tunable to be always on, even if {@link Tunable#disableAll()} is invoked. + */ + public TunableString alwaysOn() { + alwaysOn = true; + return this; + } + + @Override + public void close() { + entry.close(); + } +} diff --git a/src/main/java/org/team340/lib/logging/reduxlib/CanandgyroLogger.java b/src/main/java/org/team340/lib/logging/reduxlib/CanandgyroLogger.java index 815db6a..4aa514a 100644 --- a/src/main/java/org/team340/lib/logging/reduxlib/CanandgyroLogger.java +++ b/src/main/java/org/team340/lib/logging/reduxlib/CanandgyroLogger.java @@ -1,6 +1,7 @@ package org.team340.lib.logging.reduxlib; import com.reduxrobotics.sensors.canandgyro.Canandgyro; +import com.reduxrobotics.sensors.canandgyro.Canandgyro.Status; import edu.wpi.first.epilogue.CustomLoggerFor; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.DataLogger; @@ -15,7 +16,6 @@ public CanandgyroLogger() { @Override public void update(DataLogger logger, Canandgyro canandgyro) { logger.log("connected", canandgyro.isConnected()); - logger.log("temperature", canandgyro.getTemperature()); logger.log("accelerationX", canandgyro.getAccelerationX()); logger.log("accelerationY", canandgyro.getAccelerationY()); logger.log("accelerationZ", canandgyro.getAccelerationZ()); @@ -25,5 +25,6 @@ public void update(DataLogger logger, Canandgyro canandgyro) { logger.log("yaw", canandgyro.getYaw()); logger.log("pitch", canandgyro.getPitch()); logger.log("roll", canandgyro.getRoll()); + logger.log("status", canandgyro.getStatus(), Status.struct); } } diff --git a/src/main/java/org/team340/lib/logging/reduxlib/CanandmagLogger.java b/src/main/java/org/team340/lib/logging/reduxlib/CanandmagLogger.java index b331e15..5eb4e37 100644 --- a/src/main/java/org/team340/lib/logging/reduxlib/CanandmagLogger.java +++ b/src/main/java/org/team340/lib/logging/reduxlib/CanandmagLogger.java @@ -1,6 +1,7 @@ package org.team340.lib.logging.reduxlib; import com.reduxrobotics.sensors.canandmag.Canandmag; +import com.reduxrobotics.sensors.canandmag.Canandmag.Status; import edu.wpi.first.epilogue.CustomLoggerFor; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.DataLogger; @@ -18,6 +19,6 @@ public void update(DataLogger logger, Canandmag canandmag) { logger.log("absolutePosition", canandmag.getAbsPosition()); logger.log("magnetInRange", canandmag.magnetInRange()); logger.log("position", canandmag.getPosition()); - logger.log("temperature", canandmag.getTemperature()); + logger.log("status", canandmag.getStatus(), Status.struct); } } diff --git a/src/main/java/org/team340/lib/logging/revlib/SparkAbsoluteEncoderLogger.java b/src/main/java/org/team340/lib/logging/revlib/SparkAbsoluteEncoderLogger.java index 4e23558..f9fd082 100644 --- a/src/main/java/org/team340/lib/logging/revlib/SparkAbsoluteEncoderLogger.java +++ b/src/main/java/org/team340/lib/logging/revlib/SparkAbsoluteEncoderLogger.java @@ -1,6 +1,6 @@ package org.team340.lib.logging.revlib; -import com.revrobotics.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkAbsoluteEncoder; import edu.wpi.first.epilogue.CustomLoggerFor; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.DataLogger; diff --git a/src/main/java/org/team340/lib/logging/revlib/SparkAnalogSensorLogger.java b/src/main/java/org/team340/lib/logging/revlib/SparkAnalogSensorLogger.java new file mode 100644 index 0000000..590ea6d --- /dev/null +++ b/src/main/java/org/team340/lib/logging/revlib/SparkAnalogSensorLogger.java @@ -0,0 +1,21 @@ +package org.team340.lib.logging.revlib; + +import com.revrobotics.spark.SparkAnalogSensor; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(SparkAnalogSensor.class) +public class SparkAnalogSensorLogger extends ClassSpecificLogger { + + public SparkAnalogSensorLogger() { + super(SparkAnalogSensor.class); + } + + @Override + public void update(DataLogger logger, SparkAnalogSensor analogSensor) { + logger.log("position", analogSensor.getPosition()); + logger.log("velocity", analogSensor.getVelocity()); + logger.log("voltage", analogSensor.getVoltage()); + } +} diff --git a/src/main/java/org/team340/lib/logging/revlib/SparkClosedLoopControllerLogger.java b/src/main/java/org/team340/lib/logging/revlib/SparkClosedLoopControllerLogger.java new file mode 100644 index 0000000..42b1905 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/revlib/SparkClosedLoopControllerLogger.java @@ -0,0 +1,19 @@ +package org.team340.lib.logging.revlib; + +import com.revrobotics.spark.SparkClosedLoopController; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(SparkClosedLoopController.class) +public class SparkClosedLoopControllerLogger extends ClassSpecificLogger { + + public SparkClosedLoopControllerLogger() { + super(SparkClosedLoopController.class); + } + + @Override + public void update(DataLogger logger, SparkClosedLoopController pidController) { + // No-op + } +} diff --git a/src/main/java/org/team340/lib/logging/revlib/SparkFlexLogger.java b/src/main/java/org/team340/lib/logging/revlib/SparkFlexLogger.java index 65db170..03e3905 100644 --- a/src/main/java/org/team340/lib/logging/revlib/SparkFlexLogger.java +++ b/src/main/java/org/team340/lib/logging/revlib/SparkFlexLogger.java @@ -1,22 +1,22 @@ package org.team340.lib.logging.revlib; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkShim; import edu.wpi.first.epilogue.CustomLoggerFor; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.DataLogger; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import org.team340.lib.logging.revlib.structs.SparkFaultsStruct; +import org.team340.lib.logging.revlib.structs.SparkWarningsStruct; -@CustomLoggerFor(CANSparkFlex.class) -public class SparkFlexLogger extends ClassSpecificLogger { +@CustomLoggerFor(SparkFlex.class) +public class SparkFlexLogger extends ClassSpecificLogger { public SparkFlexLogger() { - super(CANSparkFlex.class); + super(SparkFlex.class); } @Override - public void update(DataLogger logger, CANSparkFlex sparkFlex) { + public void update(DataLogger logger, SparkFlex sparkFlex) { double appliedOutput = sparkFlex.getAppliedOutput(); double busVoltage = sparkFlex.getBusVoltage(); @@ -25,69 +25,11 @@ public void update(DataLogger logger, CANSparkFlex sparkFlex) { logger.log("busVoltage", busVoltage); logger.log("motorTemperature", sparkFlex.getMotorTemperature()); logger.log("outputCurrent", sparkFlex.getOutputCurrent()); - logger.log("faults", sparkFlex.getFaults(), faultsStruct); - logger.log("stickyFaults", sparkFlex.getStickyFaults(), faultsStruct); - - if (sparkFlex.getMotorType().equals(MotorType.kBrushless)) { - logger.log("position", sparkFlex.getEncoder().getPosition()); - logger.log("velocity", sparkFlex.getEncoder().getVelocity()); - } - } - - private static final SparkFlexFaultsStruct faultsStruct = new SparkFlexFaultsStruct(); - - private static final class SparkFlexFaultsStruct implements Struct { - - @Override - public Class getTypeClass() { - return Short.class; - } - - @Override - public String getTypeName() { - return "SparkFlexFaults"; - } - - @Override - public int getSize() { - return kSizeInt16; - } - - @Override - public String getSchema() { - return ( - "bool kBrownout:1; " + - "bool kOvercurrent:1; " + - "bool kIWDTReset:1; " + - "bool kMotorFault:1; " + - "bool kSensorFault:1; " + - "bool kStall:1; " + - "bool kEEPROMCRC:1; " + - "bool kCANTX:1; " + - "bool kCANRX:1; " + - "bool kHasReset:1; " + - "bool kDRVFault:1; " + - "bool kOtherFault:1; " + - "bool kSoftLimitFwd:1; " + - "bool kSoftLimitRev:1; " + - "bool kHardLimitFwd:1; " + - "bool kHardLimitRev:1;" - ); - } - - @Override - public Short unpack(ByteBuffer bb) { - return bb.getShort(); - } - - @Override - public void pack(ByteBuffer bb, Short value) { - bb.putShort(value); - } - - @Override - public boolean isImmutable() { - return true; - } + logger.log("position", sparkFlex.getEncoder().getPosition()); + logger.log("velocity", sparkFlex.getEncoder().getVelocity()); + logger.log("faults", SparkShim.getFaults(sparkFlex), SparkFaultsStruct.inst); + logger.log("stickyFaults", SparkShim.getStickyFaults(sparkFlex), SparkFaultsStruct.inst); + logger.log("warnings", SparkShim.getWarnings(sparkFlex), SparkWarningsStruct.inst); + logger.log("stickyWarnings", SparkShim.getStickyWarnings(sparkFlex), SparkWarningsStruct.inst); } } diff --git a/src/main/java/org/team340/lib/logging/revlib/SparkLimitSwitchLogger.java b/src/main/java/org/team340/lib/logging/revlib/SparkLimitSwitchLogger.java index 6fc2ab4..398a02d 100644 --- a/src/main/java/org/team340/lib/logging/revlib/SparkLimitSwitchLogger.java +++ b/src/main/java/org/team340/lib/logging/revlib/SparkLimitSwitchLogger.java @@ -1,6 +1,6 @@ package org.team340.lib.logging.revlib; -import com.revrobotics.SparkLimitSwitch; +import com.revrobotics.spark.SparkLimitSwitch; import edu.wpi.first.epilogue.CustomLoggerFor; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.DataLogger; diff --git a/src/main/java/org/team340/lib/logging/revlib/SparkMaxLogger.java b/src/main/java/org/team340/lib/logging/revlib/SparkMaxLogger.java index f1a5822..264dbfe 100644 --- a/src/main/java/org/team340/lib/logging/revlib/SparkMaxLogger.java +++ b/src/main/java/org/team340/lib/logging/revlib/SparkMaxLogger.java @@ -1,22 +1,22 @@ package org.team340.lib.logging.revlib; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkShim; import edu.wpi.first.epilogue.CustomLoggerFor; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.DataLogger; -import edu.wpi.first.util.struct.Struct; -import java.nio.ByteBuffer; +import org.team340.lib.logging.revlib.structs.SparkFaultsStruct; +import org.team340.lib.logging.revlib.structs.SparkWarningsStruct; -@CustomLoggerFor(CANSparkMax.class) -public class SparkMaxLogger extends ClassSpecificLogger { +@CustomLoggerFor(SparkMax.class) +public class SparkMaxLogger extends ClassSpecificLogger { public SparkMaxLogger() { - super(CANSparkMax.class); + super(SparkMax.class); } @Override - public void update(DataLogger logger, CANSparkMax sparkMax) { + public void update(DataLogger logger, SparkMax sparkMax) { double appliedOutput = sparkMax.getAppliedOutput(); double busVoltage = sparkMax.getBusVoltage(); @@ -25,69 +25,11 @@ public void update(DataLogger logger, CANSparkMax sparkMax) { logger.log("busVoltage", busVoltage); logger.log("motorTemperature", sparkMax.getMotorTemperature()); logger.log("outputCurrent", sparkMax.getOutputCurrent()); - logger.log("faults", sparkMax.getFaults(), faultsStruct); - logger.log("stickyFaults", sparkMax.getStickyFaults(), faultsStruct); - - if (sparkMax.getMotorType().equals(MotorType.kBrushless)) { - logger.log("position", sparkMax.getEncoder().getPosition()); - logger.log("velocity", sparkMax.getEncoder().getVelocity()); - } - } - - private static final SparkMaxFaultsStruct faultsStruct = new SparkMaxFaultsStruct(); - - private static final class SparkMaxFaultsStruct implements Struct { - - @Override - public Class getTypeClass() { - return Short.class; - } - - @Override - public String getTypeName() { - return "SparkFlexFaults"; - } - - @Override - public int getSize() { - return kSizeInt16; - } - - @Override - public String getSchema() { - return ( - "bool kBrownout:1; " + - "bool kOvercurrent:1; " + - "bool kIWDTReset:1; " + - "bool kMotorFault:1; " + - "bool kSensorFault:1; " + - "bool kStall:1; " + - "bool kEEPROMCRC:1; " + - "bool kCANTX:1; " + - "bool kCANRX:1; " + - "bool kHasReset:1; " + - "bool kDRVFault:1; " + - "bool kOtherFault:1; " + - "bool kSoftLimitFwd:1; " + - "bool kSoftLimitRev:1; " + - "bool kHardLimitFwd:1; " + - "bool kHardLimitRev:1;" - ); - } - - @Override - public Short unpack(ByteBuffer bb) { - return bb.getShort(); - } - - @Override - public void pack(ByteBuffer bb, Short value) { - bb.putShort(value); - } - - @Override - public boolean isImmutable() { - return true; - } + logger.log("position", sparkMax.getEncoder().getPosition()); + logger.log("velocity", sparkMax.getEncoder().getVelocity()); + logger.log("faults", SparkShim.getFaults(sparkMax), SparkFaultsStruct.inst); + logger.log("stickyFaults", SparkShim.getStickyFaults(sparkMax), SparkFaultsStruct.inst); + logger.log("warnings", SparkShim.getWarnings(sparkMax), SparkWarningsStruct.inst); + logger.log("stickyWarnings", SparkShim.getStickyWarnings(sparkMax), SparkWarningsStruct.inst); } } diff --git a/src/main/java/org/team340/lib/logging/revlib/SparkPIDControllerLogger.java b/src/main/java/org/team340/lib/logging/revlib/SparkPIDControllerLogger.java deleted file mode 100644 index da17a0e..0000000 --- a/src/main/java/org/team340/lib/logging/revlib/SparkPIDControllerLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.revlib; - -import com.revrobotics.SparkPIDController; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.DataLogger; - -@CustomLoggerFor(SparkPIDController.class) -public class SparkPIDControllerLogger extends ClassSpecificLogger { - - public SparkPIDControllerLogger() { - super(SparkPIDController.class); - } - - @Override - public void update(DataLogger logger, SparkPIDController pidController) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/revlib/structs/SparkFaultsStruct.java b/src/main/java/org/team340/lib/logging/revlib/structs/SparkFaultsStruct.java new file mode 100644 index 0000000..e6915e6 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/revlib/structs/SparkFaultsStruct.java @@ -0,0 +1,53 @@ +package org.team340.lib.logging.revlib.structs; + +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class SparkFaultsStruct implements Struct { + + public static final SparkFaultsStruct inst = new SparkFaultsStruct(); + + @Override + public Class getTypeClass() { + return Integer.class; + } + + @Override + public String getTypeName() { + return "SparkFaults"; + } + + @Override + public int getSize() { + return kSizeInt8; + } + + @Override + public String getSchema() { + return ( + "bool other:1; " + + "bool motorType:1; " + + "bool sensor:1; " + + "bool can:1; " + + "bool temperature:1; " + + "bool gateDriver:1; " + + "bool escEeprom:1; " + + "bool firmware:1;" + ); + } + + @Override + public Integer unpack(ByteBuffer bb) { + return Byte.toUnsignedInt(bb.get()); + } + + @Override + public void pack(ByteBuffer bb, Integer value) { + bb.put((byte) value.intValue()); + } + + @Override + public boolean isImmutable() { + return true; + } +} diff --git a/src/main/java/org/team340/lib/logging/revlib/structs/SparkWarningsStruct.java b/src/main/java/org/team340/lib/logging/revlib/structs/SparkWarningsStruct.java new file mode 100644 index 0000000..4be8935 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/revlib/structs/SparkWarningsStruct.java @@ -0,0 +1,53 @@ +package org.team340.lib.logging.revlib.structs; + +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class SparkWarningsStruct implements Struct { + + public static final SparkWarningsStruct inst = new SparkWarningsStruct(); + + @Override + public Class getTypeClass() { + return Integer.class; + } + + @Override + public String getTypeName() { + return "SparkWarnings"; + } + + @Override + public int getSize() { + return kSizeInt8; + } + + @Override + public String getSchema() { + return ( + "bool brownout:1; " + + "bool overcurrent:1; " + + "bool escEeprom:1; " + + "bool extEeprom:1; " + + "bool sensor:1; " + + "bool stall:1; " + + "bool hasReset:1; " + + "bool other:1;" + ); + } + + @Override + public Integer unpack(ByteBuffer bb) { + return Byte.toUnsignedInt(bb.get()); + } + + @Override + public void pack(ByteBuffer bb, Integer value) { + bb.put((byte) value.intValue()); + } + + @Override + public boolean isImmutable() { + return true; + } +} diff --git a/src/main/java/org/team340/lib/swerve/ForwardPerspective.java b/src/main/java/org/team340/lib/swerve/Perspective.java similarity index 89% rename from src/main/java/org/team340/lib/swerve/ForwardPerspective.java rename to src/main/java/org/team340/lib/swerve/Perspective.java index cf7f830..17006f9 100644 --- a/src/main/java/org/team340/lib/swerve/ForwardPerspective.java +++ b/src/main/java/org/team340/lib/swerve/Perspective.java @@ -5,28 +5,28 @@ import org.team340.lib.util.Alliance; /** - * Describes a perspective for {@link ChassisSpeeds}. + * Describes a forward (X+ direction) perspective for {@link ChassisSpeeds}. */ -public enum ForwardPerspective { +public enum Perspective { /** * The speeds are relative to the operator's perspective. If the robot * is on the blue alliance, X+ drives towards the red alliance. If the * robot is on the red alliance, X+ drives towards the blue alliance. */ - OPERATOR { + kOperator { @Override void toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { - (Alliance.isBlue() ? BLUE_ALLIANCE : RED_ALLIANCE).toRobotSpeeds(speeds, robotAngle); + (Alliance.isBlue() ? kBlueAlliance : kRedAlliance).toRobotSpeeds(speeds, robotAngle); } @Override void toPerspectiveSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { - (Alliance.isBlue() ? BLUE_ALLIANCE : RED_ALLIANCE).toPerspectiveSpeeds(speeds, robotAngle); + (Alliance.isBlue() ? kBlueAlliance : kRedAlliance).toPerspectiveSpeeds(speeds, robotAngle); } @Override Rotation2d getTareRotation() { - return (Alliance.isBlue() ? BLUE_ALLIANCE : RED_ALLIANCE).getTareRotation(); + return (Alliance.isBlue() ? kBlueAlliance : kRedAlliance).getTareRotation(); } }, @@ -34,7 +34,7 @@ Rotation2d getTareRotation() { * The speeds are relative to the blue alliance perspective. * X+ drives towards the red alliance. */ - BLUE_ALLIANCE { + kBlueAlliance { @Override void toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { speeds.toRobotRelativeSpeeds(robotAngle); @@ -55,7 +55,7 @@ Rotation2d getTareRotation() { * The speeds are relative to the red alliance perspective. * X+ drives towards the blue alliance. */ - RED_ALLIANCE { + kRedAlliance { @Override void toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { speeds.toRobotRelativeSpeeds(robotAngle.rotateBy(Rotation2d.kPi)); @@ -76,7 +76,7 @@ Rotation2d getTareRotation() { * The speeds are relative to the robot's perspective. * X+ drives forwards relative to the chassis. */ - ROBOT { + kRobot { @Override void toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) {} diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPI.java b/src/main/java/org/team340/lib/swerve/SwerveAPI.java index deee183..1e78ba8 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveAPI.java +++ b/src/main/java/org/team340/lib/swerve/SwerveAPI.java @@ -27,13 +27,16 @@ import org.team340.lib.util.Sleep; import org.team340.robot.Robot; +/** + * An implementation of a swerve drivetrain, with support for various hardware. + */ public class SwerveAPI implements AutoCloseable { public final SwerveState state; - final SwerveIMU imu; - final SwerveModule[] modules; final SwerveConfig config; + final SwerveModule[] modules; + final SwerveIMU imu; private final int moduleCount; private final double farthestModule; @@ -52,6 +55,10 @@ public class SwerveAPI implements AutoCloseable { private Consumer imuSimHook = s -> {}; + /** + * Create the drivetrain. + * @param config The drivetrain's config. + */ public SwerveAPI(SwerveConfig config) { this.config = config; @@ -125,13 +132,13 @@ public void refresh() { } /** - * Tares the rotation of the robot. Useful for fixing an out of sync or drifting IMU. In - * most cases, a forward perspective of {@link ForwardPerspective#OPERATOR OPERATOR} is - * desirable. {@link ForwardPerspective#ROBOT ROBOT} will no-op. - * @param forwardPerspective The perspective to tare the rotation to. + * Tares the rotation of the robot. Useful for fixing an out of sync or drifting + * IMU. For most cases, a perspective of {@link Perspective#kOperator} is + * desirable. {@link Perspective#kRobot} will no-op. + * @param perspective The perspective to tare the rotation to. */ - public void tareRotation(ForwardPerspective forwardPerspective) { - var rotation = forwardPerspective.getTareRotation(); + public void tareRotation(Perspective perspective) { + var rotation = perspective.getTareRotation(); if (rotation == null) return; odometryMutex.lock(); try { @@ -184,7 +191,7 @@ public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix= w_max) { speeds.vxMetersPerSecond = 0.0; @@ -285,7 +287,7 @@ public void applySpeeds( } lastRatelimit = now; - forwardPerspective.toPerspectiveSpeeds(state.targetSpeeds, lastRobotAngle); + perspective.toPerspectiveSpeeds(state.targetSpeeds, lastRobotAngle); double vx_l = state.targetSpeeds.vxMetersPerSecond; double vy_l = state.targetSpeeds.vyMetersPerSecond; @@ -317,7 +319,7 @@ public void applySpeeds( if (discretize) speeds.discretize(config.discretizationPeriod); lastRobotAngle = state.pose.getRotation(); - forwardPerspective.toRobotSpeeds(speeds, lastRobotAngle); + perspective.toRobotSpeeds(speeds, lastRobotAngle); Math2.copyInto(speeds, state.targetSpeeds); state.targetTwist.dx = speeds.vxMetersPerSecond * config.period; @@ -327,21 +329,6 @@ public void applySpeeds( applyStates(kinematics.toSwerveModuleStates(speeds)); } - /** - * Drives the modules to an X formation to stop the robot from moving. - */ - public void applyLockedWheels() { - lastRatelimit = Timer.getFPGATimestamp(); - state.targetSpeeds.vxMetersPerSecond = 0.0; - state.targetSpeeds.vyMetersPerSecond = 0.0; - state.targetSpeeds.omegaRadiansPerSecond = 0.0; - state.targetTwist.dx = 0.0; - state.targetTwist.dy = 0.0; - state.targetTwist.dtheta = 0.0; - - applyStates(lockedStates); - } - /** * Drives using module states. * @param states The states to apply. @@ -358,6 +345,28 @@ public void applyStates(SwerveModuleState[] states) { } } + /** + * Drives the modules to stop the robot from moving. + * @param lock If the wheels should be driven to an X formation to stop the robot from being pushed. + */ + public void applyStop(boolean lock) { + lastRatelimit = Timer.getFPGATimestamp(); + Math2.zero(state.targetSpeeds); + Math2.zero(state.targetTwist); + + for (int i = 0; i < moduleCount; i++) { + SwerveModuleState state; + if (lock) { + state = lockedStates[i]; + } else { + state = modules[i].getNextTarget(); + state.speedMetersPerSecond = 0.0; + } + + modules[i].applyState(state); + } + } + /** * Drives the robot using open-loop voltage. Intended for characterization. * Plumbing for recording device voltage via their Java API is intentionally diff --git a/src/main/java/org/team340/lib/swerve/SwerveModule.java b/src/main/java/org/team340/lib/swerve/SwerveModule.java index a528549..598d6f6 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveModule.java +++ b/src/main/java/org/team340/lib/swerve/SwerveModule.java @@ -149,7 +149,7 @@ public void applyState(SwerveModuleState state) { } boolean flipped = false; - if (Math.abs(angleDelta.getRadians()) > Math2.HALF_PI) { + if (Math.abs(angleDelta.getRadians()) > Math2.kHalfPi) { state.speedMetersPerSecond *= -1.0; state.angle = state.angle.rotateBy(Rotation2d.kPi); flipped = true; diff --git a/src/main/java/org/team340/lib/swerve/SwerveState.java b/src/main/java/org/team340/lib/swerve/SwerveState.java index 32c6c25..773463e 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveState.java +++ b/src/main/java/org/team340/lib/swerve/SwerveState.java @@ -26,7 +26,7 @@ public static final class Modules { /** The last target states of the modules. */ public final SwerveModuleState[] lastTarget; - public Modules(SwerveModule[] modules) { + private Modules(SwerveModule[] modules) { positions = new SwerveModulePosition[modules.length]; states = new SwerveModuleState[modules.length]; nextTarget = new SwerveModuleState[modules.length]; diff --git a/src/main/java/org/team340/lib/swerve/SwerveTunables.java b/src/main/java/org/team340/lib/swerve/SwerveTunables.java index 8b622ab..c6a4be3 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveTunables.java +++ b/src/main/java/org/team340/lib/swerve/SwerveTunables.java @@ -1,6 +1,6 @@ package org.team340.lib.swerve; -import org.team340.lib.dashboard.Tunable; +import org.team340.lib.dashboard.tunables.Tunable; import org.team340.lib.swerve.config.SwerveConfig; /** diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java index 2cf608c..2d48052 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java @@ -12,7 +12,7 @@ interface SwerveBaseHardware extends AutoCloseable { * utilized for telemetry that is not necessarily required for swerve * to function. */ - static final double TELEMETRY_CAN_PERIOD = 0.2; + static final double kTelemetryCANPeriod = 0.2; /** * Returns the device's underlying API. diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java index 1f3b58a..7d1a38f 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java @@ -10,9 +10,13 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.reduxrobotics.sensors.canandmag.Canandmag; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.epilogue.logging.DataLogger; import edu.wpi.first.epilogue.logging.errors.ErrorHandler; import edu.wpi.first.units.measure.Angle; @@ -26,12 +30,9 @@ import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.hardware.SwerveMotors.SwerveMotor; -import org.team340.lib.util.Mutable; -import org.team340.lib.util.ctre.PhoenixUtil; -import org.team340.lib.util.redux.ReduxUtil; -import org.team340.lib.util.rev.SparkAbsoluteEncoderConfig; -import org.team340.lib.util.rev.SparkFlexConfig; -import org.team340.lib.util.rev.SparkMaxConfig; +import org.team340.lib.util.PhoenixUtil; +import org.team340.lib.util.ReduxUtil; +import org.team340.lib.util.RevUtil; /** * Contains implementations for absolute encoders to be used with the {@link SwerveAPI}. @@ -91,28 +92,30 @@ public boolean hookStatus() { */ public static SwerveEncoder.Ctor sparkMaxEncoder(double offset, boolean inverted) { return (config, turnMotor) -> { - if (!(turnMotor.getAPI() instanceof CANSparkMax)) { + if (!(turnMotor.getAPI() instanceof SparkMax sparkMax)) { throw new UnsupportedOperationException("Turn motor is not a Spark Max"); } var deviceLogger = new SparkAbsoluteEncoderLogger(); - CANSparkMax sparkMax = (CANSparkMax) turnMotor.getAPI(); SparkAbsoluteEncoder encoder = sparkMax.getAbsoluteEncoder(); - new SparkMaxConfig() - .setPeriodicFramePeriod(SparkMaxConfig.Frame.S5, (int) (config.odometryPeriod * 1000.0)) - .setPeriodicFramePeriod( - SparkMaxConfig.Frame.S6, - (int) (SwerveBaseHardware.TELEMETRY_CAN_PERIOD * 1000.0) - ) - .apply(sparkMax); - - new SparkAbsoluteEncoderConfig() - .setPositionConversionFactor(1.0) - .setVelocityConversionFactor(1.0 / 60.0) - .setInverted(inverted) - .setZeroOffset(offset) - .apply(sparkMax, encoder); + var sparkConfig = new SparkMaxConfig(); + + sparkConfig.signals + .status5Period((int) (config.odometryPeriod * 1000.0)) + .status5Enabled(true) + .status6Period((int) (SwerveBaseHardware.kTelemetryCANPeriod * 1000.0)) + .status6Enabled(true); + + sparkConfig.absoluteEncoder + .positionConversionFactor(1.0) + .velocityConversionFactor(1.0 / 60.0) + .inverted(inverted) + .zeroOffset(offset); + + RevUtil.run("Configure Absolute Encoder", sparkMax, () -> + sparkMax.configure(sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters) + ); return new SwerveEncoder() { @Override @@ -143,28 +146,30 @@ public void close() {} */ public static SwerveEncoder.Ctor sparkFlexEncoder(double offset, boolean inverted) { return (config, turnMotor) -> { - if (!(turnMotor.getAPI() instanceof CANSparkFlex)) { + if (!(turnMotor.getAPI() instanceof SparkFlex sparkFlex)) { throw new UnsupportedOperationException("Turn motor is not a Spark Flex"); } var deviceLogger = new SparkAbsoluteEncoderLogger(); - CANSparkFlex sparkFlex = (CANSparkFlex) turnMotor.getAPI(); SparkAbsoluteEncoder encoder = sparkFlex.getAbsoluteEncoder(); - new SparkFlexConfig() - .setPeriodicFramePeriod(SparkFlexConfig.Frame.S5, (int) (config.odometryPeriod * 1000.0)) - .setPeriodicFramePeriod( - SparkFlexConfig.Frame.S6, - (int) (SwerveBaseHardware.TELEMETRY_CAN_PERIOD * 1000.0) - ) - .apply(sparkFlex); - - new SparkAbsoluteEncoderConfig() - .setPositionConversionFactor(1.0) - .setVelocityConversionFactor(1.0 / 60.0) - .setInverted(inverted) - .setZeroOffset(offset) - .apply(sparkFlex, encoder); + var sparkConfig = new SparkFlexConfig(); + + sparkConfig.signals + .status5Period((int) (config.odometryPeriod * 1000.0)) + .status5Enabled(true) + .status6Period((int) (SwerveBaseHardware.kTelemetryCANPeriod * 1000.0)) + .status6Enabled(true); + + sparkConfig.absoluteEncoder + .positionConversionFactor(1.0) + .velocityConversionFactor(1.0 / 60.0) + .inverted(inverted) + .zeroOffset(offset); + + RevUtil.run("Configure Absolute Encoder", sparkFlex, () -> + sparkFlex.configure(sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters) + ); return new SwerveEncoder() { @Override @@ -207,6 +212,7 @@ public static SwerveEncoder.Ctor canandmag(int id, double offset, boolean invert .setVelocityFramePeriod(config.odometryPeriod) .setZeroOffset(offset); + canandmag.clearStickyFaults(); ReduxUtil.applySettings(canandmag, settings); return new SwerveEncoder() { @@ -243,7 +249,7 @@ public static SwerveEncoder.Ctor canCoder(int id, double offset, boolean inverte return (config, turnMotor) -> { var deviceLogger = new CANcoderLogger(); CANcoder canCoder = new CANcoder(id, config.phoenixCanBus); - Mutable hookStatus = new Mutable<>(false); + boolean hookStatus = false; StatusSignal position = canCoder.getPosition().clone(); StatusSignal velocity = canCoder.getVelocity().clone(); @@ -254,16 +260,18 @@ public static SwerveEncoder.Ctor canCoder(int id, double offset, boolean inverte ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; - PhoenixUtil.run(canCoder, "Apply CANcoderConfiguration", () -> + PhoenixUtil.run("Clear Sticky Faults", canCoder, () -> canCoder.clearStickyFaults()); + PhoenixUtil.run("Apply CANcoderConfiguration", canCoder, () -> canCoder.getConfigurator().apply(canCoderConfig) ); + PhoenixUtil.run("Set Update Frequency", canCoder, () -> + BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.odometryPeriod, position, velocity) + ); + PhoenixUtil.run("Optimize Bus Utilization", canCoder, () -> + canCoder.optimizeBusUtilization(1.0 / SwerveBaseHardware.kTelemetryCANPeriod, 0.05) + ); - BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.odometryPeriod, position, velocity); - canCoder.optimizeBusUtilization(1.0 / SwerveBaseHardware.TELEMETRY_CAN_PERIOD, 0.05); - - if (turnMotor.getAPI() instanceof TalonFX) { - TalonFX talonFX = (TalonFX) turnMotor.getAPI(); - + if (turnMotor.getAPI() instanceof TalonFX talonFX) { var feedbackConfig = new FeedbackConfigs(); feedbackConfig.FeedbackRemoteSensorID = id; feedbackConfig.RotorToSensorRatio = config.turnGearRatio; @@ -274,15 +282,17 @@ public static SwerveEncoder.Ctor canCoder(int id, double offset, boolean inverte var closedLoopConfig = new ClosedLoopGeneralConfigs(); closedLoopConfig.ContinuousWrap = true; - PhoenixUtil.run(talonFX, "Apply FeedbackConfigs", () -> talonFX.getConfigurator().apply(feedbackConfig) + PhoenixUtil.run("Apply FeedbackConfigs", talonFX, () -> talonFX.getConfigurator().apply(feedbackConfig) ); - PhoenixUtil.run(talonFX, "Apply ClosedLoopGeneralConfigs", () -> + PhoenixUtil.run("Apply ClosedLoopGeneralConfigs", talonFX, () -> talonFX.getConfigurator().apply(closedLoopConfig) ); - hookStatus.value = true; + hookStatus = true; } + boolean hookFinal = hookStatus; + return new SwerveEncoder() { @Override public double getPosition() { @@ -291,7 +301,7 @@ public double getPosition() { @Override public boolean hookStatus() { - return hookStatus.get(); + return hookFinal; } @Override diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java index b1ef01a..58634f8 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java @@ -25,7 +25,8 @@ import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.hardware.SwerveIMUs.SwerveIMU.IMUSimHook; import org.team340.lib.util.Mutable; -import org.team340.lib.util.redux.ReduxUtil; +import org.team340.lib.util.PhoenixUtil; +import org.team340.lib.util.ReduxUtil; /** * Contains implementations for IMUs to be used with the {@link SwerveAPI}. @@ -151,6 +152,7 @@ public static SwerveIMU.Ctor canandgyro(int id) { .setStatusFramePeriod(config.period) .setYawFramePeriod(config.odometryPeriod); + canandgyro.clearStickyFaults(); ReduxUtil.applySettings(canandgyro, settings); return new SwerveIMU() { @@ -215,9 +217,16 @@ public static SwerveIMU.Ctor pigeon2(int id) { StatusSignal pitchVelocity = pigeon2.getAngularVelocityXWorld(); StatusSignal rollVelocity = pigeon2.getAngularVelocityYWorld(); - BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.odometryPeriod, yaw, yawVelocity); - BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.period, pitch, roll, pitchVelocity, rollVelocity); - pigeon2.optimizeBusUtilization(1.0 / SwerveBaseHardware.TELEMETRY_CAN_PERIOD, 0.05); + PhoenixUtil.run("Clear Sticky Faults", pigeon2, () -> pigeon2.clearStickyFaults()); + PhoenixUtil.run("Set Update Frequency", pigeon2, () -> + BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.odometryPeriod, yaw, yawVelocity) + ); + PhoenixUtil.run("Set Update Frequency", pigeon2, () -> + BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.period, pitch, roll, pitchVelocity, rollVelocity) + ); + PhoenixUtil.run("Optimize Bus Utilization", pigeon2, () -> + pigeon2.optimizeBusUtilization(1.0 / SwerveBaseHardware.kTelemetryCANPeriod, 0.05) + ); return new SwerveIMU() { @Override diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java index 4da2284..0a92d74 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java @@ -10,15 +10,22 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkPIDController.ArbFFUnits; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkShim; +import com.revrobotics.spark.config.ClosedLoopConfig.ClosedLoopSlot; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.epilogue.logging.DataLogger; import edu.wpi.first.epilogue.logging.errors.ErrorHandler; import edu.wpi.first.units.measure.Angle; @@ -31,11 +38,8 @@ import org.team340.lib.logging.revlib.SparkMaxLogger; import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.config.SwerveConfig; -import org.team340.lib.util.ctre.PhoenixUtil; -import org.team340.lib.util.rev.RelativeEncoderConfig; -import org.team340.lib.util.rev.SparkFlexConfig; -import org.team340.lib.util.rev.SparkMaxConfig; -import org.team340.lib.util.rev.SparkPIDControllerConfig; +import org.team340.lib.util.PhoenixUtil; +import org.team340.lib.util.RevUtil; /** * Contains implementations for motors to be used with the {@link SwerveAPI}. @@ -114,44 +118,46 @@ public static SwerveMotor construct(Ctor ctor, SwerveConfig config, boolean isMo public static SwerveMotor.Ctor sparkMax(int id, MotorType type, boolean inverted) { return (config, isMoveMotor) -> { var deviceLogger = new SparkMaxLogger(); - CANSparkMax sparkMax = new CANSparkMax(id, type); + SparkMax sparkMax = new SparkMax(id, type); RelativeEncoder relativeEncoder = sparkMax.getEncoder(); - SparkPIDController pidController = sparkMax.getPIDController(); - int PID_SLOT = 0; + SparkClosedLoopController pid = sparkMax.getClosedLoopController(); + ClosedLoopSlot pidSlot = ClosedLoopSlot.kSlot0; double[] pidGains = isMoveMotor ? config.movePID : config.turnPID; double[] ffGains = isMoveMotor ? config.moveFF : new double[] { 0.0, 0.0 }; - new SparkMaxConfig() - .clearFaults() - .enableVoltageCompensation(config.voltage) - .setSmartCurrentLimit((int) (isMoveMotor ? config.moveCurrentLimit : config.turnCurrentLimit)) - .setIdleMode( + var sparkConfig = new SparkMaxConfig() + .voltageCompensation(config.voltage) + .smartCurrentLimit((int) (isMoveMotor ? config.moveCurrentLimit : config.turnCurrentLimit)) + .idleMode( (isMoveMotor ? config.moveBrakeMode : config.turnBrakeMode) ? IdleMode.kBrake : IdleMode.kCoast ) - .setInverted(inverted) - .setPeriodicFramePeriod(SparkMaxConfig.Frame.S0, (int) (config.period * 1000.0)) - .setPeriodicFramePeriod(SparkMaxConfig.Frame.S1, (int) (config.odometryPeriod * 1000.0)) - .setPeriodicFramePeriod(SparkMaxConfig.Frame.S2, (int) (config.odometryPeriod * 1000.0)) - .setPeriodicFramePeriod(SparkMaxConfig.Frame.S3, 10000) - .setPeriodicFramePeriod(SparkMaxConfig.Frame.S4, 10000) - .setPeriodicFramePeriod(SparkMaxConfig.Frame.S5, 10000) - .setPeriodicFramePeriod(SparkMaxConfig.Frame.S6, 10000) - .setPeriodicFramePeriod(SparkMaxConfig.Frame.S7, 10000) - .apply(sparkMax); - - new SparkPIDControllerConfig() - .setFeedbackDevice(relativeEncoder) - .setPID(pidGains[0], pidGains[1], pidGains[2], PID_SLOT) - .setIZone(pidGains[3], PID_SLOT) - .apply(sparkMax, pidController); - - new RelativeEncoderConfig() - .setPositionConversionFactor(1.0) - .setVelocityConversionFactor(1.0 / 60.0) - .setMeasurementPeriod(isMoveMotor ? 16 : 32) - .setAverageDepth(isMoveMotor ? 2 : 8) - .apply(sparkMax, relativeEncoder); + .inverted(inverted); + + sparkConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(pidGains[0], pidGains[1], pidGains[2], pidSlot) + .iZone(pidGains[3], pidSlot); + + sparkConfig.encoder + .positionConversionFactor(1.0) + .velocityConversionFactor(1.0 / 60.0) + .uvwMeasurementPeriod(isMoveMotor ? 16 : 32) + .uvwAverageDepth(isMoveMotor ? 2 : 8); + + sparkConfig.signals + .status0Period((int) (config.period * 1000.0)) + .status1Period((int) (config.odometryPeriod * 1000.0)) + .status2Period((int) (config.odometryPeriod * 1000.0)) + .status3Enabled(false) + .status4Enabled(false) + .status5Enabled(false) + .status6Enabled(false) + .status7Enabled(false); + + RevUtil.run("Configure", sparkMax, () -> + sparkMax.configure(sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters) + ); return new SwerveMotor() { @Override @@ -161,13 +167,7 @@ public double getPosition() { @Override public void setPosition(double position) { - pidController.setReference( - position, - CANSparkBase.ControlType.kPosition, - PID_SLOT, - 0.0, - ArbFFUnits.kVoltage - ); + pid.setReference(position, ControlType.kPosition, pidSlot.value, 0.0, ArbFFUnits.kVoltage); } @Override @@ -177,10 +177,10 @@ public double getVelocity() { @Override public void setVelocity(double velocity) { - pidController.setReference( + pid.setReference( velocity, - CANSparkBase.ControlType.kVelocity, - PID_SLOT, + ControlType.kVelocity, + pidSlot.value, ffGains[0] * Math.signum(velocity) + ffGains[1] * velocity, ArbFFUnits.kVoltage ); @@ -199,10 +199,10 @@ public void reapplyGains() { } double[] pidGains = isMoveMotor ? config.movePID : config.turnPID; - pidController.setP(pidGains[0], PID_SLOT); - pidController.setI(pidGains[1], PID_SLOT); - pidController.setD(pidGains[2], PID_SLOT); - pidController.setIZone(pidGains[3], PID_SLOT); + SparkShim.setP(sparkMax, pidSlot, pidGains[0]); + SparkShim.setI(sparkMax, pidSlot, pidGains[1]); + SparkShim.setD(sparkMax, pidSlot, pidGains[2]); + SparkShim.setIZone(sparkMax, pidSlot, pidGains[3]); } @Override @@ -237,44 +237,46 @@ public void close() { public static SwerveMotor.Ctor sparkFlex(int id, MotorType type, boolean inverted) { return (config, isMoveMotor) -> { var deviceLogger = new SparkFlexLogger(); - CANSparkFlex sparkFlex = new CANSparkFlex(id, type); + SparkFlex sparkFlex = new SparkFlex(id, type); RelativeEncoder relativeEncoder = sparkFlex.getEncoder(); - SparkPIDController pidController = sparkFlex.getPIDController(); - int PID_SLOT = 0; + SparkClosedLoopController pid = sparkFlex.getClosedLoopController(); + ClosedLoopSlot pidSlot = ClosedLoopSlot.kSlot0; double[] pidGains = isMoveMotor ? config.movePID : config.turnPID; double[] ffGains = isMoveMotor ? config.moveFF : new double[] { 0.0, 0.0 }; - new SparkFlexConfig() - .clearFaults() - .enableVoltageCompensation(config.voltage) - .setSmartCurrentLimit((int) (isMoveMotor ? config.moveCurrentLimit : config.turnCurrentLimit)) - .setIdleMode( + var sparkConfig = new SparkFlexConfig() + .voltageCompensation(config.voltage) + .smartCurrentLimit((int) (isMoveMotor ? config.moveCurrentLimit : config.turnCurrentLimit)) + .idleMode( (isMoveMotor ? config.moveBrakeMode : config.turnBrakeMode) ? IdleMode.kBrake : IdleMode.kCoast ) - .setInverted(inverted) - .setPeriodicFramePeriod(SparkFlexConfig.Frame.S0, (int) (config.period * 1000.0)) - .setPeriodicFramePeriod(SparkFlexConfig.Frame.S1, (int) (config.odometryPeriod * 1000.0)) - .setPeriodicFramePeriod(SparkFlexConfig.Frame.S2, (int) (config.odometryPeriod * 1000.0)) - .setPeriodicFramePeriod(SparkFlexConfig.Frame.S3, 10000) - .setPeriodicFramePeriod(SparkFlexConfig.Frame.S4, 10000) - .setPeriodicFramePeriod(SparkFlexConfig.Frame.S5, 10000) - .setPeriodicFramePeriod(SparkFlexConfig.Frame.S6, 10000) - .setPeriodicFramePeriod(SparkFlexConfig.Frame.S7, 10000) - .apply(sparkFlex); - - new SparkPIDControllerConfig() - .setFeedbackDevice(relativeEncoder) - .setPID(pidGains[0], pidGains[1], pidGains[2], PID_SLOT) - .setIZone(pidGains[3], PID_SLOT) - .apply(sparkFlex, pidController); - - new RelativeEncoderConfig() - .setPositionConversionFactor(1.0) - .setVelocityConversionFactor(1.0 / 60.0) - .setMeasurementPeriod(isMoveMotor ? 32 : 100) - .setAverageDepth(isMoveMotor ? 8 : 64) - .apply(sparkFlex, relativeEncoder); + .inverted(inverted); + + sparkConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(pidGains[0], pidGains[1], pidGains[2], pidSlot) + .iZone(pidGains[3], pidSlot); + + sparkConfig.encoder + .positionConversionFactor(1.0) + .velocityConversionFactor(1.0 / 60.0) + .quadratureMeasurementPeriod(isMoveMotor ? 32 : 100) + .quadratureAverageDepth(isMoveMotor ? 8 : 64); + + sparkConfig.signals + .status0Period((int) (config.period * 1000.0)) + .status1Period((int) (config.odometryPeriod * 1000.0)) + .status2Period((int) (config.odometryPeriod * 1000.0)) + .status3Enabled(false) + .status4Enabled(false) + .status5Enabled(false) + .status6Enabled(false) + .status7Enabled(false); + + RevUtil.run("Configure", sparkFlex, () -> + sparkFlex.configure(sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters) + ); return new SwerveMotor() { @Override @@ -284,13 +286,7 @@ public double getPosition() { @Override public void setPosition(double position) { - pidController.setReference( - position, - CANSparkBase.ControlType.kPosition, - PID_SLOT, - 0.0, - ArbFFUnits.kVoltage - ); + pid.setReference(position, ControlType.kPosition, pidSlot.value, 0.0, ArbFFUnits.kVoltage); } @Override @@ -300,10 +296,10 @@ public double getVelocity() { @Override public void setVelocity(double velocity) { - pidController.setReference( + pid.setReference( velocity, - CANSparkBase.ControlType.kVelocity, - PID_SLOT, + ControlType.kVelocity, + pidSlot.value, ffGains[0] * Math.signum(velocity) + ffGains[1] * velocity, ArbFFUnits.kVoltage ); @@ -322,10 +318,10 @@ public void reapplyGains() { } double[] pidGains = isMoveMotor ? config.movePID : config.turnPID; - pidController.setP(pidGains[0], PID_SLOT); - pidController.setI(pidGains[1], PID_SLOT); - pidController.setD(pidGains[2], PID_SLOT); - pidController.setIZone(pidGains[3], PID_SLOT); + SparkShim.setP(sparkFlex, pidSlot, pidGains[0]); + SparkShim.setI(sparkFlex, pidSlot, pidGains[1]); + SparkShim.setD(sparkFlex, pidSlot, pidGains[2]); + SparkShim.setIZone(sparkFlex, pidSlot, pidGains[3]); } @Override @@ -360,7 +356,6 @@ public static SwerveMotor.Ctor talonFX(int id, boolean inverted) { return (config, isMoveMotor) -> { var deviceLogger = new TalonFXLogger(); TalonFX talonFX = new TalonFX(id, config.phoenixCanBus); - int PID_SLOT = 0; StatusSignal position = talonFX.getPosition().clone(); StatusSignal velocity = talonFX.getVelocity().clone(); @@ -369,13 +364,11 @@ public static SwerveMotor.Ctor talonFX(int id, boolean inverted) { boolean timesync = config.phoenixPro && config.phoenixCanBus.isNetworkFD(); PositionVoltage positionControl = new PositionVoltage(0.0); - positionControl.Slot = PID_SLOT; positionControl.EnableFOC = enableFOC; positionControl.UseTimesync = timesync; positionControl.UpdateFreqHz = 0.0; VelocityVoltage velocityControl = new VelocityVoltage(0.0); - velocityControl.Slot = PID_SLOT; velocityControl.EnableFOC = enableFOC; velocityControl.UseTimesync = timesync; velocityControl.UpdateFreqHz = 0.0; @@ -407,10 +400,14 @@ public static SwerveMotor.Ctor talonFX(int id, boolean inverted) { talonConfig.Slot0.kS = ffGains[0]; talonConfig.Slot0.kV = ffGains[1]; - PhoenixUtil.run(talonFX, "Apply TalonFXConfiguration", () -> talonFX.getConfigurator().apply(talonConfig)); - - BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.odometryPeriod, position, velocity); - talonFX.optimizeBusUtilization(1.0 / SwerveBaseHardware.TELEMETRY_CAN_PERIOD, 0.05); + PhoenixUtil.run("Clear Sticky Faults", talonFX, () -> talonFX.clearStickyFaults()); + PhoenixUtil.run("Apply TalonFXConfiguration", talonFX, () -> talonFX.getConfigurator().apply(talonConfig)); + PhoenixUtil.run("Set Update Frequency", talonFX, () -> + BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.odometryPeriod, position, velocity) + ); + PhoenixUtil.run("Optimize Bus Utilization", talonFX, () -> + talonFX.optimizeBusUtilization(1.0 / SwerveBaseHardware.kTelemetryCANPeriod, 0.05) + ); return new SwerveMotor() { @Override diff --git a/src/main/java/org/team340/lib/util/Alliance.java b/src/main/java/org/team340/lib/util/Alliance.java index 870e06f..e394938 100644 --- a/src/main/java/org/team340/lib/util/Alliance.java +++ b/src/main/java/org/team340/lib/util/Alliance.java @@ -3,8 +3,8 @@ import edu.wpi.first.wpilibj.DriverStation; /** - * Utility class for getting the robot's alliance. - * Can optionally be overridden using {@link Alliance#enableOverride(boolean)}. + * Shorthand for getting the robot's alliance. + * Defaults to the blue alliance if the current alliance is unknown. */ public final class Alliance { @@ -12,34 +12,12 @@ private Alliance() { throw new AssertionError("This is a utility class!"); } - private static boolean overrideActive = false; - private static boolean overrideIsBlue = false; - - /** - * Overrides FMS alliance data in favor of a user-set value when - * using {@link Alliance#isBlue()} and {@link Alliance#isRed()}. - * @param isBlue The value to override with. - */ - public static void enableOverride(boolean isBlue) { - overrideActive = true; - overrideIsBlue = isBlue; - } - - /** - * Disables the override if active. - */ - public static void disableOverride() { - overrideActive = false; - } - /** * Returns {@code true} if the robot is on the blue alliance. * If the robot's alliance is unknown, defaults to {@code true} (blue). */ public static boolean isBlue() { - return overrideActive - ? overrideIsBlue - : DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue).equals(DriverStation.Alliance.Blue); + return DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue).equals(DriverStation.Alliance.Blue); } /** diff --git a/src/main/java/org/team340/lib/util/CommandBuilder.java b/src/main/java/org/team340/lib/util/CommandBuilder.java index 41ee455..9f8ac26 100644 --- a/src/main/java/org/team340/lib/util/CommandBuilder.java +++ b/src/main/java/org/team340/lib/util/CommandBuilder.java @@ -1,12 +1,12 @@ package org.team340.lib.util; +import edu.wpi.first.util.function.BooleanConsumer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.Subsystem; import java.util.ConcurrentModificationException; -import java.util.function.Consumer; -import java.util.function.Supplier; +import java.util.function.BooleanSupplier; /** * A command builder. Very similar to {@link FunctionalCommand}. @@ -16,8 +16,8 @@ public class CommandBuilder extends Command { private Runnable onInitialize = () -> {}; private Runnable onExecute = () -> {}; - private Consumer onEnd = interrupted -> {}; - private Supplier isFinished = () -> false; + private BooleanConsumer onEnd = interrupted -> {}; + private BooleanSupplier isFinished = () -> false; /** * Create the command builder. @@ -71,7 +71,7 @@ public CommandBuilder onEnd(Runnable onEnd) { * The action to take when the command ends. Called when either the command finishes normally, * or when it interrupted/canceled. Supplied boolean is if the command was interrupted. */ - public CommandBuilder onEnd(Consumer onEnd) { + public CommandBuilder onEnd(BooleanConsumer onEnd) { if (this.isScheduled()) throw new ConcurrentModificationException( "Cannot change methods of a command while it is scheduled" ); @@ -93,7 +93,7 @@ public CommandBuilder isFinished(boolean isFinished) { * Whether the command has finished. Once a command finishes, the scheduler will call its end() * method and un-schedule it. By default, this returns {@code false}. */ - public CommandBuilder isFinished(Supplier isFinished) { + public CommandBuilder isFinished(BooleanSupplier isFinished) { if (this.isScheduled()) throw new ConcurrentModificationException( "Cannot change methods of a command while it is scheduled" ); @@ -118,6 +118,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return isFinished.get(); + return isFinished.getAsBoolean(); } } diff --git a/src/main/java/org/team340/lib/util/DummySubsystem.java b/src/main/java/org/team340/lib/util/DummySubsystem.java index f41f30f..d27a4ba 100644 --- a/src/main/java/org/team340/lib/util/DummySubsystem.java +++ b/src/main/java/org/team340/lib/util/DummySubsystem.java @@ -5,4 +5,4 @@ /** * A dummy subsystem. Intended to be used as a secondary mutex within the command scheduler. */ -public class DummySubsystem implements Subsystem {} +public final class DummySubsystem implements Subsystem {} diff --git a/src/main/java/org/team340/lib/util/GRRSubsystem.java b/src/main/java/org/team340/lib/util/GRRSubsystem.java index 346a30c..37cecae 100644 --- a/src/main/java/org/team340/lib/util/GRRSubsystem.java +++ b/src/main/java/org/team340/lib/util/GRRSubsystem.java @@ -2,6 +2,9 @@ import edu.wpi.first.wpilibj2.command.Subsystem; +/** + * A {@link Subsystem} implementation. + */ public abstract class GRRSubsystem implements Subsystem { public GRRSubsystem() { @@ -11,7 +14,7 @@ public GRRSubsystem() { /** * Creates a command builder that requires this subsystem. */ - protected CommandBuilder commandBuilder() { + public CommandBuilder commandBuilder() { return new CommandBuilder(this); } @@ -19,7 +22,7 @@ protected CommandBuilder commandBuilder() { * Creates a command builder that requires this subsystem. * @param name The name of the command. */ - protected CommandBuilder commandBuilder(String name) { + public CommandBuilder commandBuilder(String name) { return new CommandBuilder(name, this); } } diff --git a/src/main/java/org/team340/lib/util/Math2.java b/src/main/java/org/team340/lib/util/Math2.java index e0235ae..840bac8 100644 --- a/src/main/java/org/team340/lib/util/Math2.java +++ b/src/main/java/org/team340/lib/util/Math2.java @@ -1,8 +1,5 @@ package org.team340.lib.util; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -20,23 +17,23 @@ private Math2() { } /** Shared maximum accuracy floating point. */ - public static final double EPSILON = 1e-8; + public static final double kEpsilon = 1e-8; /** {@code PI/6} (30deg) */ - public static final double SIXTH_PI = Math.PI / 6.0; + public static final double kSixthPi = Math.PI / 6.0; /** {@code PI/4} (45deg) */ - public static final double QUARTER_PI = Math.PI / 4.0; + public static final double kQuarterPi = Math.PI / 4.0; /** {@code PI/3} (60deg) */ - public static final double THIRD_PI = Math.PI / 3.0; + public static final double kThirdPi = Math.PI / 3.0; /** {@code PI/2} (90deg) */ - public static final double HALF_PI = Math.PI / 2.0; + public static final double kHalfPi = Math.PI / 2.0; /** {@code 2PI/3} (120deg) */ - public static final double TWO_THIRDS_PI = (2.0 * Math.PI) / 3.0; + public static final double kTwoThirdsPi = (2.0 * Math.PI) / 3.0; /** {@code 3PI/4} (135deg) */ - public static final double THREE_QUARTERS_PI = (3.0 * Math.PI) / 4.0; + public static final double kThreeQuartersPi = (3.0 * Math.PI) / 4.0; /** {@code 5PI/6} (150deg) */ - public static final double FIVE_SIXTHS_PI = (5.0 * Math.PI) / 6.0; + public static final double kFiveSixthsPi = (5.0 * Math.PI) / 6.0; /** {@code PI*2} (360deg) */ - public static final double TWO_PI = Math.PI * 2.0; + public static final double kTwoPi = Math.PI * 2.0; /** * Returns a random double from {@code 0.0} to {@code max}. @@ -65,7 +62,7 @@ public static double random(double min, double max) { * @return {@code true} if the values are equal. */ public static boolean epsilonEquals(Optional a, Optional b) { - return epsilonEquals(a, b, EPSILON); + return epsilonEquals(a, b, kEpsilon); } /** @@ -91,7 +88,7 @@ public static boolean epsilonEquals(Optional a, Optional b, doub * @return {@code true} if the values are equal. */ public static boolean epsilonEquals(double a, double b) { - return epsilonEquals(a, b, EPSILON); + return epsilonEquals(a, b, kEpsilon); } /** @@ -105,102 +102,6 @@ public static boolean epsilonEquals(double a, double b, double epsilon) { return (a - epsilon <= b) && (a + epsilon >= b); } - /** - * Checks if two {@link Translation2d}s are equal within the accuracy of the default epsilon. - * @param a The first value to compare. - * @param b The second value to compare. - * @return {@code true} if the values are equal. - */ - public static boolean epsilonEquals(Translation2d a, Translation2d b) { - return epsilonEquals(a, b, EPSILON); - } - - /** - * Checks if two {@link Translation2d}s are equal within the accuracy of a provided epsilon. - * @param a The first value to compare. - * @param b The second value to compare. - * @param epsilon Epsilon value to compare with. - * @return {@code true} if the values are equal. - */ - public static boolean epsilonEquals(Translation2d a, Translation2d b, double epsilon) { - return (epsilonEquals(a.getX(), b.getX(), epsilon) && epsilonEquals(a.getY(), b.getY(), epsilon)); - } - - /** - * Checks if two {@link Transform2d}s are equal within the accuracy of the default epsilon. - * @param a The first value to compare. - * @param b The second value to compare. - * @return {@code true} if the values are equal. - */ - public static boolean epsilonEquals(Transform2d a, Transform2d b) { - return epsilonEquals(a, b, EPSILON); - } - - /** - * Checks if two {@link Transform2d}s are equal within the accuracy of a provided epsilon. - * @param a The first value to compare. - * @param b The second value to compare. - * @param epsilon Epsilon value to compare with. - * @return {@code true} if the values are equal. - */ - public static boolean epsilonEquals(Transform2d a, Transform2d b, double epsilon) { - return ( - epsilonEquals(a.getX(), b.getX(), epsilon) && - epsilonEquals(a.getY(), b.getY(), epsilon) && - epsilonEquals(a.getRotation().getRadians(), b.getRotation().getRadians(), epsilon) - ); - } - - /** - * Checks if two {@link Pose2d}s are equal within the accuracy of the default epsilon. - * @param a The first value to compare. - * @param b The second value to compare. - * @return {@code true} if the values are equal. - */ - public static boolean epsilonEquals(Pose2d a, Pose2d b) { - return epsilonEquals(a, b, EPSILON); - } - - /** - * Checks if two {@link Pose2d}s are equal within the accuracy of a provided epsilon. - * @param a The first value to compare. - * @param b The second value to compare. - * @param epsilon Epsilon value to compare with. - * @return {@code true} if the values are equal. - */ - public static boolean epsilonEquals(Pose2d a, Pose2d b, double epsilon) { - return ( - epsilonEquals(a.getX(), b.getX(), epsilon) && - epsilonEquals(a.getY(), b.getY(), epsilon) && - epsilonEquals(a.getRotation().getRadians(), b.getRotation().getRadians(), epsilon) - ); - } - - /** - * Checks if two {@link Twist2d}s are equal within the accuracy of the default epsilon. - * @param a The first value to compare. - * @param b The second value to compare. - * @return {@code true} if the values are equal. - */ - public static boolean epsilonEquals(Twist2d a, Twist2d b) { - return epsilonEquals(a, b, EPSILON); - } - - /** - * Checks if two {@link Twist2d}s are equal within the accuracy of a provided epsilon. - * @param a The first value to compare. - * @param b The second value to compare. - * @param epsilon Epsilon value to compare with. - * @return {@code true} if the values are equal. - */ - public static boolean epsilonEquals(Twist2d a, Twist2d b, double epsilon) { - return ( - epsilonEquals(a.dx, b.dx, epsilon) && - epsilonEquals(a.dy, b.dy, epsilon) && - epsilonEquals(a.dtheta, b.dtheta, epsilon) - ); - } - /** * Copies values from a source {@link Twist2d} object to another. * @param source The twist to copy from. @@ -252,11 +153,100 @@ public static SwerveModuleState copyInto(SwerveModuleState source, SwerveModuleS } /** - * Formats radians to be human readable by converting - * to degrees and rounding to 2 decimal points. - * @param radians The radians to format. - */ - public String formatRadians(double radians) { - return String.format("%.2f", Math.toDegrees(radians)); + * Zeroes a {@link ChassisSpeeds} object in place. + * @param speeds The speeds to zero. + * @return The provided speeds object. + */ + public static ChassisSpeeds zero(ChassisSpeeds speeds) { + speeds.vxMetersPerSecond = 0.0; + speeds.vyMetersPerSecond = 0.0; + speeds.omegaRadiansPerSecond = 0.0; + return speeds; + } + + /** + * Zeroes a {@link Twist2d} object in place. + * @param twist The twist to zero. + * @return The provided twist object. + */ + public static Twist2d zero(Twist2d twist) { + twist.dx = 0.0; + twist.dy = 0.0; + twist.dtheta = 0.0; + return twist; + } + + /** + * {@return the floating-point binary16 value, encoded in a {@code + * short}, closest in value to the argument} + * The conversion is computed under the {@linkplain + * java.math.RoundingMode#HALF_EVEN round to nearest even rounding + * mode}. + * + * Special cases: + *

+ * + * @apiNote + * This method corresponds to the convertFormat operation defined + * in IEEE 754 from the binary32 format to the binary16 format. + * The operation of this method is analogous to a primitive + * narrowing conversion (JLS {@jls 5.1.3}). + * + * @param f the {@code float} value to convert to binary16 + * @since 20 + */ + public static short floatToFloat16(float f) { + int doppel = Float.floatToRawIntBits(f); + short sign_bit = (short) ((doppel & 0x8000_0000) >> 16); + + if (Float.isNaN(f)) { + return (short) (sign_bit | + 0x7c00 | + ((doppel & 0x007f_e000) >> 13) | + ((doppel & 0x0000_1ff0) >> 4) | + (doppel & 0x0000_000f)); + } + + float abs_f = Math.abs(f); + + if (abs_f >= (0x1.ffcp15f + 0x0.002p15f)) { + return (short) (sign_bit | 0x7c00); + } + + if (abs_f <= 0x1.0p-24f * 0.5f) { + return sign_bit; + } + + int exp = Math.getExponent(f); + assert -25 <= exp && exp <= 15; + + int expdelta = 0; + int msb = 0x0000_0000; + if (exp < -14) { + expdelta = -14 - exp; + exp = -15; + msb = 0x0080_0000; + } + int f_signif_bits = (doppel & 0x007f_ffff) | msb; + + short signif_bits = (short) (f_signif_bits >> (13 + expdelta)); + + int lsb = f_signif_bits & (1 << (13 + expdelta)); + int round = f_signif_bits & (1 << (12 + expdelta)); + int sticky = f_signif_bits & ((1 << (12 + expdelta)) - 1); + + if (round != 0 && ((lsb | sticky) != 0)) { + signif_bits++; + } + + assert (0xf800 & signif_bits) == 0x0; + + return (short) (sign_bit | (((exp + 15) << 10) + signif_bits)); } } diff --git a/src/main/java/org/team340/lib/util/Mutable.java b/src/main/java/org/team340/lib/util/Mutable.java index cca1a18..e617aae 100644 --- a/src/main/java/org/team340/lib/util/Mutable.java +++ b/src/main/java/org/team340/lib/util/Mutable.java @@ -1,5 +1,7 @@ package org.team340.lib.util; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; import java.util.function.Consumer; import java.util.function.Supplier; @@ -16,6 +18,7 @@ * to provide stateful behavior does not suffer from aforementioned race * conditions as commands are invoked synchronously. */ +@Logged(strategy = Strategy.OPT_IN) public class Mutable implements Supplier, Consumer { public T value; diff --git a/src/main/java/org/team340/lib/util/ctre/PhoenixUtil.java b/src/main/java/org/team340/lib/util/PhoenixUtil.java similarity index 89% rename from src/main/java/org/team340/lib/util/ctre/PhoenixUtil.java rename to src/main/java/org/team340/lib/util/PhoenixUtil.java index 28b27b1..85cf829 100644 --- a/src/main/java/org/team340/lib/util/ctre/PhoenixUtil.java +++ b/src/main/java/org/team340/lib/util/PhoenixUtil.java @@ -1,4 +1,4 @@ -package org.team340.lib.util.ctre; +package org.team340.lib.util; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.hardware.ParentDevice; @@ -17,24 +17,24 @@ private PhoenixUtil() { /** * Runs a Phoenix API call and checks for errors. Will * try up to 3 times if the target API call fails. - * @param device The device the call is relevant to. * @param name The name of the API call. + * @param device The device the call is relevant to. * @param target The target call to run. * @return {@code true} if success ({@link StatusCode#isOK()}), {@code false} otherwise. */ - public static boolean run(ParentDevice device, String name, Supplier target) { - return run(device, name, target, 3); + public static boolean run(String name, ParentDevice device, Supplier target) { + return run(name, device, target, 3); } /** * Runs a Phoenix API call and checks for errors. - * @param device The device the call is relevant to. * @param name The name of the API call. + * @param device The device the call is relevant to. * @param target The target call to run. * @param maxTries The number of times to try the call before failing. {@code 1} only runs the call once. * @return {@code true} if success ({@link StatusCode#isOK()}), {@code false} otherwise. */ - public static boolean run(ParentDevice device, String name, Supplier target, int maxTries) { + public static boolean run(String name, ParentDevice device, Supplier target, int maxTries) { String results = ""; for (int i = 0; i < maxTries; i++) { StatusCode result = target.get(); diff --git a/src/main/java/org/team340/lib/util/redux/ReduxUtil.java b/src/main/java/org/team340/lib/util/ReduxUtil.java similarity index 97% rename from src/main/java/org/team340/lib/util/redux/ReduxUtil.java rename to src/main/java/org/team340/lib/util/ReduxUtil.java index 5bba3a6..16b4a5d 100644 --- a/src/main/java/org/team340/lib/util/redux/ReduxUtil.java +++ b/src/main/java/org/team340/lib/util/ReduxUtil.java @@ -1,4 +1,4 @@ -package org.team340.lib.util.redux; +package org.team340.lib.util; import com.reduxrobotics.frames.DoubleFrame; import com.reduxrobotics.frames.Frame; @@ -11,6 +11,9 @@ import edu.wpi.first.wpilibj.Timer; import java.util.Map; +/** + * Utilities for ReduxLib. + */ public final class ReduxUtil { private ReduxUtil() { @@ -94,11 +97,14 @@ public static boolean applySettings(Canandgyro canandgyro, Canandgyro.Settings s boolean failed = false; Map device = canandgyro.getSettings(0.5).getFilteredMap(); for (var entry : settings.getFilteredMap().entrySet()) { - if (device.get(entry.getKey()) != entry.getValue()) failed = true; + if (device.get(entry.getKey()) != entry.getValue()) { + results += (results.isEmpty() ? "" : ", ") + "Failed check"; + failed = true; + break; + } } if (!failed) return true; - results += (results.isEmpty() ? "" : ", ") + "Failed check"; } DriverStation.reportError( diff --git a/src/main/java/org/team340/lib/util/RevUtil.java b/src/main/java/org/team340/lib/util/RevUtil.java new file mode 100644 index 0000000..289b7e5 --- /dev/null +++ b/src/main/java/org/team340/lib/util/RevUtil.java @@ -0,0 +1,59 @@ +package org.team340.lib.util; + +import com.ctre.phoenix6.StatusCode; +import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkLowLevel; +import edu.wpi.first.wpilibj.DriverStation; +import java.util.function.Supplier; + +/** + * Utilities for REVLib. + */ +public final class RevUtil { + + private RevUtil() { + throw new AssertionError("This is a utility class!"); + } + + /** + * Runs a REVLib API call and checks for errors. Will + * try up to 3 times if the target API call fails. + * @param name The name of the API call. + * @param device The device the call is relevant to. + * @param target The target call to run. + * @return {@code true} if success ({@link REVLibError#kOk}), {@code false} otherwise. + */ + public static boolean run(String name, SparkLowLevel device, Supplier target) { + return run(name, device, target, 3); + } + + /** + * Runs a REVLib API call and checks for errors. + * @param name The name of the API call. + * @param device The device the call is relevant to. + * @param target The target call to run. + * @param maxTries The number of times to try the call before failing. {@code 1} only runs the call once. + * @return {@code true} if success ({@link StatusCode#isOK()}), {@code false} otherwise. + */ + public static boolean run(String name, SparkLowLevel device, Supplier target, int maxTries) { + String results = ""; + for (int i = 0; i < maxTries; i++) { + REVLibError result = target.get(); + if (result.equals(REVLibError.kOk)) return true; + results += (results.isEmpty() ? "" : ", ") + result.name(); + } + + DriverStation.reportError( + "[RevUtil] " + + device.getClass().getSimpleName() + + " (ID " + + device.getDeviceId() + + ") \"" + + name + + "\": " + + results, + false + ); + return false; + } +} diff --git a/src/main/java/org/team340/lib/util/rev/RelativeEncoderConfig.java b/src/main/java/org/team340/lib/util/rev/RelativeEncoderConfig.java deleted file mode 100644 index 513ad8f..0000000 --- a/src/main/java/org/team340/lib/util/rev/RelativeEncoderConfig.java +++ /dev/null @@ -1,117 +0,0 @@ -package org.team340.lib.util.rev; - -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import org.team340.lib.util.Math2; - -/** - * Config builder for {@link RelativeEncoder}. - */ -public class RelativeEncoderConfig extends RevConfigBase { - - /** - * Clones this config. - */ - public RelativeEncoderConfig clone() { - var config = new RelativeEncoderConfig(); - config.configSteps.addAll(configSteps); - return config; - } - - /** - * Applies the config to a Spark Max attached encoder. Note that this is a blocking operation. - * Errors are printed when calling {@link RevConfigRegistry#burnFlashAll()}. - * @param sparkMax The Spark Max the encoder is attached to. - * @param relativeEncoder The relative encoder. - */ - public void apply(CANSparkMax sparkMax, RelativeEncoder relativeEncoder) { - applySteps(relativeEncoder, "Spark Max (ID " + sparkMax.getDeviceId() + ") Relative Encoder"); - } - - /** - * Applies the config to a Spark Flex attached encoder. Note that this is a blocking operation. - * Errors are printed when calling {@link RevConfigRegistry#burnFlashAll()}. - * @param sparkFlex The Spark Flex the encoder is attached to. - * @param relativeEncoder The relative encoder. - */ - public void apply(CANSparkFlex sparkFlex, RelativeEncoder relativeEncoder) { - applySteps(relativeEncoder, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") Relative Encoder"); - } - - /** - * Sets the sampling depth of the velocity calculation process for a quadrature or hall sensor - * encoder. This value sets the number of samples in the average for velocity readings. For a - * quadrature encoder, this can be any value from {@code 1} to {@code 64} (default). For a hall - * sensor, it must be either {@code 1}, {@code 2}, {@code 4}, or {@code 8} (default). - * @param depth The velocity calculation process's sampling depth. - */ - public RelativeEncoderConfig setAverageDepth(int depth) { - addStep( - relativeEncoder -> relativeEncoder.setAverageDepth(depth), - relativeEncoder -> relativeEncoder.getAverageDepth() == depth, - "Average Depth" - ); - return this; - } - - /** - * Sets the phase of the motor feedback sensor so that it is set to be in phase with the motor itself. - * This only works for quadrature encoders and analog sensors. This will throw an error if the user - * tries to set the inversion of the hall sensor. - * @param inverted The phase of the sensor. - */ - public RelativeEncoderConfig setInverted(boolean inverted) { - addStep( - relativeEncoder -> relativeEncoder.setInverted(inverted), - relativeEncoder -> relativeEncoder.getInverted() == inverted, - "Inverted" - ); - return this; - } - - /** - * Sets the position measurement period used to calculate the velocity of a quadrature or hall sensor - * encoder. For a quadrature encoder, this number may be between {@code 1} and {@code 100} (default). For - * a hall sensor, this number may be between {@code 8} and {@code 64}. The default for a hall sensor is 32ms. - * @param periodMs Measurement period in milliseconds. - */ - public RelativeEncoderConfig setMeasurementPeriod(int periodMs) { - addStep( - relativeEncoder -> relativeEncoder.setMeasurementPeriod(periodMs), - relativeEncoder -> relativeEncoder.getMeasurementPeriod() == periodMs, - "Measurement Period" - ); - return this; - } - - /** - * Sets the conversion factor for position of the encoder. Multiplied - * by the native output units to give you position. - * @param factor The conversion factor to multiply the native units by. - */ - public RelativeEncoderConfig setPositionConversionFactor(double factor) { - addStep( - relativeEncoder -> relativeEncoder.setPositionConversionFactor(factor), - relativeEncoder -> - Math2.epsilonEquals(relativeEncoder.getPositionConversionFactor(), factor, RevConfigRegistry.EPSILON), - "Position Conversion Factor" - ); - return this; - } - - /** - * Sets the conversion factor for velocity of the encoder. Multiplied - * by the native output units to give you velocity. - * @param factor The conversion factor to multiply the native units by. - */ - public RelativeEncoderConfig setVelocityConversionFactor(double factor) { - addStep( - relativeEncoder -> relativeEncoder.setVelocityConversionFactor(factor), - relativeEncoder -> - Math2.epsilonEquals(relativeEncoder.getVelocityConversionFactor(), factor, RevConfigRegistry.EPSILON), - "Velocity Conversion Factor" - ); - return this; - } -} diff --git a/src/main/java/org/team340/lib/util/rev/RevConfigBase.java b/src/main/java/org/team340/lib/util/rev/RevConfigBase.java deleted file mode 100644 index 64a54f5..0000000 --- a/src/main/java/org/team340/lib/util/rev/RevConfigBase.java +++ /dev/null @@ -1,76 +0,0 @@ -package org.team340.lib.util.rev; - -import com.revrobotics.REVLibError; -import edu.wpi.first.wpilibj.RobotBase; -import java.util.ArrayList; -import java.util.List; -import java.util.function.Function; -import org.team340.lib.util.Sleep; - -/** - * Base class for configuring REV hardware. - */ -abstract class RevConfigBase { - - private static final double CHECK_PERIOD = 0.015; - - private static final record RevConfigStep( - Function applier, - Function checker, - boolean trustCheck, - String name - ) {} - - final List> configSteps = new ArrayList<>(); - - /** - * Stores a configuration step. - */ - void addStep(Function applier, String name) { - configSteps.add(new RevConfigStep<>(applier, device -> true, false, name)); - } - - /** - * Stores a configuration step. - */ - void addStep(Function applier, Function checker, String name) { - configSteps.add(new RevConfigStep<>(applier, checker, true, name)); - } - - /** - * Stores a configuration step. - */ - void addStep(Function applier, Function checker, boolean trustCheck, String name) { - configSteps.add(new RevConfigStep(applier, checker, trustCheck, name)); - } - - /** - * Applies the config. Note that this is a blocking operation. Errors - * are printed when calling {@link RevConfigRegistry#burnFlashAll()}. - * @param device The device to apply the config to. - * @param identifier A string used to identify the device in logs. - */ - void applySteps(T device, String identifier) { - for (RevConfigStep step : configSteps) { - boolean ok = true; - boolean check = true; - String results = ""; - - for (int i = 0; i < 3; i++) { - REVLibError result = step.applier.apply(device); - Sleep.seconds(CHECK_PERIOD, true); - - ok = REVLibError.kOk.equals(result) || - (RobotBase.isSimulation() && result.equals(REVLibError.kParamMismatchType)); - check = step.checker().apply(device); - if (ok && check && step.trustCheck) break; - - results += (results.isEmpty() ? "" : ", ") + result.name() + (!check ? " (Failed Check)" : ""); - } - - if (!ok || !check) { - RevConfigRegistry.addError(identifier + " \"" + step.name() + "\": " + results); - } - } - } -} diff --git a/src/main/java/org/team340/lib/util/rev/RevConfigRegistry.java b/src/main/java/org/team340/lib/util/rev/RevConfigRegistry.java deleted file mode 100644 index 1c1b43f..0000000 --- a/src/main/java/org/team340/lib/util/rev/RevConfigRegistry.java +++ /dev/null @@ -1,178 +0,0 @@ -package org.team340.lib.util.rev; - -import com.revrobotics.REVLibError; -import edu.wpi.first.wpilibj.DriverStation; -import java.text.Collator; -import java.util.Collection; -import java.util.Comparator; -import java.util.LinkedHashMap; -import java.util.Map; -import java.util.TreeSet; -import java.util.concurrent.ConcurrentHashMap; -import java.util.function.Supplier; -import org.team340.lib.util.Sleep; - -/** - * Utilities for REV hardware configs. - */ -public final class RevConfigRegistry { - - private RevConfigRegistry() { - throw new AssertionError("This is a utility class!"); - } - - static final double EPSILON = 1e-3; - - private static final double BURN_FLASH_DELAY = 1.0; - private static final double BURN_FLASH_INTERVAL = 0.075; - - private static final Collection errors = new TreeSet<>(ErrorComparator.getInstance()); - private static final Map frameRefreshers = new ConcurrentHashMap<>(); - private static final Map> burnFlash = new LinkedHashMap<>(); - - private static Thread frameThread = null; - private static volatile boolean frameThreadActive = false; - - /** - * Saves a configuration error string to be logged. - * @param errorString The error string. - */ - static void addError(String errorString) { - errors.add(errorString); - } - - /** - * Saves a callback to be called periodically to refresh a device's CAN frame period setting. - * All refreshers should be associated with a unique identifier, that is relevant to the device - * and specific frame being refreshed, to ensure if a device has a frame configured twice only - * the last configured period is utilized while refreshing. - * @param identifier An identifier for the refresher. - * @param refresher A {@link Runnable} to invoke. - */ - static void addFrameRefresher(String identifier, Runnable refresher) { - frameRefreshers.put(identifier, refresher); - } - - /** - * Saves a callback to burn flash to REV hardware. - * @param identifier An readable identifier for the hardware. Must be unique. - * @param callback A callback that burns flash and returns the result. - */ - static void addBurnFlash(String identifier, Supplier callback) { - burnFlash.put(identifier, callback); - } - - /** - * Burns flash to all registered REV hardware. Additionally, any - * errors accumulated while previously configuring hardware will - * be printed to stdout. Note that this is a blocking operation. - */ - public static void burnFlashAll() { - Sleep.seconds(BURN_FLASH_DELAY, true); - - for (var entry : burnFlash.entrySet()) { - REVLibError result = entry.getValue().get(); - Sleep.seconds(BURN_FLASH_INTERVAL, true); - if (!result.equals(REVLibError.kOk)) addError(entry.getKey() + " \"Burn Flash\": " + result.name()); - } - - if (errors.isEmpty()) { - System.out.println("\n[RevConfigRegistry] All REV hardware configured successfully\n"); - } else { - String error = "[RevConfigRegistry] " + errors.size() + " errors while configuring REV hardware:"; - for (String e : errors) { - error += "\n\t" + e; - } - DriverStation.reportError(error, false); - errors.clear(); - } - } - - /** - * Enables refreshing device CAN frame period settings, in the case of a reset. - * Frames are refreshed sequentially, and delayed evenly across the default - * period of 10 seconds to prevent the CAN output buffer overflowing. - */ - public static void enableFrameRefreshing() { - enableFrameRefreshing(10.0); - } - - /** - * Enables refreshing device CAN frame period settings, in the case of a reset. - * Frames are refreshed sequentially, and delayed evenly across the specified - * period to prevent the CAN output buffer overflowing. - * @param period The period at which to refresh a single frame at in seconds. - */ - public static void enableFrameRefreshing(double period) { - if (!frameThreadActive) { - frameThread = new Thread(() -> { - while (frameThreadActive) { - if (frameRefreshers.size() == 0) { - Sleep.seconds(period); - continue; - } - - double delay = period / frameRefreshers.size(); - for (Runnable callback : frameRefreshers.values()) { - callback.run(); - Sleep.seconds(delay); - } - } - }); - - frameThread.setName("RevConfigRegistry"); - frameThread.setDaemon(true); - frameThreadActive = true; - frameThread.start(); - } - } - - /** - * Disables refreshing device CAN frame period - * settings, if previously enabled. - */ - public static void disableFrameRefreshing() { - if (frameThreadActive) { - frameThreadActive = false; - if (frameThread != null && frameThread.isAlive()) { - try { - frameThread.interrupt(); - frameThread.join(); - } catch (Exception e) { - Thread.currentThread().interrupt(); - } - } - } - } - - private static final class ErrorComparator implements Comparator { - - private static ErrorComparator instance; - private final Collator localeComparator = Collator.getInstance(); - - public static ErrorComparator getInstance() { - if (instance == null) instance = new ErrorComparator(); - return instance; - } - - private ErrorComparator() {} - - @Override - public int compare(String arg0, String arg1) { - int idDiff; - try { - idDiff = Integer.parseInt(arg0.split(" ")[1]) - Integer.parseInt(arg1.split(" ")[1]); - } catch (Exception e) { - idDiff = 0; - } - - if (idDiff == 0) { - int localeDiff = localeComparator.compare(arg0, arg1); - if (localeDiff == 0) return 1; - else return localeDiff; - } else { - return idDiff; - } - } - } -} diff --git a/src/main/java/org/team340/lib/util/rev/SparkAbsoluteEncoderConfig.java b/src/main/java/org/team340/lib/util/rev/SparkAbsoluteEncoderConfig.java deleted file mode 100644 index 3fed5f7..0000000 --- a/src/main/java/org/team340/lib/util/rev/SparkAbsoluteEncoderConfig.java +++ /dev/null @@ -1,116 +0,0 @@ -package org.team340.lib.util.rev; - -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkAbsoluteEncoder; -import org.team340.lib.util.Math2; - -/** - * Config builder for {@link SparkAbsoluteEncoder}. - */ -public class SparkAbsoluteEncoderConfig extends RevConfigBase { - - /** - * Clones this config. - */ - public SparkAbsoluteEncoderConfig clone() { - var config = new SparkAbsoluteEncoderConfig(); - config.configSteps.addAll(configSteps); - return config; - } - - /** - * Applies the config to a Spark Max attached encoder. Note that this is a blocking operation. - * Errors are printed when calling {@link RevConfigRegistry#burnFlashAll()}. - * @param sparkMax The Spark Max the encoder is attached to. - * @param absoluteEncoder The absolute encoder. - */ - public void apply(CANSparkMax sparkMax, SparkAbsoluteEncoder absoluteEncoder) { - applySteps(absoluteEncoder, "Spark Max (ID " + sparkMax.getDeviceId() + ") Absolute Encoder"); - } - - /** - * Applies the config to a Spark Flex attached encoder. Note that this is a blocking operation. - * Errors are printed when calling {@link RevConfigRegistry#burnFlashAll()}. - * @param sparkFlex The Spark Flex the encoder is attached to. - * @param absoluteEncoder The absolute encoder. - */ - public void apply(CANSparkFlex sparkFlex, SparkAbsoluteEncoder absoluteEncoder) { - applySteps(absoluteEncoder, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") Absolute Encoder"); - } - - /** - * Sets the average sampling depth for an absolute encoder. This is a bit size and should be - * either {@code 1}, {@code 2}, {@code 4}, {@code 8}, {@code 16}, {@code 32}, {@code 64}, or {@code 128}. - * @param depth The average sampling depth of {@code 1}, {@code 2}, {@code 4}, {@code 8}, {@code 16}, {@code 32}, {@code 64}, or {@code 128}. - */ - public SparkAbsoluteEncoderConfig setAverageDepth(int depth) { - addStep( - absoluteEncoder -> absoluteEncoder.setAverageDepth(depth), - absoluteEncoder -> absoluteEncoder.getAverageDepth() == depth, - "Average Depth" - ); - return this; - } - - /** - * Sets the phase of the absolute encoder so that it is set to be in phase with the motor itself. - * @param inverted The phase of the encoder. - */ - public SparkAbsoluteEncoderConfig setInverted(boolean inverted) { - addStep( - absoluteEncoder -> absoluteEncoder.setInverted(inverted), - absoluteEncoder -> absoluteEncoder.getInverted() == inverted, - "Inverted" - ); - return this; - } - - /** - * Sets the conversion factor for position of the encoder. Multiplied - * by the native output units to give you position. - * @param factor The conversion factor to multiply the native units (rotations) by. - */ - public SparkAbsoluteEncoderConfig setPositionConversionFactor(double factor) { - addStep( - absoluteEncoder -> absoluteEncoder.setPositionConversionFactor(factor), - absoluteEncoder -> - Math2.epsilonEquals(absoluteEncoder.getPositionConversionFactor(), factor, RevConfigRegistry.EPSILON), - "Position Conversion Factor" - ); - return this; - } - - /** - * Sets the conversion factor for velocity of the encoder. Multiplied - * by the native output units to give you velocity. - * @param factor The conversion factor to multiply the native units (rotations per minute) by. - */ - public SparkAbsoluteEncoderConfig setVelocityConversionFactor(double factor) { - addStep( - absoluteEncoder -> absoluteEncoder.setVelocityConversionFactor(factor), - absoluteEncoder -> - Math2.epsilonEquals(absoluteEncoder.getVelocityConversionFactor(), factor, RevConfigRegistry.EPSILON), - "Velocity Conversion Factor" - ); - return this; - } - - /** - * Sets the zero offset of an absolute encoder (the position that is reported as zero). - * The zero offset is specified as the reported position of the encoder in the desired - * zero position, if the zero offset was set to 0. It is influenced by the absolute - * encoder's position conversion factor, and whether it is inverted. Always call - * {@code setConversionFactor()} and {@code setInverted()} - * before calling this function. - * @param offset The zero offset with the position conversion factor applied. - */ - public SparkAbsoluteEncoderConfig setZeroOffset(double offset) { - addStep( - absoluteEncoder -> absoluteEncoder.setZeroOffset(offset), - absoluteEncoder -> Math2.epsilonEquals(absoluteEncoder.getZeroOffset(), offset, RevConfigRegistry.EPSILON), - "Zero Offset" - ); - return this; - } -} diff --git a/src/main/java/org/team340/lib/util/rev/SparkFlexConfig.java b/src/main/java/org/team340/lib/util/rev/SparkFlexConfig.java deleted file mode 100644 index c9e6ae2..0000000 --- a/src/main/java/org/team340/lib/util/rev/SparkFlexConfig.java +++ /dev/null @@ -1,601 +0,0 @@ -package org.team340.lib.util.rev; - -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.REVLibError; -import org.team340.lib.util.Math2; -import org.team340.lib.util.Sleep; - -/** - * Config builder for {@link CANSparkFlex}. - */ -public class SparkFlexConfig extends RevConfigBase { - - private static final double FACTORY_DEFAULTS_SLEEP = 0.025; - private static final SparkFlexConfig kDefaults = new SparkFlexConfig() - .clearFaults() - .restoreFactoryDefaults() - .enableVoltageCompensation(12.0) - .setPeriodicFramePeriod(Frame.S0, 20) - .setPeriodicFramePeriod(Frame.S1, 20) - .setPeriodicFramePeriod(Frame.S2, 20) - .setPeriodicFramePeriod(Frame.S3, 10000) - .setPeriodicFramePeriod(Frame.S4, 10000) - .setPeriodicFramePeriod(Frame.S5, 10000) - .setPeriodicFramePeriod(Frame.S6, 10000) - .setPeriodicFramePeriod(Frame.S7, 10000); - - /** - * Returns a default Spark Flex config. - */ - public static SparkFlexConfig defaults() { - return kDefaults.clone(); - } - - /** - * Clones this config. - */ - public SparkFlexConfig clone() { - var config = new SparkFlexConfig(); - config.configSteps.addAll(configSteps); - return config; - } - - /** - * Applies the config. Note that this is a blocking operation. Errors - * are printed when calling {@link RevConfigRegistry#burnFlashAll()}. - * @param sparkFlex The Spark Flex to apply the config to. - */ - public void apply(CANSparkFlex sparkFlex) { - String identifier = "Spark Flex (ID " + sparkFlex.getDeviceId() + ")"; - applySteps(sparkFlex, identifier); - RevConfigRegistry.addBurnFlash(identifier, () -> sparkFlex.burnFlash()); - } - - /** - * Spark Flex CAN status frames. - * @see https://docs.revrobotics.com/brushless/spark-max/control-interfaces#periodic-status-frames - */ - public static enum Frame { - /** - *

Periodic Status 0

- * - * Default rate: {@code 10ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Applied **** Output The actual value sent to the motors from the motor controller. This value is also used by any follower controllers to set their output.
Faults Each bit represents a different fault on the controller. These fault bits clear automatically when the fault goes away.
Sticky Faults The same as the Faults field, however the bits do not reset until a power cycle or a 'Clear Faults' command is sent.
Is Follower A single bit that is true if the controller is configured to follow another controller.
- */ - S0(PeriodicFrame.kStatus0), - /** - *

Periodic Status 1

- * - * Default rate: {@code 20ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Motor Velocity 32-bit IEEE floating-point representation of the motor velocity in RPM using the selected sensor.
Motor Temperature 8-bit unsigned value representing motor temperature in °C for the NEO Brushless Motor.
Motor Voltage 12-bit fixed-point value that is converted to a floating point voltage value (in Volts) by the roboRIO SDK. This is the input voltage to the controller.
Motor Current 12-bit fixed-point value that is converted to a floating point current value (in Amps) by the roboRIO SDK. This is the raw phase current of the motor.
- */ - S1(PeriodicFrame.kStatus1), - /** - *

Periodic Status 2

- * - * Default rate: {@code 20ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Motor Position 32-bit IEEE floating-point representation of the motor position in rotations.
- */ - S2(PeriodicFrame.kStatus2), - /** - *

Periodic Status 3

- * - * Default rate: {@code 50ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Analog Sensor Voltage 10-bit fixed-point value that is converted to a floating point voltage value (in Volts) by the roboRIO SDK. This is the voltage being output by the analog sensor.
Analog Sensor Velocity 22-bit fixed-point value that is converted to a floating point voltage value (in RPM) by the roboRIO SDK. This is the velocity reported by the analog sensor.
Analog Sensor Position 32-bit IEEE floating-point representation of the velocity in RPM reported by the analog sensor.
- */ - S3(PeriodicFrame.kStatus3), - /** - *

Periodic Status 4

- * - * Default rate: {@code 20ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Alternate Encoder Velocity 32-bit IEEE floating-point representation of the velocity in RPM of the alternate encoder.
Alternate Encoder Position 32-bit IEEE floating-point representation of the position in rotations of the alternate encoder.
- */ - S4(PeriodicFrame.kStatus4), - /** - *

Periodic Status 5

- * - * Default rate: {@code 200ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Duty Cycle Absolute Encoder Position 32-bit IEEE floating-point representation of the position of the duty cycle absolute encoder.
Duty Cycle Absolute Encoder Absolute Angle 16-bit integer representation of the absolute angle of the duty cycle absolute encoder.
- */ - S5(PeriodicFrame.kStatus5), - /** - *

Periodic Status 6

- * - * Default rate: {@code 200ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Duty Cycle Absolute Encoder Velocity 32-bit IEEE floating-point representation of the velocity in RPM of the duty cycle absolute encoder.
Duty Cycle Absolute Encoder Frequency 16-bit unsigned integer representation of the frequency at which the duty cycle signal is being sent.
- */ - S6(PeriodicFrame.kStatus6), - /** - *

Periodic Status 7

- * - * Default rate: {@code 250ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - *
Available Data Description
IAccum I accumulator of the PID controller.
- */ - S7(PeriodicFrame.kStatus7); - - private final PeriodicFrame frame; - - private Frame(PeriodicFrame frame) { - this.frame = frame; - } - - public PeriodicFrame frame() { - return frame; - } - } - - /** - * Clears all sticky faults. - */ - public SparkFlexConfig clearFaults() { - addStep(sparkFlex -> sparkFlex.clearFaults(), "Clear Faults"); - return this; - } - - /** - * Disables the voltage compensation setting for all modes. - */ - public SparkFlexConfig disableVoltageCompensation() { - addStep( - sparkFlex -> sparkFlex.disableVoltageCompensation(), - sparkFlex -> - Math2.epsilonEquals(sparkFlex.getVoltageCompensationNominalVoltage(), 0.0, RevConfigRegistry.EPSILON), - "Disable Voltage Compensation" - ); - return this; - } - - /** - * Enables soft limits. - * @param direction The direction of motion to restrict. - * @param enable Set {@code true} to enable soft limits. - */ - public SparkFlexConfig enableSoftLimit(CANSparkFlex.SoftLimitDirection direction, boolean enable) { - addStep( - sparkFlex -> sparkFlex.enableSoftLimit(direction, enable), - sparkFlex -> sparkFlex.isSoftLimitEnabled(direction) == enable, - "Enable Soft Limit" - ); - return this; - } - - /** - * Sets the voltage compensation setting for all modes on the Spark Flex and enables voltage compensation. - * @param nominalVoltage Nominal voltage to compensate output to. - */ - public SparkFlexConfig enableVoltageCompensation(double nominalVoltage) { - addStep( - sparkFlex -> sparkFlex.enableVoltageCompensation(nominalVoltage), - sparkFlex -> - Math2.epsilonEquals( - sparkFlex.getVoltageCompensationNominalVoltage(), - nominalVoltage, - RevConfigRegistry.EPSILON - ), - "Enable Voltage Compensation" - ); - return this; - } - - /** - * Causes this controller's output to mirror the provided leader. - * Only voltage output is mirrored. Settings changed on the leader do not affect the follower. - * The motor will spin in the same direction as the leader. This can be changed by passing a {@code true} constant after the leader parameter. - * @param leader The motor controller to follow. - */ - public SparkFlexConfig follow(CANSparkFlex leader) { - addStep(sparkFlex -> sparkFlex.follow(leader), sparkFlex -> sparkFlex.isFollower(), false, "Follow"); - return this; - } - - /** - * Causes this controller's output to mirror the provided leader. - * Only voltage output is mirrored. Settings changed on the leader do not affect the follower. - * @param leader The motor controller to follow. - * @param invert Set the follower to output opposite of the leader. - */ - public SparkFlexConfig follow(CANSparkFlex leader, boolean invert) { - addStep(sparkFlex -> sparkFlex.follow(leader, invert), sparkFlex -> sparkFlex.isFollower(), false, "Follow"); - return this; - } - - /** - * Causes this controller's output to mirror the provided leader. - * Only voltage output is mirrored. Settings changed on the leader do not affect the follower. - * The motor will spin in the same direction as the leader. This can be changed by passing a {@code true} constant after the deviceID parameter. - * @param leader The type of motor controller to follow (Talon SRX, SPARK MAX, etc.). - * @param deviceId The CAN ID of the device to follow. - */ - public SparkFlexConfig follow(CANSparkFlex.ExternalFollower leader, int deviceId) { - addStep(sparkFlex -> sparkFlex.follow(leader, deviceId), sparkFlex -> sparkFlex.isFollower(), false, "Follow"); - return this; - } - - /** - * Causes this controller's output to mirror the provided leader. - * Only voltage output is mirrored. Settings changed on the leader do not affect the follower. - * @param leader The type of motor controller to follow (Talon SRX, SPARK MAX, etc.). - * @param deviceId The CAN ID of the device to follow. - * @param invert Set the follower to output opposite of the leader. - */ - public SparkFlexConfig follow(CANSparkFlex.ExternalFollower leader, int deviceId, boolean invert) { - addStep( - sparkFlex -> sparkFlex.follow(leader, deviceId, invert), - sparkFlex -> sparkFlex.isFollower(), - false, - "Follow" - ); - return this; - } - - /** - * Sets timeout for sending CAN messages with {@code setParameter*()} and {@code getParameter*()} - * calls.These calls will block for up to this amount of time before returning a timeout error. - * A timeout of {@code 0} will make the {@code setParameter*()} calls non-blocking, and instead - * will check the response in a separate thread. With this configuration, any error messages will - * appear on the driver station but will not be returned by the {@code getLastError()} call. - * @param milliseconds The timeout in milliseconds. - */ - public SparkFlexConfig setCANTimeout(int milliseconds) { - addStep(sparkFlex -> sparkFlex.setCANTimeout(milliseconds), "CAN Timeout"); - return this; - } - - /** - * Sets the ramp rate for closed loop control modes. - * This is the maximum rate at which the motor controller's output is allowed to change. - * @param rate Time in seconds to go from {@code 0.0} to full throttle. - */ - public SparkFlexConfig setClosedLoopRampRate(double rate) { - addStep( - sparkFlex -> sparkFlex.setClosedLoopRampRate(rate), - sparkFlex -> Math2.epsilonEquals(sparkFlex.getClosedLoopRampRate(), rate, RevConfigRegistry.EPSILON), - "Closed Loop Ramp Rate" - ); - return this; - } - - /** - * Sets the idle mode setting for the SPARK MAX. - * @param mode Idle mode (coast or brake). - */ - public SparkFlexConfig setIdleMode(CANSparkFlex.IdleMode mode) { - addStep( - sparkFlex -> sparkFlex.setIdleMode(mode), - sparkFlex -> sparkFlex.getIdleMode().equals(mode), - "Idle Mode" - ); - return this; - } - - /** - * Common interface for inverting direction of a speed controller. - * This call has no effect if the controller is a follower. To invert a follower, see the {@code follow()} method. - * @param isInverted The state of inversion, true is inverted. - */ - public SparkFlexConfig setInverted(boolean isInverted) { - addStep( - sparkFlex -> { - sparkFlex.setInverted(isInverted); - return REVLibError.kOk; - }, - sparkFlex -> sparkFlex.getInverted() == isInverted, - "Inverted" - ); - return this; - } - - /** - * Sets the ramp rate for open loop control modes. - * This is the maximum rate at which the motor controller's output is allowed to change. - * @param rate Time in seconds to go from {@code 0.0} to full throttle. - */ - public SparkFlexConfig setOpenLoopRampRate(double rate) { - addStep( - sparkFlex -> sparkFlex.setOpenLoopRampRate(rate), - sparkFlex -> Math2.epsilonEquals(sparkFlex.getOpenLoopRampRate(), rate, RevConfigRegistry.EPSILON), - "Open Loop Ramp Rate" - ); - return this; - } - - /** - * Sets the rate of transmission for a periodic frame. - * Each motor controller sends back status frames with different data at set rates. Use this function to change the default rates. - * These values are not stored in the flash after calling {@code burnFlash()} and is reset on powerup. - * @param frame The status frame to configure. - * @param periodMs The rate the controller sends the frame in milliseconds. - */ - public SparkFlexConfig setPeriodicFramePeriod(Frame frame, int periodMs) { - addStep( - sparkFlex -> { - RevConfigRegistry.addFrameRefresher(sparkFlex.hashCode() + "." + frame.name(), () -> - sparkFlex.setPeriodicFramePeriod(frame.frame, periodMs) - ); - return sparkFlex.setPeriodicFramePeriod(frame.frame, periodMs); - }, - "Periodic Frame Status " + frame.ordinal() - ); - return this; - } - - /** - * Sets the secondary current limit in amps. - * The motor controller will disable the output of the controller briefly if the - * current limit is exceeded to reduce the current. This limit is a simplified - * 'on/off' controller. This limit is enabled by default but is set higher than - * the default Smart Current Limit. The time the controller is off after the current - * limit is reached is determined by the parameter limitCycles, which is the number - * of PWM cycles (20kHz). The recommended value is the default of {@code 0} which is - * the minimum time and is part of a PWM cycle from when the over current is detected. - * This allows the controller to regulate the current close to the limit value. - * @param limit The current limit in amps. - */ - public SparkFlexConfig setSecondaryCurrentLimit(double limit) { - addStep(sparkFlex -> sparkFlex.setSecondaryCurrentLimit(limit), "Secondary Current Limit"); - return this; - } - - /** - * Sets the secondary current limit in amps. - * The motor controller will disable the output of the controller briefly if the - * current limit is exceeded to reduce the current. This limit is a simplified - * 'on/off' controller. This limit is enabled by default but is set higher than - * the default Smart Current Limit. The time the controller is off after the current - * limit is reached is determined by the parameter limitCycles, which is the number - * of PWM cycles (20kHz). The recommended value is the default of {@code 0} which is - * the minimum time and is part of a PWM cycle from when the over current is detected. - * This allows the controller to regulate the current close to the limit value. - * @param limit The current limit in amps. - * @param chopCycles The number of additional PWM cycles to turn the driver off after overcurrent is detected. - */ - public SparkFlexConfig setSecondaryCurrentLimit(double limit, int chopCycles) { - addStep(sparkFlex -> sparkFlex.setSecondaryCurrentLimit(limit, chopCycles), "Secondary Current Limit"); - return this; - } - - /** - * Sets the current limit in amps. - * The motor controller will reduce the controller voltage output to avoid surpassing - * this limit. This limit is enabled by default and used for brushless only. This limit - * is highly recommended when using the NEO brushless motor. The NEO Brushless Motor - * has a low internal resistance, which can mean large current spikes that could be - * enough to cause damage to the motor and controller. This current limit provides a - * smarter strategy to deal with high current draws and keep the motor and controller - * operating in a safe region. - * @param limit The current limit in amps. - */ - public SparkFlexConfig setSmartCurrentLimit(int limit) { - addStep(sparkFlex -> sparkFlex.setSmartCurrentLimit(limit), "Smart Current Limit"); - return this; - } - - /** - * Sets the current limit in amps. - * The motor controller will reduce the controller voltage output to avoid surpassing - * this limit. This limit is enabled by default and used for brushless only. This limit - * is highly recommended when using the NEO brushless motor. The NEO Brushless Motor - * has a low internal resistance, which can mean large current spikes that could be - * enough to cause damage to the motor and controller. This current limit provides a - * smarter strategy to deal with high current draws and keep the motor and controller - * operating in a safe region. The controller can also limit the current based on the RPM - * of the motor in a linear fashion to help with controllability in closed loop control. - * For a response that is linear the entire RPM range leave limit RPM at {@code 0}. - * @param stallLimit The current limit in amps at {@code 0} RPM. - * @param freeLimit The current limit at free speed ({@code 5700} RPM for NEO). - */ - public SparkFlexConfig setSmartCurrentLimit(int stallLimit, int freeLimit) { - addStep(sparkFlex -> sparkFlex.setSmartCurrentLimit(stallLimit, freeLimit), "Smart Current Limit"); - return this; - } - - /** - * Sets the current limit in amps. - * The motor controller will reduce the controller voltage output to avoid surpassing - * this limit. This limit is enabled by default and used for brushless only. This limit - * is highly recommended when using the NEO brushless motor. The NEO Brushless Motor - * has a low internal resistance, which can mean large current spikes that could be - * enough to cause damage to the motor and controller. This current limit provides a - * smarter strategy to deal with high current draws and keep the motor and controller - * operating in a safe region. The controller can also limit the current based on the RPM - * of the motor in a linear fashion to help with controllability in closed loop control. - * For a response that is linear the entire RPM range leave limit RPM at {@code 0}. - * @param stallLimit The current limit in amps at {@code 0} RPM. - * @param freeLimit The current limit at free speed ({@code 5700} RPM for NEO). - * @param limitRPM RPM less than this value will be set to the stallLimit, RPM values greater than limitRPM will scale linearly to freeLimit. - */ - public SparkFlexConfig setSmartCurrentLimit(int stallLimit, int freeLimit, int limitRPM) { - addStep(sparkFlex -> sparkFlex.setSmartCurrentLimit(stallLimit, freeLimit, limitRPM), "Smart Current Limit"); - return this; - } - - /** - * Sets the soft limit based on position. The default unit is rotations, but will match - * the unit scaling set by the user. Note that this value is not scaled internally so - * care must be taken to make sure these units match the desired conversion. - * @param direction The direction of motion to restrict. - * @param limit Position soft limit of the controller. - */ - public SparkFlexConfig setSoftLimit(CANSparkFlex.SoftLimitDirection direction, double limit) { - addStep( - sparkFlex -> sparkFlex.setSoftLimit(direction, (float) limit), - sparkFlex -> Math2.epsilonEquals(sparkFlex.getSoftLimit(direction), limit, RevConfigRegistry.EPSILON), - "Soft Limit" - ); - return this; - } - - /** - * Restores motor controller parameters to factory defaults. - *

This option should be declared first, to ensure all other configuration options aren't overwritten.

- */ - public SparkFlexConfig restoreFactoryDefaults() { - addStep( - sparkFlex -> { - REVLibError res = sparkFlex.restoreFactoryDefaults(); - Sleep.seconds(FACTORY_DEFAULTS_SLEEP, true); - return res; - }, - "Restore Factory Defaults" - ); - return this; - } - - /** - * Restores motor controller parameters to factory defaults. - * @param persist If {@code true}, burn the flash with the factory default parameters. - *

This option should be declared first, to ensure all other configuration options aren't overwritten.

- */ - public SparkFlexConfig restoreFactoryDefaults(boolean persist) { - addStep( - sparkFlex -> { - REVLibError res = sparkFlex.restoreFactoryDefaults(persist); - Sleep.seconds(FACTORY_DEFAULTS_SLEEP, true); - return res; - }, - "Restore Factory Defaults" - ); - return this; - } -} diff --git a/src/main/java/org/team340/lib/util/rev/SparkLimitSwitchConfig.java b/src/main/java/org/team340/lib/util/rev/SparkLimitSwitchConfig.java deleted file mode 100644 index f5f999a..0000000 --- a/src/main/java/org/team340/lib/util/rev/SparkLimitSwitchConfig.java +++ /dev/null @@ -1,53 +0,0 @@ -package org.team340.lib.util.rev; - -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkLimitSwitch; - -/** - * Config builder for {@link SparkLimitSwitch}. - */ -public class SparkLimitSwitchConfig extends RevConfigBase { - - /** - * Clones this config. - */ - public SparkLimitSwitchConfig clone() { - var config = new SparkLimitSwitchConfig(); - config.configSteps.addAll(configSteps); - return config; - } - - /** - * Applies the config to a Spark Max attached limit switch. Note that this is a blocking - * operation. Errors are printed when calling {@link RevConfigRegistry#burnFlashAll()}. - * @param sparkMax The Spark Max the limit switch is attached to. - * @param limitSwitch The limit switch. - */ - public void apply(CANSparkMax sparkMax, SparkLimitSwitch limitSwitch) { - applySteps(limitSwitch, "Spark Max (ID " + sparkMax.getDeviceId() + ") Limit Switch"); - } - - /** - * Applies the config to a Spark Flex limit switch. Note that this is a blocking operation. - * Errors are printed when calling {@link RevConfigRegistry#burnFlashAll()}. - * @param sparkFlex The Spark Flex the limit switch is attached to. - * @param limitSwitch The limit switch. - */ - public void apply(CANSparkFlex sparkFlex, SparkLimitSwitch limitSwitch) { - applySteps(limitSwitch, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") Limit Switch"); - } - - /** - * Enables or disables controller shutdown based on the limit switch. - * @param enable Enable/disable motor shutdown based on the limit switch state. This does not affect the result of the get() command. - */ - public SparkLimitSwitchConfig enableLimitSwitch(boolean enable) { - addStep( - limitSwitch -> limitSwitch.enableLimitSwitch(enable), - limitSwitch -> limitSwitch.isLimitSwitchEnabled() == enable, - "Enabled" - ); - return this; - } -} diff --git a/src/main/java/org/team340/lib/util/rev/SparkMaxConfig.java b/src/main/java/org/team340/lib/util/rev/SparkMaxConfig.java deleted file mode 100644 index d6bc33a..0000000 --- a/src/main/java/org/team340/lib/util/rev/SparkMaxConfig.java +++ /dev/null @@ -1,597 +0,0 @@ -package org.team340.lib.util.rev; - -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; -import org.team340.lib.util.Math2; -import org.team340.lib.util.Sleep; - -/** - * Config builder for {@link CANSparkMax}. - */ -public class SparkMaxConfig extends RevConfigBase { - - private static final double FACTORY_DEFAULTS_SLEEP = 0.050; - private static final SparkMaxConfig kDefaults = new SparkMaxConfig() - .clearFaults() - .restoreFactoryDefaults() - .enableVoltageCompensation(12.0) - .setPeriodicFramePeriod(Frame.S0, 20) - .setPeriodicFramePeriod(Frame.S1, 20) - .setPeriodicFramePeriod(Frame.S2, 20) - .setPeriodicFramePeriod(Frame.S3, 10000) - .setPeriodicFramePeriod(Frame.S4, 10000) - .setPeriodicFramePeriod(Frame.S5, 10000) - .setPeriodicFramePeriod(Frame.S6, 10000) - .setPeriodicFramePeriod(Frame.S7, 10000); - - /** - * Returns a default Spark MAX config. - */ - public static SparkMaxConfig defaults() { - return kDefaults.clone(); - } - - /** - * Clones this config. - */ - public SparkMaxConfig clone() { - var config = new SparkMaxConfig(); - config.configSteps.addAll(configSteps); - return config; - } - - /** - * Applies the config. Note that this is a blocking operation. Errors - * are printed when calling {@link RevConfigRegistry#burnFlashAll()}. - * @param sparkMax The Spark Max to apply the config to. - */ - public void apply(CANSparkMax sparkMax) { - String identifier = "Spark Max (ID " + sparkMax.getDeviceId() + ")"; - applySteps(sparkMax, identifier); - RevConfigRegistry.addBurnFlash(identifier, () -> sparkMax.burnFlash()); - } - - /** - * Spark Flex CAN status frames. - * @see https://docs.revrobotics.com/brushless/spark-max/control-interfaces#periodic-status-frames - */ - public static enum Frame { - /** - *

Periodic Status 0

- * - * Default rate: {@code 10ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Applied **** Output The actual value sent to the motors from the motor controller. This value is also used by any follower controllers to set their output.
Faults Each bit represents a different fault on the controller. These fault bits clear automatically when the fault goes away.
Sticky Faults The same as the Faults field, however the bits do not reset until a power cycle or a 'Clear Faults' command is sent.
Is Follower A single bit that is true if the controller is configured to follow another controller.
- */ - S0(PeriodicFrame.kStatus0), - /** - *

Periodic Status 1

- * - * Default rate: {@code 20ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Motor Velocity 32-bit IEEE floating-point representation of the motor velocity in RPM using the selected sensor.
Motor Temperature 8-bit unsigned value representing motor temperature in °C for the NEO Brushless Motor.
Motor Voltage 12-bit fixed-point value that is converted to a floating point voltage value (in Volts) by the roboRIO SDK. This is the input voltage to the controller.
Motor Current 12-bit fixed-point value that is converted to a floating point current value (in Amps) by the roboRIO SDK. This is the raw phase current of the motor.
- */ - S1(PeriodicFrame.kStatus1), - /** - *

Periodic Status 2

- * - * Default rate: {@code 20ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Motor Position 32-bit IEEE floating-point representation of the motor position in rotations.
- */ - S2(PeriodicFrame.kStatus2), - /** - *

Periodic Status 3

- * - * Default rate: {@code 50ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Analog Sensor Voltage 10-bit fixed-point value that is converted to a floating point voltage value (in Volts) by the roboRIO SDK. This is the voltage being output by the analog sensor.
Analog Sensor Velocity 22-bit fixed-point value that is converted to a floating point voltage value (in RPM) by the roboRIO SDK. This is the velocity reported by the analog sensor.
Analog Sensor Position 32-bit IEEE floating-point representation of the velocity in RPM reported by the analog sensor.
- */ - S3(PeriodicFrame.kStatus3), - /** - *

Periodic Status 4

- * - * Default rate: {@code 20ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Alternate Encoder Velocity 32-bit IEEE floating-point representation of the velocity in RPM of the alternate encoder.
Alternate Encoder Position 32-bit IEEE floating-point representation of the position in rotations of the alternate encoder.
- */ - S4(PeriodicFrame.kStatus4), - /** - *

Periodic Status 5

- * - * Default rate: {@code 200ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Duty Cycle Absolute Encoder Position 32-bit IEEE floating-point representation of the position of the duty cycle absolute encoder.
Duty Cycle Absolute Encoder Absolute Angle 16-bit integer representation of the absolute angle of the duty cycle absolute encoder.
- */ - S5(PeriodicFrame.kStatus5), - /** - *

Periodic Status 6

- * - * Default rate: {@code 200ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - * - * - * - * - *
Available Data Description
Duty Cycle Absolute Encoder Velocity 32-bit IEEE floating-point representation of the velocity in RPM of the duty cycle absolute encoder.
Duty Cycle Absolute Encoder Frequency 16-bit unsigned integer representation of the frequency at which the duty cycle signal is being sent.
- */ - S6(PeriodicFrame.kStatus6), - /** - *

Periodic Status 7

- * - * Default rate: {@code 250ms} - * - *

Frame content:

- * - * - * - * - * - * - * - * - * - * - *
Available Data Description
IAccum I accumulator of the PID controller.
- */ - S7(PeriodicFrame.kStatus7); - - private final PeriodicFrame frame; - - private Frame(PeriodicFrame frame) { - this.frame = frame; - } - - public PeriodicFrame frame() { - return frame; - } - } - - /** - * Clears all sticky faults. - */ - public SparkMaxConfig clearFaults() { - addStep(sparkMax -> sparkMax.clearFaults(), "Clear Faults"); - return this; - } - - /** - * Disables the voltage compensation setting for all modes. - */ - public SparkMaxConfig disableVoltageCompensation() { - addStep( - sparkMax -> sparkMax.disableVoltageCompensation(), - sparkMax -> - Math2.epsilonEquals(sparkMax.getVoltageCompensationNominalVoltage(), 0.0, RevConfigRegistry.EPSILON), - "Disable Voltage Compensation" - ); - return this; - } - - /** - * Enables soft limits. - * @param direction The direction of motion to restrict. - * @param enable Set {@code true} to enable soft limits. - */ - public SparkMaxConfig enableSoftLimit(CANSparkMax.SoftLimitDirection direction, boolean enable) { - addStep( - sparkMax -> sparkMax.enableSoftLimit(direction, enable), - sparkMax -> sparkMax.isSoftLimitEnabled(direction) == enable, - "Enable Soft Limit" - ); - return this; - } - - /** - * Sets the voltage compensation setting for all modes on the Spark Max and enables voltage compensation. - * @param nominalVoltage Nominal voltage to compensate output to. - */ - public SparkMaxConfig enableVoltageCompensation(double nominalVoltage) { - addStep( - sparkMax -> sparkMax.enableVoltageCompensation(nominalVoltage), - sparkMax -> - Math2.epsilonEquals( - sparkMax.getVoltageCompensationNominalVoltage(), - nominalVoltage, - RevConfigRegistry.EPSILON - ), - "Enable Voltage Compensation" - ); - return this; - } - - /** - * Causes this controller's output to mirror the provided leader. - * Only voltage output is mirrored. Settings changed on the leader do not affect the follower. - * The motor will spin in the same direction as the leader. This can be changed by passing a {@code true} constant after the leader parameter. - * @param leader The motor controller to follow. - */ - public SparkMaxConfig follow(CANSparkMax leader) { - addStep(sparkMax -> sparkMax.follow(leader), sparkMax -> sparkMax.isFollower(), false, "Follow"); - return this; - } - - /** - * Causes this controller's output to mirror the provided leader. - * Only voltage output is mirrored. Settings changed on the leader do not affect the follower. - * @param leader The motor controller to follow. - * @param invert Set the follower to output opposite of the leader. - */ - public SparkMaxConfig follow(CANSparkMax leader, boolean invert) { - addStep(sparkMax -> sparkMax.follow(leader, invert), sparkMax -> sparkMax.isFollower(), false, "Follow"); - return this; - } - - /** - * Causes this controller's output to mirror the provided leader. - * Only voltage output is mirrored. Settings changed on the leader do not affect the follower. - * The motor will spin in the same direction as the leader. This can be changed by passing a {@code true} constant after the deviceID parameter. - * @param leader The type of motor controller to follow (Talon SRX, SPARK MAX, etc.). - * @param deviceId The CAN ID of the device to follow. - */ - public SparkMaxConfig follow(CANSparkMax.ExternalFollower leader, int deviceId) { - addStep(sparkMax -> sparkMax.follow(leader, deviceId), sparkMax -> sparkMax.isFollower(), false, "Follow"); - return this; - } - - /** - * Causes this controller's output to mirror the provided leader. - * Only voltage output is mirrored. Settings changed on the leader do not affect the follower. - * @param leader The type of motor controller to follow (Talon SRX, SPARK MAX, etc.). - * @param deviceId The CAN ID of the device to follow. - * @param invert Set the follower to output opposite of the leader. - */ - public SparkMaxConfig follow(CANSparkMax.ExternalFollower leader, int deviceId, boolean invert) { - addStep( - sparkMax -> sparkMax.follow(leader, deviceId, invert), - sparkMax -> sparkMax.isFollower(), - false, - "Follow" - ); - return this; - } - - /** - * Sets timeout for sending CAN messages with {@code setParameter*()} and {@code getParameter*()} - * calls.These calls will block for up to this amount of time before returning a timeout error. - * A timeout of {@code 0} will make the {@code setParameter*()} calls non-blocking, and instead - * will check the response in a separate thread. With this configuration, any error messages will - * appear on the driver station but will not be returned by the {@code getLastError()} call. - * @param milliseconds The timeout in milliseconds. - */ - public SparkMaxConfig setCANTimeout(int milliseconds) { - addStep(sparkMax -> sparkMax.setCANTimeout(milliseconds), "CAN Timeout"); - return this; - } - - /** - * Sets the ramp rate for closed loop control modes. - * This is the maximum rate at which the motor controller's output is allowed to change. - * @param rate Time in seconds to go from {@code 0.0} to full throttle. - */ - public SparkMaxConfig setClosedLoopRampRate(double rate) { - addStep( - sparkMax -> sparkMax.setClosedLoopRampRate(rate), - sparkMax -> Math2.epsilonEquals(sparkMax.getClosedLoopRampRate(), rate, RevConfigRegistry.EPSILON), - "Closed Loop Ramp Rate" - ); - return this; - } - - /** - * Sets the idle mode setting for the SPARK MAX. - * @param mode Idle mode (coast or brake). - */ - public SparkMaxConfig setIdleMode(CANSparkMax.IdleMode mode) { - addStep(sparkMax -> sparkMax.setIdleMode(mode), sparkMax -> sparkMax.getIdleMode().equals(mode), "Idle Mode"); - return this; - } - - /** - * Common interface for inverting direction of a speed controller. - * This call has no effect if the controller is a follower. To invert a follower, see the {@code follow()} method. - * @param isInverted The state of inversion, true is inverted. - */ - public SparkMaxConfig setInverted(boolean isInverted) { - addStep( - sparkMax -> { - sparkMax.setInverted(isInverted); - return REVLibError.kOk; - }, - sparkMax -> sparkMax.getInverted() == isInverted, - "Inverted" - ); - return this; - } - - /** - * Sets the ramp rate for open loop control modes. - * This is the maximum rate at which the motor controller's output is allowed to change. - * @param rate Time in seconds to go from {@code 0.0} to full throttle. - */ - public SparkMaxConfig setOpenLoopRampRate(double rate) { - addStep( - sparkMax -> sparkMax.setOpenLoopRampRate(rate), - sparkMax -> Math2.epsilonEquals(sparkMax.getOpenLoopRampRate(), rate, RevConfigRegistry.EPSILON), - "Open Loop Ramp Rate" - ); - return this; - } - - /** - * Sets the rate of transmission for a periodic frame. - * Each motor controller sends back status frames with different data at set rates. Use this function to change the default rates. - * These values are not stored in the flash after calling {@code burnFlash()} and is reset on powerup. - * @param frame The status frame to configure. - * @param periodMs The rate the controller sends the frame in milliseconds. - */ - public SparkMaxConfig setPeriodicFramePeriod(Frame frame, int periodMs) { - addStep( - sparkMax -> { - RevConfigRegistry.addFrameRefresher(sparkMax.hashCode() + "." + frame.name(), () -> - sparkMax.setPeriodicFramePeriod(frame.frame, periodMs) - ); - return sparkMax.setPeriodicFramePeriod(frame.frame, periodMs); - }, - "Periodic Frame Status " + frame.ordinal() - ); - return this; - } - - /** - * Sets the secondary current limit in amps. - * The motor controller will disable the output of the controller briefly if the - * current limit is exceeded to reduce the current. This limit is a simplified - * 'on/off' controller. This limit is enabled by default but is set higher than - * the default Smart Current Limit. The time the controller is off after the current - * limit is reached is determined by the parameter limitCycles, which is the number - * of PWM cycles (20kHz). The recommended value is the default of {@code 0} which is - * the minimum time and is part of a PWM cycle from when the over current is detected. - * This allows the controller to regulate the current close to the limit value. - * @param limit The current limit in amps. - */ - public SparkMaxConfig setSecondaryCurrentLimit(double limit) { - addStep(sparkMax -> sparkMax.setSecondaryCurrentLimit(limit), "Secondary Current Limit"); - return this; - } - - /** - * Sets the secondary current limit in amps. - * The motor controller will disable the output of the controller briefly if the - * current limit is exceeded to reduce the current. This limit is a simplified - * 'on/off' controller. This limit is enabled by default but is set higher than - * the default Smart Current Limit. The time the controller is off after the current - * limit is reached is determined by the parameter limitCycles, which is the number - * of PWM cycles (20kHz). The recommended value is the default of {@code 0} which is - * the minimum time and is part of a PWM cycle from when the over current is detected. - * This allows the controller to regulate the current close to the limit value. - * @param limit The current limit in amps. - * @param chopCycles The number of additional PWM cycles to turn the driver off after overcurrent is detected. - */ - public SparkMaxConfig setSecondaryCurrentLimit(double limit, int chopCycles) { - addStep(sparkMax -> sparkMax.setSecondaryCurrentLimit(limit, chopCycles), "Secondary Current Limit"); - return this; - } - - /** - * Sets the current limit in amps. - * The motor controller will reduce the controller voltage output to avoid surpassing - * this limit. This limit is enabled by default and used for brushless only. This limit - * is highly recommended when using the NEO brushless motor. The NEO Brushless Motor - * has a low internal resistance, which can mean large current spikes that could be - * enough to cause damage to the motor and controller. This current limit provides a - * smarter strategy to deal with high current draws and keep the motor and controller - * operating in a safe region. - * @param limit The current limit in amps. - */ - public SparkMaxConfig setSmartCurrentLimit(int limit) { - addStep(sparkMax -> sparkMax.setSmartCurrentLimit(limit), "Smart Current Limit"); - return this; - } - - /** - * Sets the current limit in amps. - * The motor controller will reduce the controller voltage output to avoid surpassing - * this limit. This limit is enabled by default and used for brushless only. This limit - * is highly recommended when using the NEO brushless motor. The NEO Brushless Motor - * has a low internal resistance, which can mean large current spikes that could be - * enough to cause damage to the motor and controller. This current limit provides a - * smarter strategy to deal with high current draws and keep the motor and controller - * operating in a safe region. The controller can also limit the current based on the RPM - * of the motor in a linear fashion to help with controllability in closed loop control. - * For a response that is linear the entire RPM range leave limit RPM at {@code 0}. - * @param stallLimit The current limit in amps at {@code 0} RPM. - * @param freeLimit The current limit at free speed ({@code 5700} RPM for NEO). - */ - public SparkMaxConfig setSmartCurrentLimit(int stallLimit, int freeLimit) { - addStep(sparkMax -> sparkMax.setSmartCurrentLimit(stallLimit, freeLimit), "Smart Current Limit"); - return this; - } - - /** - * Sets the current limit in amps. - * The motor controller will reduce the controller voltage output to avoid surpassing - * this limit. This limit is enabled by default and used for brushless only. This limit - * is highly recommended when using the NEO brushless motor. The NEO Brushless Motor - * has a low internal resistance, which can mean large current spikes that could be - * enough to cause damage to the motor and controller. This current limit provides a - * smarter strategy to deal with high current draws and keep the motor and controller - * operating in a safe region. The controller can also limit the current based on the RPM - * of the motor in a linear fashion to help with controllability in closed loop control. - * For a response that is linear the entire RPM range leave limit RPM at {@code 0}. - * @param stallLimit The current limit in amps at {@code 0} RPM. - * @param freeLimit The current limit at free speed ({@code 5700} RPM for NEO). - * @param limitRPM RPM less than this value will be set to the stallLimit, RPM values greater than limitRPM will scale linearly to freeLimit. - */ - public SparkMaxConfig setSmartCurrentLimit(int stallLimit, int freeLimit, int limitRPM) { - addStep(sparkMax -> sparkMax.setSmartCurrentLimit(stallLimit, freeLimit, limitRPM), "Smart Current Limit"); - return this; - } - - /** - * Sets the soft limit based on position. The default unit is rotations, but will match - * the unit scaling set by the user. Note that this value is not scaled internally so - * care must be taken to make sure these units match the desired conversion. - * @param direction The direction of motion to restrict. - * @param limit Position soft limit of the controller. - */ - public SparkMaxConfig setSoftLimit(CANSparkMax.SoftLimitDirection direction, double limit) { - addStep( - sparkMax -> sparkMax.setSoftLimit(direction, (float) limit), - sparkMax -> Math2.epsilonEquals(sparkMax.getSoftLimit(direction), limit, RevConfigRegistry.EPSILON), - "Soft Limit" - ); - return this; - } - - /** - * Restores motor controller parameters to factory defaults. - *

This option should be declared first, to ensure all other configuration options aren't overwritten.

- */ - public SparkMaxConfig restoreFactoryDefaults() { - addStep( - sparkMax -> { - REVLibError res = sparkMax.restoreFactoryDefaults(); - Sleep.seconds(FACTORY_DEFAULTS_SLEEP, true); - return res; - }, - "Restore Factory Defaults" - ); - return this; - } - - /** - * Restores motor controller parameters to factory defaults. - * @param persist If {@code true}, burn the flash with the factory default parameters. - *

This option should be declared first, to ensure all other configuration options aren't overwritten.

- */ - public SparkMaxConfig restoreFactoryDefaults(boolean persist) { - addStep( - sparkMax -> { - REVLibError res = sparkMax.restoreFactoryDefaults(persist); - Sleep.seconds(FACTORY_DEFAULTS_SLEEP, true); - return res; - }, - "Restore Factory Defaults" - ); - return this; - } -} diff --git a/src/main/java/org/team340/lib/util/rev/SparkPIDControllerConfig.java b/src/main/java/org/team340/lib/util/rev/SparkPIDControllerConfig.java deleted file mode 100644 index 9601039..0000000 --- a/src/main/java/org/team340/lib/util/rev/SparkPIDControllerConfig.java +++ /dev/null @@ -1,465 +0,0 @@ -package org.team340.lib.util.rev; - -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkMax; -import com.revrobotics.MotorFeedbackSensor; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkPIDController.AccelStrategy; -import org.team340.lib.util.Math2; - -/** - * Config builder for {@link SparkPIDController}. - */ -public class SparkPIDControllerConfig extends RevConfigBase { - - /** - * Clones this config. - */ - public SparkPIDControllerConfig clone() { - var config = new SparkPIDControllerConfig(); - config.configSteps.addAll(configSteps); - return config; - } - - /** - * Applies the config. Note that this is a blocking operation. Errors - * are printed when calling {@link RevConfigRegistry#burnFlashAll()}. - * @param sparkMax The Spark Max to apply the config to. - * @param pidController The PID controller. - */ - public void apply(CANSparkMax sparkMax, SparkPIDController pidController) { - applySteps(pidController, "Spark Max (ID " + sparkMax.getDeviceId() + ") PID Controller"); - } - - /** - * Applies the config. Note that this is a blocking operation. Errors - * are printed when calling {@link RevConfigRegistry#burnFlashAll()}. - * @param sparkFlex The Spark Flex to apply the config to. - * @param pidController The PID controller. - */ - public void apply(CANSparkFlex sparkFlex, SparkPIDController pidController) { - applySteps(pidController, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") PID Controller"); - } - - /** - * Sets the derivative gain constant of the PIDF controller on the Spark Max. - * @param gain The derivative gain value, must be positive. - */ - public SparkPIDControllerConfig setD(double gain) { - addStep( - pidController -> pidController.setD(gain), - pidController -> Math2.epsilonEquals(pidController.getD(), gain, RevConfigRegistry.EPSILON), - "D Gain" - ); - return this; - } - - /** - * Sets the derivative gain constant of the PIDF controller on the Spark Max. - * @param gain The derivative gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setD(double gain, int slotId) { - addStep( - pidController -> pidController.setD(gain, slotId), - pidController -> Math2.epsilonEquals(pidController.getD(slotId), gain, RevConfigRegistry.EPSILON), - "D Gain (Slot " + slotId + ")" - ); - return this; - } - - /** - * Sets the the derivative filter constant of the PIDF controller on the Spark Max. - * @param gain The derivative filter value, must be a positive number between {@code 0.0} and {@code 1.0}. - */ - public SparkPIDControllerConfig setDFilter(double gain) { - addStep( - pidController -> pidController.setDFilter(gain), - pidController -> Math2.epsilonEquals(pidController.getDFilter(0), gain, RevConfigRegistry.EPSILON), - "D Filter" - ); - return this; - } - - /** - * Sets the the derivative filter constant of the PIDF controller on the Spark Max. - * @param gain The derivative filter value, must be a positive number between {@code 0.0} and {@code 1.0}. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setDFilter(double gain, int slotId) { - addStep( - pidController -> pidController.setDFilter(gain, slotId), - pidController -> Math2.epsilonEquals(pidController.getDFilter(slotId), gain, RevConfigRegistry.EPSILON), - "D Filter (Slot " + slotId + ")" - ); - return this; - } - - /** - * Sets the controller's feedback device. The default feedback device in brushless mode - * is assumed to be the integrated encoder and the default feedback device in brushed - * mode is assumed to be a quadrature encoder. This is used to changed to another - * feedback device for the controller, such as an analog sensor. If there is a limited - * range on the feedback sensor that should be observed by the PIDController, it can be - * set by calling {@code setFeedbackSensorRange()} on the sensor object. - * @param sensor The sensor to use as a feedback device. - */ - public SparkPIDControllerConfig setFeedbackDevice(MotorFeedbackSensor sensor) { - addStep(pidController -> pidController.setFeedbackDevice(sensor), "Feedback Device"); - return this; - } - - /** - * Sets the feed-forward gain constant of the PIDF controller on the Spark Max. - * @param gain The feed-forward gain value, must be positive. - */ - public SparkPIDControllerConfig setFF(double gain) { - addStep( - pidController -> pidController.setFF(gain), - pidController -> Math2.epsilonEquals(pidController.getFF(), gain, RevConfigRegistry.EPSILON), - "FF Gain" - ); - return this; - } - - /** - * Sets the feed-forward gain constant of the PIDF controller on the Spark Max. - * @param gain The feed-forward gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setFF(double gain, int slotId) { - addStep( - pidController -> pidController.setFF(gain, slotId), - pidController -> Math2.epsilonEquals(pidController.getFF(slotId), gain, RevConfigRegistry.EPSILON), - "FF Gain (Slot " + slotId + ")" - ); - return this; - } - - /** - * Sets the integral gain constant of the PIDF controller on the Spark Max. - * @param gain The integral gain value, must be positive. - */ - public SparkPIDControllerConfig setI(double gain) { - addStep( - pidController -> pidController.setI(gain), - pidController -> Math2.epsilonEquals(pidController.getI(), gain, RevConfigRegistry.EPSILON), - "I Gain" - ); - return this; - } - - /** - * Sets the integral gain constant of the PIDF controller on the Spark Max. - * @param gain The integral gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setI(double gain, int slotId) { - addStep( - pidController -> pidController.setI(gain, slotId), - pidController -> Math2.epsilonEquals(pidController.getI(slotId), gain, RevConfigRegistry.EPSILON), - "I Gain (Slot " + slotId + ")" - ); - return this; - } - - /** - * Configure the maximum I accumulator of the PID controller. This value is used to constrain the - * I accumulator to help manage integral wind-up - * @param iMaxAccum The max value to constrain the I accumulator to. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setIMaxAccum(double iMaxAccum, int slotId) { - addStep( - pidController -> pidController.setIMaxAccum(iMaxAccum, slotId), - pidController -> - Math2.epsilonEquals(pidController.getIMaxAccum(slotId), iMaxAccum, RevConfigRegistry.EPSILON), - "I Max Accumulator (Slot " + slotId + ")" - ); - return this; - } - - /** - * Sets the IZone range of the PIDF controller on the Spark Max. This value specifies the - * range the error must be within for the integral constant to take effect. - * @param iZone The I zone value, must be positive. Set to {@code 0.0} to disable. - */ - public SparkPIDControllerConfig setIZone(double iZone) { - addStep( - pidController -> pidController.setIZone(iZone), - pidController -> Math2.epsilonEquals(pidController.getIZone(), iZone, RevConfigRegistry.EPSILON), - "I Zone" - ); - return this; - } - - /** - * Sets the IZone range of the PIDF controller on the Spark Max. This value specifies the - * range the error must be within for the integral constant to take effect. - * @param iZone The I zone value, must be positive. Set to {@code 0.0} to disable. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setIZone(double iZone, int slotId) { - addStep( - pidController -> pidController.setIZone(iZone, slotId), - pidController -> Math2.epsilonEquals(pidController.getIZone(slotId), iZone, RevConfigRegistry.EPSILON), - "I Zone (Slot " + slotId + ")" - ); - return this; - } - - /** - * Sets the min amd max output for the closed loop mode. - * @param min Reverse power minimum to allow the controller to output. - * @param max Forward power maximum to allow the controller to output. - */ - public SparkPIDControllerConfig setOutputRange(double min, double max) { - addStep( - pidController -> pidController.setOutputRange(min, max), - pidController -> - Math2.epsilonEquals(pidController.getOutputMin(), min, RevConfigRegistry.EPSILON) && - Math2.epsilonEquals(pidController.getOutputMax(), max, RevConfigRegistry.EPSILON), - "Output Range" - ); - return this; - } - - /** - * Sets the min amd max output for the closed loop mode. - * @param min Reverse power minimum to allow the controller to output. - * @param max Forward power maximum to allow the controller to output. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setOutputRange(double min, double max, int slotId) { - addStep( - pidController -> pidController.setOutputRange(min, max, slotId), - pidController -> - Math2.epsilonEquals(pidController.getOutputMin(slotId), min, RevConfigRegistry.EPSILON) && - Math2.epsilonEquals(pidController.getOutputMax(slotId), max, RevConfigRegistry.EPSILON), - "Output Range (Slot " + slotId + ")" - ); - return this; - } - - /** - * Sets the proportional gain constant of the PIDF controller on the Spark Max. - * @param gain The proportional gain value, must be positive. - */ - public SparkPIDControllerConfig setP(double gain) { - addStep( - pidController -> pidController.setP(gain), - pidController -> Math2.epsilonEquals(pidController.getP(), gain, RevConfigRegistry.EPSILON), - "P Gain" - ); - return this; - } - - /** - * Sets the proportional gain constant of the PIDF controller on the Spark Max. - * @param gain The proportional gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setP(double gain, int slotId) { - addStep( - pidController -> pidController.setP(gain, slotId), - pidController -> Math2.epsilonEquals(pidController.getP(slotId), gain, RevConfigRegistry.EPSILON), - "P Gain (Slot " + slotId + ")" - ); - return this; - } - - /** - * Enables or disables PID Wrapping for position closed loop control. - * @param enable Whether position PID wrapping should be enabled. - */ - public SparkPIDControllerConfig setPositionPIDWrappingEnabled(boolean enable) { - addStep( - pidController -> pidController.setPositionPIDWrappingEnabled(enable), - pidController -> pidController.getPositionPIDWrappingEnabled() == enable, - "Position PID Wrapping Enabled" - ); - return this; - } - - /** - * Sets the maximum input value for PID Wrapping with position closed loop control. - * @param max The value of max input for the position. - */ - public SparkPIDControllerConfig setPositionPIDWrappingMaxInput(double max) { - addStep( - pidController -> pidController.setPositionPIDWrappingMaxInput(max), - pidController -> - Math2.epsilonEquals(pidController.getPositionPIDWrappingMaxInput(), max, RevConfigRegistry.EPSILON), - "Position PID Wrapping Max Input" - ); - return this; - } - - /** - * Sets the minimum input value for PID Wrapping with position closed loop control. - * @param min The value of min input for the position. - */ - public SparkPIDControllerConfig setPositionPIDWrappingMinInput(double min) { - addStep( - pidController -> pidController.setPositionPIDWrappingMinInput(min), - pidController -> - Math2.epsilonEquals(pidController.getPositionPIDWrappingMinInput(), min, RevConfigRegistry.EPSILON), - "Position PID Wrapping Min Input" - ); - return this; - } - - /** - * Sets the minimum and maximum input values for PID Wrapping with position closed loop control. - * @param min The value of min input for the position. - * @param max The value of max input for the position. - */ - public SparkPIDControllerConfig setPositionPIDWrappingInputLimits(double min, double max) { - setPositionPIDWrappingMinInput(min); - setPositionPIDWrappingMaxInput(max); - return this; - } - - /** - * Sets PID gains on the Spark Max. - * @param pGain The proportional gain value, must be positive. - * @param iGain The integral gain value, must be positive. - * @param dGain The derivative gain value, must be positive. - */ - public SparkPIDControllerConfig setPID(double pGain, double iGain, double dGain) { - setP(pGain); - setI(iGain); - setD(dGain); - return this; - } - - /** - * Sets PID gains on the Spark Max. - * @param pGain The proportional gain value, must be positive. - * @param iGain The integral gain value, must be positive. - * @param dGain The derivative gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setPID(double pGain, double iGain, double dGain, int slotId) { - setP(pGain, slotId); - setI(iGain, slotId); - setD(dGain, slotId); - return this; - } - - /** - * Sets PIDF gains on the Spark Max. - * @param pGain The proportional gain value, must be positive. - * @param iGain The integral gain value, must be positive. - * @param dGain The derivative gain value, must be positive. - * @param ffGain The feed-forward gain value, must be positive. - */ - public SparkPIDControllerConfig setPIDF(double pGain, double iGain, double dGain, double ffGain) { - setP(pGain); - setI(iGain); - setD(dGain); - setFF(ffGain); - return this; - } - - /** - * Sets PIDF gains on the Spark Max. - * @param pGain The proportional gain value, must be positive. - * @param iGain The integral gain value, must be positive. - * @param dGain The derivative gain value, must be positive. - * @param ffGain The feed-forward gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setPIDF(double pGain, double iGain, double dGain, double ffGain, int slotId) { - setP(pGain, slotId); - setI(iGain, slotId); - setD(dGain, slotId); - setFF(ffGain, slotId); - return this; - } - - /** - * Configure the acceleration strategy used to control acceleration on the motor. - * @param accelStrategy The acceleration strategy to use for the automatically generated motion profile. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setSmartMotionAccelStrategy(AccelStrategy accelStrategy, int slotId) { - addStep( - pidController -> pidController.setSmartMotionAccelStrategy(accelStrategy, slotId), - pidController -> pidController.getSmartMotionAccelStrategy(slotId).equals(accelStrategy), - "Smart Motion Acceleration Strategy (Slot " + slotId + ")" - ); - return this; - } - - /** - * Configure the allowed closed loop error of SmartMotion mode. This value is how much deviation - * from your setpoint is tolerated and is useful in preventing oscillation around your setpoint. - * @param allowedErr The allowed deviation for your setpoint vs actual position in rotations. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setSmartMotionAllowedClosedLoopError(double allowedErr, int slotId) { - addStep( - pidController -> pidController.setSmartMotionAllowedClosedLoopError(allowedErr, slotId), - pidController -> - Math2.epsilonEquals( - pidController.getSmartMotionAllowedClosedLoopError(slotId), - allowedErr, - RevConfigRegistry.EPSILON - ), - "Smart Motion Allowed Closed Loop Error (Slot " + slotId + ")" - ); - return this; - } - - /** - * Configure the maximum acceleration of the SmartMotion mode. This is the acceleration that the - * motor velocity will increase at until the max velocity is reached - * @param maxAccel The maximum acceleration for the motion profile in RPM per second. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setSmartMotionMaxAccel(double maxAccel, int slotId) { - addStep( - pidController -> pidController.setSmartMotionMaxAccel(maxAccel, slotId), - pidController -> - Math2.epsilonEquals(pidController.getSmartMotionMaxAccel(slotId), maxAccel, RevConfigRegistry.EPSILON), - "Smart Motion Max Acceleration (Slot " + slotId + ")" - ); - return this; - } - - /** - * Configure the maximum velocity of the SmartMotion mode. This is the velocity that is reached in - * the middle of the profile and is what the motor should spend most of its time at. - * @param maxVel The maximum cruise velocity for the motion profile in RPM. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setSmartMotionMaxVelocity(double maxVel, int slotId) { - addStep( - pidController -> pidController.setSmartMotionMaxVelocity(maxVel, slotId), - pidController -> - Math2.epsilonEquals(pidController.getSmartMotionMaxVelocity(slotId), maxVel, RevConfigRegistry.EPSILON), - "Smart Motion Max Velocity (Slot " + slotId + ")" - ); - return this; - } - - /** - * Configure the minimum velocity of the SmartMotion mode. Any requested velocities below this value will be set to {@code 0.0}. - * @param minVel The minimum velocity for the motion profile in RPM. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setSmartMotionMinOutputVelocity(double minVel, int slotId) { - addStep( - pidController -> pidController.setSmartMotionMinOutputVelocity(minVel, slotId), - pidController -> - Math2.epsilonEquals( - pidController.getSmartMotionMinOutputVelocity(slotId), - minVel, - RevConfigRegistry.EPSILON - ), - "Smart Motion Min Velocity (Slot " + slotId + ")" - ); - return this; - } -} diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java index 1ef3d07..512d61d 100644 --- a/src/main/java/org/team340/robot/Constants.java +++ b/src/main/java/org/team340/robot/Constants.java @@ -9,14 +9,15 @@ */ public final class Constants { - public static final double PERIOD = 0.020; + public static final double kPeriod = 0.020; + public static final double kVoltage = 12.0; - public static final ControllerConfig DRIVER = new ControllerConfig() + public static final ControllerConfig kDriver = new ControllerConfig() .setPort(0) .setDeadbands(0.05, 0.05) .setThresholds(0.5, 0.05); - public static final ControllerConfig CO_DRIVER = new ControllerConfig() + public static final ControllerConfig kCoDriver = new ControllerConfig() .setPort(1) .setDeadbands(0.05, 0.05) .setThresholds(0.5, 0.05); @@ -27,22 +28,22 @@ public final class Constants { */ public static final class RobotMap { - public static final String CANBUS = "*"; + public static final String kSwerveCANBus = "Swerve"; - public static final int FL_MOVE = 2; - public static final int FL_TURN = 3; - public static final int FR_MOVE = 4; - public static final int FR_TURN = 5; - public static final int BL_MOVE = 6; - public static final int BL_TURN = 7; - public static final int BR_MOVE = 8; - public static final int BR_TURN = 9; + public static final int kFlMove = 2; + public static final int kFlTurn = 3; + public static final int kFrMove = 4; + public static final int kFrTurn = 5; + public static final int kBlMove = 6; + public static final int kBlTurn = 7; + public static final int kBrMove = 8; + public static final int kBrTurn = 9; - public static final int FL_ENCODER = 10; - public static final int FR_ENCODER = 11; - public static final int BL_ENCODER = 12; - public static final int BR_ENCODER = 13; + public static final int kFlEncoder = 10; + public static final int kFrEncoder = 11; + public static final int kBlEncoder = 12; + public static final int kBrEncoder = 13; - public static final int PIGEON = 14; + public static final int kCanandgyro = 14; } } diff --git a/src/main/java/org/team340/robot/Robot.java b/src/main/java/org/team340/robot/Robot.java index ac4acb2..6c3c51c 100644 --- a/src/main/java/org/team340/robot/Robot.java +++ b/src/main/java/org/team340/robot/Robot.java @@ -13,7 +13,9 @@ import org.team340.lib.controller.Controller; import org.team340.lib.dashboard.GRRDashboard; import org.team340.lib.util.Profiler; -import org.team340.lib.util.rev.RevConfigRegistry; +import org.team340.robot.commands.Autos; +import org.team340.robot.commands.Routines; +import org.team340.robot.subsystems.Foo; import org.team340.robot.subsystems.Swerve; /** @@ -25,18 +27,22 @@ @Logged public final class Robot extends TimedRobot { + public final Foo foo; + public final Swerve swerve; + + public final Routines routines; + private final Autos autos; + private final Controller driver; private final Controller coDriver; - private final Swerve swerve; - private Command autoCommand; public Robot() { - super(Constants.PERIOD); + super(Constants.kPeriod); DriverStation.silenceJoystickConnectionWarning(true); - // Start logging + // Configure logging SignalLogger.enableAutoLogging(false); DataLogManager.start(); DriverStation.startDataLog(DataLogManager.getLog()); @@ -44,31 +50,27 @@ public Robot() { config.root = "Telemetry"; }); - // Create controllers - driver = new Controller(Constants.DRIVER); - coDriver = new Controller(Constants.CO_DRIVER); - - // Initialize Subsystems + // Initialize subsystems + foo = new Foo(); swerve = new Swerve(); - // Finish configuration of REV hardware - RevConfigRegistry.burnFlashAll(); - RevConfigRegistry.enableFrameRefreshing(); + // Initialize compositions + routines = new Routines(this); + autos = new Autos(this); - // Configure Autos - GRRDashboard.addAuto("Example", none()); + // Initialize controllers + driver = new Controller(Constants.kDriver); + coDriver = new Controller(Constants.kCoDriver); - // Configure Default Commands + // Set default commands swerve.setDefaultCommand(swerve.drive(driver::getLeftX, driver::getLeftY, driver::getTriggerDifference)); - // Driver A => Do nothing + // Driver bindings driver.a().onTrue(none()); - - // Driver POV Left => Tare rotation driver.povLeft().onTrue(swerve.tareRotation()); - // Co-Driver A => Do nothing - coDriver.a().onTrue(none()); + // Co-driver bindings + coDriver.a().onTrue(foo.bar()); } @Override @@ -116,9 +118,7 @@ public void teleopPeriodic() {} public void teleopExit() {} @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } + public void testInit() {} @Override public void testPeriodic() {} diff --git a/src/main/java/org/team340/robot/commands/Autos.java b/src/main/java/org/team340/robot/commands/Autos.java index 36a6557..765f987 100644 --- a/src/main/java/org/team340/robot/commands/Autos.java +++ b/src/main/java/org/team340/robot/commands/Autos.java @@ -1,3 +1,40 @@ package org.team340.robot.commands; -public class Autos {} +import static edu.wpi.first.wpilibj2.command.Commands.*; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.wpilibj2.command.Command; +import org.team340.lib.dashboard.GRRDashboard; +import org.team340.robot.Robot; +import org.team340.robot.subsystems.Foo; +import org.team340.robot.subsystems.Swerve; + +/** + * The Autos class declares autonomous modes, and adds them + * to the dashboard to be selected by the drive team. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class Autos { + + private final Foo foo; + private final Swerve swerve; + private final Routines routines; + + public Autos(Robot robot) { + foo = robot.foo; + swerve = robot.swerve; + routines = robot.routines; + + // Add autonomous modes to the dashboard + GRRDashboard.addAuto("Example", example()); + } + + private Command example() { + return sequence( + foo.bar(), + swerve.drive(() -> 0.5, () -> 0.0, () -> 0.0).withTimeout(1.0), + routines.example() + ).withName("Autos.example()"); + } +} diff --git a/src/main/java/org/team340/robot/commands/Routines.java b/src/main/java/org/team340/robot/commands/Routines.java index 28b03be..5851ab5 100644 --- a/src/main/java/org/team340/robot/commands/Routines.java +++ b/src/main/java/org/team340/robot/commands/Routines.java @@ -1,3 +1,33 @@ package org.team340.robot.commands; -public class Routines {} +import static edu.wpi.first.wpilibj2.command.Commands.*; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.wpilibj2.command.Command; +import org.team340.robot.Robot; +import org.team340.robot.subsystems.Foo; +import org.team340.robot.subsystems.Swerve; + +/** + * The Routines class contains command compositions, such as sequences + * and parallel command groups, that require multiple subsystems. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class Routines { + + private final Foo foo; + private final Swerve swerve; + + public Routines(Robot robot) { + foo = robot.foo; + swerve = robot.swerve; + } + + /** + * An example routine. + */ + public Command example() { + return sequence(foo.bar(), swerve.stop(true)).withName("Routines.example()"); + } +} diff --git a/src/main/java/org/team340/robot/subsystems/Foo.java b/src/main/java/org/team340/robot/subsystems/Foo.java new file mode 100644 index 0000000..850ffea --- /dev/null +++ b/src/main/java/org/team340/robot/subsystems/Foo.java @@ -0,0 +1,20 @@ +package org.team340.robot.subsystems; + +import static edu.wpi.first.wpilibj2.command.Commands.*; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.wpilibj2.command.Command; +import org.team340.lib.util.GRRSubsystem; + +/** + * An example subsystem. + */ +@Logged +public final class Foo extends GRRSubsystem { + + public Foo() {} + + public Command bar() { + return print("Foo Bar").withName("Foo.bar()"); + } +} diff --git a/src/main/java/org/team340/robot/subsystems/Swerve.java b/src/main/java/org/team340/robot/subsystems/Swerve.java index 62df83d..95fc66b 100644 --- a/src/main/java/org/team340/robot/subsystems/Swerve.java +++ b/src/main/java/org/team340/robot/subsystems/Swerve.java @@ -4,8 +4,8 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.Supplier; -import org.team340.lib.swerve.ForwardPerspective; +import java.util.function.DoubleSupplier; +import org.team340.lib.swerve.Perspective; import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; @@ -16,56 +16,59 @@ import org.team340.robot.Constants; import org.team340.robot.Constants.RobotMap; +/** + * The robot's swerve drivetrain. + */ @Logged -public class Swerve extends GRRSubsystem { +public final class Swerve extends GRRSubsystem { - private static final SwerveModuleConfig FRONT_LEFT = new SwerveModuleConfig() + private static final SwerveModuleConfig kFrontLeft = new SwerveModuleConfig() .setName("frontLeft") .setLocation(0.28, 0.28) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.FL_MOVE, false)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.FL_TURN, false)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.FL_ENCODER, 0.0, false)); + .setMoveMotor(SwerveMotors.talonFX(RobotMap.kFlMove, false)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.kFlTurn, false)) + .setEncoder(SwerveEncoders.canCoder(RobotMap.kFlEncoder, 0.0, false)); - private static final SwerveModuleConfig FRONT_RIGHT = new SwerveModuleConfig() + private static final SwerveModuleConfig kFrontRight = new SwerveModuleConfig() .setName("frontRight") .setLocation(0.28, -0.28) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.FR_MOVE, false)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.FR_TURN, false)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.FR_ENCODER, 0.0, false)); + .setMoveMotor(SwerveMotors.talonFX(RobotMap.kFrMove, false)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.kFrTurn, false)) + .setEncoder(SwerveEncoders.canCoder(RobotMap.kFrEncoder, 0.0, false)); - private static final SwerveModuleConfig BACK_LEFT = new SwerveModuleConfig() + private static final SwerveModuleConfig kBackLeft = new SwerveModuleConfig() .setName("backLeft") .setLocation(-0.28, 0.28) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.BL_MOVE, false)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.BL_TURN, false)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.BL_ENCODER, 0.0, false)); + .setMoveMotor(SwerveMotors.talonFX(RobotMap.kBlMove, false)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.kBlTurn, false)) + .setEncoder(SwerveEncoders.canCoder(RobotMap.kBlEncoder, 0.0, false)); - private static final SwerveModuleConfig BACK_RIGHT = new SwerveModuleConfig() + private static final SwerveModuleConfig kBackRight = new SwerveModuleConfig() .setName("backRight") .setLocation(-0.28, -0.28) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.BR_MOVE, false)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.BR_TURN, false)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.BR_ENCODER, 0.0, false)); + .setMoveMotor(SwerveMotors.talonFX(RobotMap.kBrMove, false)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.kBrTurn, false)) + .setEncoder(SwerveEncoders.canCoder(RobotMap.kBrEncoder, 0.0, false)); - private static final SwerveConfig CONFIG = new SwerveConfig() - .setTimings(Constants.PERIOD, 0.004, 0.02) + private static final SwerveConfig kConfig = new SwerveConfig() + .setTimings(Constants.kPeriod, 0.004, 0.02) .setMovePID(0.1, 0.0, 0.0, 0.0) .setMoveFF(0.0, 2.4) .setTurnPID(0.5, 0.0, 0.0, 0.0) .setBrakeMode(false, true) .setLimits(5.0, 16.0, 12.0, 31.416) .setDriverProfile(4.5, 1.0, 5.498, 2.0) - .setPowerProperties(12.0, 80.0, 60.0) + .setPowerProperties(Constants.kVoltage, 80.0, 60.0) .setMechanicalProperties(75.0 / 14.0, 18.75, 0.0, Units.inchesToMeters(4.0)) .setOdometryStd(0.1, 0.1, 0.1) - .setIMU(SwerveIMUs.pigeon2(RobotMap.PIGEON)) - .setPhoenixFeatures(new CANBus(RobotMap.CANBUS), true, true, true) - .setModules(FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT); + .setIMU(SwerveIMUs.canandgyro(RobotMap.kCanandgyro)) + .setPhoenixFeatures(new CANBus(RobotMap.kSwerveCANBus), true, true, true) + .setModules(kFrontLeft, kFrontRight, kBackLeft, kBackRight); private final SwerveAPI api; public Swerve() { - api = new SwerveAPI(CONFIG); + api = new SwerveAPI(kConfig); api.enableTunables("Swerve"); } @@ -80,19 +83,34 @@ public void periodic() { * @param y The Y value from the driver's joystick. * @param angular The CCW+ angular speed to apply, from {@code [-1.0, 1.0]}. */ - public Command drive(Supplier x, Supplier y, Supplier angular) { + public Command drive(DoubleSupplier x, DoubleSupplier y, DoubleSupplier angular) { return commandBuilder("Swerve.drive()").onExecute(() -> - api.applyDriverInput(x.get(), y.get(), angular.get(), ForwardPerspective.OPERATOR, true, true) + api.applyDriverInput( + x.getAsDouble(), + y.getAsDouble(), + angular.getAsDouble(), + Perspective.kOperator, + true, + true + ) ); } + /** + * Drives the modules to stop the robot from moving. + * @param lock If the wheels should be driven to an X formation to stop the robot from being pushed. + */ + public Command stop(boolean lock) { + return commandBuilder("Swerve.stop(" + lock + ")").onExecute(() -> api.applyStop(lock)); + } + /** * Tares the rotation of the robot. Useful for * fixing an out of sync or drifting IMU. */ public Command tareRotation() { return commandBuilder("Swerve.tareRotation()") - .onInitialize(() -> api.tareRotation(ForwardPerspective.OPERATOR)) + .onInitialize(() -> api.tareRotation(Perspective.kOperator)) .isFinished(true) .ignoringDisable(true); } diff --git a/vendordeps/Phoenix6-frc2025-beta-latest.json b/vendordeps/Phoenix6-25.0.0-beta-2.json similarity index 90% rename from vendordeps/Phoenix6-frc2025-beta-latest.json rename to vendordeps/Phoenix6-25.0.0-beta-2.json index 6bc1eac..c56e61a 100644 --- a/vendordeps/Phoenix6-frc2025-beta-latest.json +++ b/vendordeps/Phoenix6-25.0.0-beta-2.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-frc2025-beta-latest.json", + "fileName": "Phoenix6-25.0.0-beta-2.json", "name": "CTRE-Phoenix (v6)", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -24,14 +24,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.0.0-beta-1" + "version": "25.0.0-beta-2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -44,7 +44,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -57,7 +57,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -70,7 +70,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -83,7 +83,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -109,7 +109,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -122,7 +122,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -135,7 +135,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -148,7 +148,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -161,7 +161,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -176,7 +176,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -191,7 +191,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -206,7 +206,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -221,7 +221,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -236,7 +236,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -251,7 +251,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -266,7 +266,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -281,7 +281,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -296,7 +296,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -311,7 +311,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -326,7 +326,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-1", + "version": "25.0.0-beta-2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index b10691d..ee18ef4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,25 +1,25 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.4", + "version": "2025.0.0-beta-2", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.4" + "version": "2025.0.0-beta-2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.0-beta-2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.4", + "version": "2025.0.0-beta-2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.0-beta-2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/ReduxLib_2024.json b/vendordeps/ReduxLib_2025.json similarity index 83% rename from vendordeps/ReduxLib_2024.json rename to vendordeps/ReduxLib_2025.json index ee7264b..bd164ce 100644 --- a/vendordeps/ReduxLib_2024.json +++ b/vendordeps/ReduxLib_2025.json @@ -1,25 +1,25 @@ { - "fileName": "ReduxLib_2024.json", + "fileName": "ReduxLib_2025.json", "name": "ReduxLib", - "version": "2024.3.1", + "version": "2025.0.0-beta0", "frcYear": 2025, "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", "mavenUrls": [ "https://maven.reduxrobotics.com/" ], - "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json", + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", "javaDependencies": [ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-java", - "version": "2024.3.1" + "version": "2025.0.0-beta0" } ], "jniDependencies": [ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-driver", - "version": "2024.3.1", + "version": "2025.0.0-beta0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-cpp", - "version": "2024.3.1", + "version": "2025.0.0-beta0", "libName": "ReduxLib-cpp", "headerClassifier": "headers", "sourcesClassifier": "sources",