From 130abe3a75b8a32891f8b2d5324812645396b9c6 Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Tue, 29 Apr 2025 21:23:53 -0400 Subject: [PATCH 1/7] Maths --- .../org/team340/lib/swerve/SwerveAPI.java | 2 +- src/main/java/org/team340/lib/util/Math2.java | 63 ++++++------------- 2 files changed, 19 insertions(+), 46 deletions(-) diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPI.java b/src/main/java/org/team340/lib/swerve/SwerveAPI.java index 7268d4b..1c86bc1 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveAPI.java +++ b/src/main/java/org/team340/lib/swerve/SwerveAPI.java @@ -364,7 +364,7 @@ public void applySpeeds(ChassisSpeeds speeds, Perspective perspective, boolean d double dtheta = speeds.omegaRadiansPerSecond * config.discretizationPeriod; double sin = -dtheta / 2.0; - double cos = Math2.epsilonEquals(Math.cos(dtheta) - 1.0, 0.0) + double cos = Math.abs(Math.cos(dtheta) - 1.0) < 1e-6 ? 1.0 - ((1.0 / 12.0) * dtheta * dtheta) : (sin * Math.sin(dtheta)) / (Math.cos(dtheta) - 1.0); diff --git a/src/main/java/org/team340/lib/util/Math2.java b/src/main/java/org/team340/lib/util/Math2.java index 0feec29..759795c 100644 --- a/src/main/java/org/team340/lib/util/Math2.java +++ b/src/main/java/org/team340/lib/util/Math2.java @@ -1,10 +1,12 @@ package org.team340.lib.util; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +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; import edu.wpi.first.math.kinematics.SwerveModuleState; -import java.util.Optional; /** * Math utilities. @@ -16,8 +18,6 @@ private Math2() { throw new AssertionError("This is a utility class!"); } - /** Shared maximum accuracy floating point. */ - public static final double kEpsilon = 1e-6; /** {@code PI/6} (30deg) */ public static final double kSixthPi = Math.PI / 6.0; /** {@code PI/4} (45deg) */ @@ -53,53 +53,26 @@ public static double random(double min, double max) { } /** - * Check if two optional values are equal within the accuracy of the default epsilon. - * If one option is empty and the other is present, or both options are present and - * unequal, {@code false} is returned. If both options empty, or both options are - * present and equal, {@code true} is returned. - * @param a The first value to compare. - * @param b The second value to compare. - * @return {@code true} if the values are equal. + * Checks if the distance between two {@link Translation2d}s is within a specified tolerance. + * @param expected The expected {@link Translation2d}. + * @param actual The actual {@link Translation2d}. + * @param tolerance The allowed distance between the actual and the expected translations, in meters. */ - public static boolean epsilonEquals(Optional a, Optional b) { - return epsilonEquals(a, b, kEpsilon); + public static boolean isNear(Translation2d expected, Translation2d actual, double tolerance) { + double dx = expected.getX() - actual.getX(); + double dy = expected.getY() - actual.getY(); + return dx * dx + dy * dy < tolerance * tolerance; } /** - * Check if two optional values are equal within the accuracy of the default epsilon. - * If one option is empty and the other is present, or both options are present and - * unequal, {@code false} is returned. If both options empty, or both options are - * present and equal, {@code true} is returned. - * @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. + * Checks if the angle between two {@link Rotation2d}s is within a specified tolerance. + * @param expected The expected {@link Rotation2d}. + * @param actual The actual {@link Rotation2d}. + * @param tolerance The allowed difference between the actual and the expected rotations, in radians. */ - public static boolean epsilonEquals(Optional a, Optional b, double epsilon) { - if (a == b) return true; - if (a.isEmpty() || b.isEmpty()) return a.isEmpty() && b.isEmpty(); - return epsilonEquals(a.get(), b.get(), epsilon); - } - - /** - * Check if two values 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(double a, double b) { - return epsilonEquals(a, b, kEpsilon); - } - - /** - * Check if two values 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(double a, double b, double epsilon) { - return (a - epsilon <= b) && (a + epsilon >= b); + public static boolean isNear(Rotation2d expected, Rotation2d actual, double tolerance) { + double dot = expected.getCos() * actual.getCos() + expected.getSin() * actual.getSin(); + return Math.acos(MathUtil.clamp(dot, -1.0, 1.0)) < tolerance; } /** From ab70fa78fd0c91ace699c1394dac3023742bd67b Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Tue, 29 Apr 2025 21:28:50 -0400 Subject: [PATCH 2/7] Add swerve default command --- src/main/java/org/team340/robot/Robot.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/org/team340/robot/Robot.java b/src/main/java/org/team340/robot/Robot.java index 8d18d93..3ab41d0 100644 --- a/src/main/java/org/team340/robot/Robot.java +++ b/src/main/java/org/team340/robot/Robot.java @@ -58,6 +58,9 @@ public Robot() { // Create triggers RobotModeTriggers.autonomous().whileTrue(autos.runSelectedAuto()); + // Set default commands + swerve.setDefaultCommand(swerve.drive(this::driverX, this::driverY, this::driverAngular)); + // Driver bindings driver.povLeft().onTrue(swerve.tareRotation()); From 24542b83351863ad1bfa322d3e75580cbb59a804 Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Tue, 29 Apr 2025 21:39:51 -0400 Subject: [PATCH 3/7] Remove zero methods --- .../org/team340/lib/swerve/SwerveAPI.java | 5 +++- src/main/java/org/team340/lib/util/Math2.java | 24 ------------------- 2 files changed, 4 insertions(+), 25 deletions(-) diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPI.java b/src/main/java/org/team340/lib/swerve/SwerveAPI.java index 1c86bc1..47f8eaf 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveAPI.java +++ b/src/main/java/org/team340/lib/swerve/SwerveAPI.java @@ -403,7 +403,10 @@ public void applyStates(SwerveModuleState[] states) { */ public void applyStop(boolean lock) { lastRatelimit = Timer.getFPGATimestamp(); - Math2.zero(state.targetSpeeds); + + state.targetSpeeds.vxMetersPerSecond = 0.0; + state.targetSpeeds.vyMetersPerSecond = 0.0; + state.targetSpeeds.omegaRadiansPerSecond = 0.0; for (int i = 0; i < moduleCount; i++) { SwerveModuleState state; diff --git a/src/main/java/org/team340/lib/util/Math2.java b/src/main/java/org/team340/lib/util/Math2.java index 759795c..b148fb3 100644 --- a/src/main/java/org/team340/lib/util/Math2.java +++ b/src/main/java/org/team340/lib/util/Math2.java @@ -124,28 +124,4 @@ public static SwerveModuleState copyInto(SwerveModuleState source, SwerveModuleS output.angle = source.angle; return output; } - - /** - * 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; - } } From 6cfb66afc2a6478c0c3ecbacfca762e7083a193e Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Tue, 13 May 2025 19:46:24 -0400 Subject: [PATCH 4/7] PAPF and cleanup --- .vscode/settings.json | 3 + .../apriltag/AprilTagFieldLayoutLogger.java | 19 - .../lib/logging/phoenix/CANBusLogger.java | 19 - .../lib/logging/phoenix/CANcoderLogger.java | 12 +- .../lib/logging/phoenix/CANdleLogger.java | 19 + .../lib/logging/phoenix/CANrangeLogger.java | 10 +- .../logging/phoenix/StatusSignalLogger.java | 18 - .../phoenix/controls/CoastOutLogger.java | 19 - .../phoenix/controls/DutyCycleOutLogger.java | 19 - .../DynamicMotionMagicVoltageLogger.java | 19 - .../phoenix/controls/FollowerLogger.java | 19 - .../controls/MotionMagicVoltageLogger.java | 19 - .../controls/PositionVoltageLogger.java | 19 - .../controls/StrictFollowerLogger.java | 19 - .../controls/TorqueCurrentFOCLogger.java | 19 - .../VelocityTorqueCurrentFOCLogger.java | 19 - .../controls/VelocityVoltageLogger.java | 19 - .../phoenix/controls/VoltageOutLogger.java | 19 - .../photonlib/EstimatedRobotPoseLogger.java | 19 - .../logging/photonlib/PhotonCameraLogger.java | 19 - .../photonlib/PhotonPoseEstimatorLogger.java | 19 - .../wpilibj/AddressableLEDBufferLogger.java | 19 - .../logging/wpilibj/AddressableLEDLogger.java | 19 - .../org/team340/lib/math/AllianceFlip.java | 164 ++++++ .../org/team340/lib/{util => math}/Math2.java | 68 +-- .../org/team340/lib/math/PAPFController.java | 472 ++++++++++++++++++ .../org/team340/lib/swerve/Perspective.java | 14 +- .../org/team340/lib/swerve/SwerveAPI.java | 83 ++- .../team340/lib/swerve/SwerveAPILogger.java | 1 + .../org/team340/lib/swerve/SwerveModule.java | 6 +- .../org/team340/lib/swerve/SwerveState.java | 3 + .../lib/swerve/hardware/SwerveEncoders.java | 26 +- .../org/team340/lib/util/AutoChooser.java | 102 ++++ .../java/org/team340/lib/util/Profiler.java | 208 -------- .../java/org/team340/lib/util/Tunable.java | 2 +- .../team340/lib/util/vendors/ReduxUtil.java | 11 +- .../java/org/team340/robot/Constants.java | 35 +- src/main/java/org/team340/robot/Robot.java | 21 +- .../org/team340/robot/commands/Autos.java | 40 +- .../org/team340/robot/subsystems/Swerve.java | 171 ++++--- vendordeps/ChoreoLib-2025.0.3.json | 44 -- vendordeps/Phoenix6-frc2025-latest.json | 90 ++-- vendordeps/photonlib.json | 71 --- 43 files changed, 1071 insertions(+), 965 deletions(-) delete mode 100644 src/main/java/org/team340/lib/logging/apriltag/AprilTagFieldLayoutLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/CANBusLogger.java create mode 100644 src/main/java/org/team340/lib/logging/phoenix/CANdleLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/StatusSignalLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/controls/CoastOutLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/controls/DutyCycleOutLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/controls/DynamicMotionMagicVoltageLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/controls/FollowerLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/controls/MotionMagicVoltageLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/controls/PositionVoltageLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/controls/StrictFollowerLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/controls/TorqueCurrentFOCLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/controls/VelocityTorqueCurrentFOCLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/controls/VelocityVoltageLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/phoenix/controls/VoltageOutLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/photonlib/EstimatedRobotPoseLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/photonlib/PhotonCameraLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/photonlib/PhotonPoseEstimatorLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDBufferLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDLogger.java create mode 100644 src/main/java/org/team340/lib/math/AllianceFlip.java rename src/main/java/org/team340/lib/{util => math}/Math2.java (79%) create mode 100644 src/main/java/org/team340/lib/math/PAPFController.java create mode 100644 src/main/java/org/team340/lib/util/AutoChooser.java delete mode 100644 src/main/java/org/team340/lib/util/Profiler.java delete mode 100644 vendordeps/ChoreoLib-2025.0.3.json delete mode 100644 vendordeps/photonlib.json diff --git a/.vscode/settings.json b/.vscode/settings.json index 7a8a458..3b046a9 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -78,6 +78,7 @@ "Canbus", "CANcoder", "CANdi", + "CANrange", "Checkstyle", "Choreo", "ChoreoLib", @@ -127,6 +128,7 @@ "NTURI", "Odometry", "Overcurrent", + "PAPF", "PIDF", "Powerup", "Protobuf", @@ -156,6 +158,7 @@ "TalonSRX", "Tauri", "Teleop", + "Timestep", "Timesync", "Topicsonly", "Traj", diff --git a/src/main/java/org/team340/lib/logging/apriltag/AprilTagFieldLayoutLogger.java b/src/main/java/org/team340/lib/logging/apriltag/AprilTagFieldLayoutLogger.java deleted file mode 100644 index 1e14a01..0000000 --- a/src/main/java/org/team340/lib/logging/apriltag/AprilTagFieldLayoutLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.apriltag; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(AprilTagFieldLayout.class) -public class AprilTagFieldLayoutLogger extends ClassSpecificLogger { - - public AprilTagFieldLayoutLogger() { - super(AprilTagFieldLayout.class); - } - - @Override - public void update(EpilogueBackend backend, AprilTagFieldLayout fieldLayout) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/CANBusLogger.java b/src/main/java/org/team340/lib/logging/phoenix/CANBusLogger.java deleted file mode 100644 index 0243526..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/CANBusLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix; - -import com.ctre.phoenix6.CANBus; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(CANBus.class) -public class CANBusLogger extends ClassSpecificLogger { - - public CANBusLogger() { - super(CANBus.class); - } - - @Override - public void update(EpilogueBackend backend, CANBus canBus) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java b/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java index b5458cc..a56d696 100644 --- a/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java +++ b/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java @@ -24,13 +24,13 @@ public CANcoderLogger() { } @Override - public void update(EpilogueBackend backend, CANcoder canCoder) { + public void update(EpilogueBackend backend, CANcoder cancoder) { cache - .computeIfAbsent(canCoder, key -> { - var absolutePosition = canCoder.getAbsolutePosition(false); - var magnetHealth = canCoder.getMagnetHealth(false); - var position = canCoder.getPosition(false); - var velocity = canCoder.getVelocity(false); + .computeIfAbsent(cancoder, key -> { + var absolutePosition = cancoder.getAbsolutePosition(false); + var magnetHealth = cancoder.getMagnetHealth(false); + var position = cancoder.getPosition(false); + var velocity = cancoder.getVelocity(false); BaseStatusSignal[] signals = { absolutePosition, magnetHealth, position, velocity }; diff --git a/src/main/java/org/team340/lib/logging/phoenix/CANdleLogger.java b/src/main/java/org/team340/lib/logging/phoenix/CANdleLogger.java new file mode 100644 index 0000000..36ffeaf --- /dev/null +++ b/src/main/java/org/team340/lib/logging/phoenix/CANdleLogger.java @@ -0,0 +1,19 @@ +package org.team340.lib.logging.phoenix; + +import com.ctre.phoenix6.hardware.CANdle; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; + +@CustomLoggerFor(CANdle.class) +public class CANdleLogger extends ClassSpecificLogger { + + public CANdleLogger() { + super(CANdle.class); + } + + @Override + public void update(EpilogueBackend backend, CANdle candle) { + // No-op + } +} diff --git a/src/main/java/org/team340/lib/logging/phoenix/CANrangeLogger.java b/src/main/java/org/team340/lib/logging/phoenix/CANrangeLogger.java index ed21047..cc00672 100644 --- a/src/main/java/org/team340/lib/logging/phoenix/CANrangeLogger.java +++ b/src/main/java/org/team340/lib/logging/phoenix/CANrangeLogger.java @@ -19,12 +19,12 @@ public CANrangeLogger() { } @Override - public void update(EpilogueBackend backend, CANrange canRange) { + public void update(EpilogueBackend backend, CANrange canrange) { cache - .computeIfAbsent(canRange, key -> { - var ambientSignal = canRange.getAmbientSignal(false); - var distance = canRange.getDistance(false); - var isDetected = canRange.getIsDetected(false); + .computeIfAbsent(canrange, key -> { + var ambientSignal = canrange.getAmbientSignal(false); + var distance = canrange.getDistance(false); + var isDetected = canrange.getIsDetected(false); BaseStatusSignal[] signals = { ambientSignal, distance, isDetected }; diff --git a/src/main/java/org/team340/lib/logging/phoenix/StatusSignalLogger.java b/src/main/java/org/team340/lib/logging/phoenix/StatusSignalLogger.java deleted file mode 100644 index 84a12ef..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/StatusSignalLogger.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.team340.lib.logging.phoenix; - -import com.ctre.phoenix6.StatusSignal; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(StatusSignal.class) -public class StatusSignalLogger extends ClassSpecificLogger> { - - @SuppressWarnings({ "unchecked", "rawtypes" }) - public StatusSignalLogger() { - super((Class) StatusSignal.class); - } - - @Override - public void update(EpilogueBackend backend, StatusSignal signal) {} -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/CoastOutLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/CoastOutLogger.java deleted file mode 100644 index 1953e57..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/CoastOutLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.CoastOut; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(CoastOut.class) -public class CoastOutLogger extends ClassSpecificLogger { - - public CoastOutLogger() { - super(CoastOut.class); - } - - @Override - public void update(EpilogueBackend backend, CoastOut control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/DutyCycleOutLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/DutyCycleOutLogger.java deleted file mode 100644 index eda3331..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/DutyCycleOutLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.DutyCycleOut; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(DutyCycleOut.class) -public class DutyCycleOutLogger extends ClassSpecificLogger { - - public DutyCycleOutLogger() { - super(DutyCycleOut.class); - } - - @Override - public void update(EpilogueBackend backend, DutyCycleOut control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/DynamicMotionMagicVoltageLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/DynamicMotionMagicVoltageLogger.java deleted file mode 100644 index 375e056..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/DynamicMotionMagicVoltageLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(DynamicMotionMagicVoltage.class) -public class DynamicMotionMagicVoltageLogger extends ClassSpecificLogger { - - public DynamicMotionMagicVoltageLogger() { - super(DynamicMotionMagicVoltage.class); - } - - @Override - public void update(EpilogueBackend backend, DynamicMotionMagicVoltage control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/FollowerLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/FollowerLogger.java deleted file mode 100644 index 01700ac..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/FollowerLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.Follower; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(Follower.class) -public class FollowerLogger extends ClassSpecificLogger { - - public FollowerLogger() { - super(Follower.class); - } - - @Override - public void update(EpilogueBackend backend, Follower control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/MotionMagicVoltageLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/MotionMagicVoltageLogger.java deleted file mode 100644 index 25077bd..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/MotionMagicVoltageLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(MotionMagicVoltage.class) -public class MotionMagicVoltageLogger extends ClassSpecificLogger { - - public MotionMagicVoltageLogger() { - super(MotionMagicVoltage.class); - } - - @Override - public void update(EpilogueBackend backend, MotionMagicVoltage control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/PositionVoltageLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/PositionVoltageLogger.java deleted file mode 100644 index 02da2e5..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/PositionVoltageLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.PositionVoltage; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(PositionVoltage.class) -public class PositionVoltageLogger extends ClassSpecificLogger { - - public PositionVoltageLogger() { - super(PositionVoltage.class); - } - - @Override - public void update(EpilogueBackend backend, PositionVoltage control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/StrictFollowerLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/StrictFollowerLogger.java deleted file mode 100644 index 23a8120..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/StrictFollowerLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.StrictFollower; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(StrictFollower.class) -public class StrictFollowerLogger extends ClassSpecificLogger { - - public StrictFollowerLogger() { - super(StrictFollower.class); - } - - @Override - public void update(EpilogueBackend backend, StrictFollower control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/TorqueCurrentFOCLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/TorqueCurrentFOCLogger.java deleted file mode 100644 index c7ee95d..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/TorqueCurrentFOCLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(TorqueCurrentFOC.class) -public class TorqueCurrentFOCLogger extends ClassSpecificLogger { - - public TorqueCurrentFOCLogger() { - super(TorqueCurrentFOC.class); - } - - @Override - public void update(EpilogueBackend backend, TorqueCurrentFOC control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityTorqueCurrentFOCLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityTorqueCurrentFOCLogger.java deleted file mode 100644 index ce3bb6e..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityTorqueCurrentFOCLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(VelocityTorqueCurrentFOC.class) -public class VelocityTorqueCurrentFOCLogger extends ClassSpecificLogger { - - public VelocityTorqueCurrentFOCLogger() { - super(VelocityTorqueCurrentFOC.class); - } - - @Override - public void update(EpilogueBackend backend, VelocityTorqueCurrentFOC control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityVoltageLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityVoltageLogger.java deleted file mode 100644 index f28f974..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/VelocityVoltageLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.VelocityVoltage; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(VelocityVoltage.class) -public class VelocityVoltageLogger extends ClassSpecificLogger { - - public VelocityVoltageLogger() { - super(VelocityVoltage.class); - } - - @Override - public void update(EpilogueBackend backend, VelocityVoltage control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/phoenix/controls/VoltageOutLogger.java b/src/main/java/org/team340/lib/logging/phoenix/controls/VoltageOutLogger.java deleted file mode 100644 index d79a619..0000000 --- a/src/main/java/org/team340/lib/logging/phoenix/controls/VoltageOutLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.phoenix.controls; - -import com.ctre.phoenix6.controls.VoltageOut; -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; - -@CustomLoggerFor(VoltageOut.class) -public class VoltageOutLogger extends ClassSpecificLogger { - - public VoltageOutLogger() { - super(VoltageOut.class); - } - - @Override - public void update(EpilogueBackend backend, VoltageOut control) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/photonlib/EstimatedRobotPoseLogger.java b/src/main/java/org/team340/lib/logging/photonlib/EstimatedRobotPoseLogger.java deleted file mode 100644 index 99ddfb7..0000000 --- a/src/main/java/org/team340/lib/logging/photonlib/EstimatedRobotPoseLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.photonlib; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import org.photonvision.EstimatedRobotPose; - -@CustomLoggerFor(EstimatedRobotPose.class) -public class EstimatedRobotPoseLogger extends ClassSpecificLogger { - - public EstimatedRobotPoseLogger() { - super(EstimatedRobotPose.class); - } - - @Override - public void update(EpilogueBackend backend, EstimatedRobotPose estimatedRobotPose) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/photonlib/PhotonCameraLogger.java b/src/main/java/org/team340/lib/logging/photonlib/PhotonCameraLogger.java deleted file mode 100644 index 151cac6..0000000 --- a/src/main/java/org/team340/lib/logging/photonlib/PhotonCameraLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.photonlib; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import org.photonvision.PhotonCamera; - -@CustomLoggerFor(PhotonCamera.class) -public class PhotonCameraLogger extends ClassSpecificLogger { - - public PhotonCameraLogger() { - super(PhotonCamera.class); - } - - @Override - public void update(EpilogueBackend backend, PhotonCamera photonCamera) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/photonlib/PhotonPoseEstimatorLogger.java b/src/main/java/org/team340/lib/logging/photonlib/PhotonPoseEstimatorLogger.java deleted file mode 100644 index 602e4e8..0000000 --- a/src/main/java/org/team340/lib/logging/photonlib/PhotonPoseEstimatorLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.photonlib; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import org.photonvision.PhotonPoseEstimator; - -@CustomLoggerFor(PhotonPoseEstimator.class) -public class PhotonPoseEstimatorLogger extends ClassSpecificLogger { - - public PhotonPoseEstimatorLogger() { - super(PhotonPoseEstimator.class); - } - - @Override - public void update(EpilogueBackend backend, PhotonPoseEstimator photonPoseEstimator) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDBufferLogger.java b/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDBufferLogger.java deleted file mode 100644 index e854ea1..0000000 --- a/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDBufferLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpilibj; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; - -@CustomLoggerFor(AddressableLEDBuffer.class) -public class AddressableLEDBufferLogger extends ClassSpecificLogger { - - public AddressableLEDBufferLogger() { - super(AddressableLEDBuffer.class); - } - - @Override - public void update(EpilogueBackend backend, AddressableLEDBuffer buffer) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDLogger.java b/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDLogger.java deleted file mode 100644 index ab294dd..0000000 --- a/src/main/java/org/team340/lib/logging/wpilibj/AddressableLEDLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpilibj; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.wpilibj.AddressableLED; - -@CustomLoggerFor(AddressableLED.class) -public class AddressableLEDLogger extends ClassSpecificLogger { - - public AddressableLEDLogger() { - super(AddressableLED.class); - } - - @Override - public void update(EpilogueBackend backend, AddressableLED addressableLED) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/math/AllianceFlip.java b/src/main/java/org/team340/lib/math/AllianceFlip.java new file mode 100644 index 0000000..07eac6d --- /dev/null +++ b/src/main/java/org/team340/lib/math/AllianceFlip.java @@ -0,0 +1,164 @@ +package org.team340.lib.math; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.function.Supplier; +import org.team340.lib.util.Alliance; + +/** + * Utility class for flipping field locations based on the robot's alliance. + */ +public final class AllianceFlip { + + private final double fieldLength; + private final double fieldWidth; + private final FlipType flipType; + + /** + * Create the alliance flipper. + * @param fieldLength The length of the field, in meters. + * @param fieldWidth The width of the field, in meters. + * @param flipType The field's symmetry. + */ + public AllianceFlip(double fieldLength, double fieldWidth, FlipType flipType) { + this.fieldLength = fieldLength; + this.fieldWidth = fieldLength; + this.flipType = flipType; + } + + /** + * Flips the provided translation to the opposite alliance. + * @param translation The translation to flip. + */ + public Translation2d flip(Translation2d translation) { + return new Translation2d( + flipType.x(translation.getX(), fieldLength), + flipType.y(translation.getY(), fieldWidth) + ); + } + + /** + * Flips the provided rotation to the opposite alliance. + * @param rotation The rotation to flip. + */ + public Rotation2d flip(Rotation2d rotation) { + return switch (flipType) { + case MIRROR -> new Rotation2d(-rotation.getCos(), rotation.getSin()); + case ROTATE -> new Rotation2d(-rotation.getCos(), -rotation.getSin()); + }; + } + + /** + * Flips the provided pose to the opposite alliance. + * @param pose The pose to flip. + */ + public Pose2d flip(Pose2d pose) { + return new Pose2d(flip(pose.getTranslation()), flip(pose.getRotation())); + } + + /** + * Mirrors the provided translation across the width of the field. + * @param translation The translation to mirror. + */ + public Translation2d mirror(Translation2d translation) { + return new Translation2d(translation.getX(), fieldWidth - translation.getY()); + } + + /** + * Mirrors the provided rotation across the width of the field. + * @param rotation The rotation to mirror. + */ + public Rotation2d mirror(Rotation2d rotation) { + return rotation.unaryMinus(); + } + + /** + * Mirrors the provided pose across the width of the field. + * @param pose The pose to mirror. + */ + public Pose2d mirror(Pose2d pose) { + return new Pose2d(mirror(pose.getTranslation()), mirror(pose.getRotation())); + } + + /** + * Creates a supplier that returns the provided {@link Pose2d}, + * flipped based on the robot's current alliance. + * @param pose The blue origin relative pose. + */ + public Supplier flipped(Pose2d pose) { + return flipped(pose, false); + } + + /** + * Creates a supplier that returns the provided {@link Pose2d}, + * flipped based on the robot's current alliance. + * @param pose The blue origin relative pose. + * @param mirrored If the pose should also be mirrored + * across the width of the field. + */ + public Supplier flipped(Pose2d pose, boolean mirrored) { + Pose2d blue = mirrored ? mirror(pose) : pose; + Pose2d red = flip(pose); + return () -> Alliance.isBlue() ? blue : red; + } + + /** + * Symmetry types for a field. + */ + public static enum FlipType { + MIRROR { + @Override + protected double x(double x, double fieldLength) { + return fieldLength - x; + } + + @Override + protected double y(double y, double fieldWidth) { + return y; + } + + @Override + protected double heading(double heading) { + return Math.PI - heading; + } + }, + + ROTATE { + @Override + protected double x(double x, double fieldLength) { + return fieldLength - x; + } + + @Override + protected double y(double y, double fieldWidth) { + return fieldWidth - y; + } + + @Override + protected double heading(double heading) { + return Math.PI + heading; + } + }; + + /** + * Flips the X coordinate. + * @param x The X coordinate to flip. + * @param fieldLength The field's length. + */ + protected abstract double x(double x, double fieldLength); + + /** + * Flips the Y coordinate. + * @param y The Y coordinate to flip. + * @param fieldWidth the field's width. + */ + protected abstract double y(double y, double fieldWidth); + + /** + * Flips the heading. + * @param heading The heading to flip. + */ + protected abstract double heading(double heading); + } +} diff --git a/src/main/java/org/team340/lib/util/Math2.java b/src/main/java/org/team340/lib/math/Math2.java similarity index 79% rename from src/main/java/org/team340/lib/util/Math2.java rename to src/main/java/org/team340/lib/math/Math2.java index b148fb3..720dc4c 100644 --- a/src/main/java/org/team340/lib/util/Math2.java +++ b/src/main/java/org/team340/lib/math/Math2.java @@ -1,4 +1,4 @@ -package org.team340.lib.util; +package org.team340.lib.math; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; @@ -18,38 +18,31 @@ private Math2() { throw new AssertionError("This is a utility class!"); } - /** {@code PI/6} (30deg) */ - public static final double kSixthPi = Math.PI / 6.0; - /** {@code PI/4} (45deg) */ - public static final double kQuarterPi = Math.PI / 4.0; - /** {@code PI/3} (60deg) */ - public static final double kThirdPi = Math.PI / 3.0; - /** {@code PI/2} (90deg) */ - public static final double kHalfPi = Math.PI / 2.0; - /** {@code 2PI/3} (120deg) */ - public static final double kTwoThirdsPi = (2.0 * Math.PI) / 3.0; - /** {@code 3PI/4} (135deg) */ - public static final double kThreeQuartersPi = (3.0 * Math.PI) / 4.0; - /** {@code 5PI/6} (150deg) */ - public static final double kFiveSixthsPi = (5.0 * Math.PI) / 6.0; - /** {@code PI*2} (360deg) */ - public static final double kTwoPi = Math.PI * 2.0; + public static final double PI2 = 6.283185307179586; /** - * Returns a random double from {@code 0.0} to {@code max}. - * @param max The maximum value to return. + * Discretizes a continuous-time chassis speed in place. + * @param speeds The continuous speeds. + * @param dtSeconds The duration of the timestep the speeds should be applied for. + * @return The provided speeds object. + * @see {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} */ - public static double random(double max) { - return Math.random() * max; - } + public static ChassisSpeeds discretize(ChassisSpeeds speeds, double dtSeconds) { + double dtheta = speeds.omegaRadiansPerSecond * dtSeconds; - /** - * Returns a random double from {@code min} to {@code max}. - * @param min The minimum value to return. - * @param max The maximum value to return. - */ - public static double random(double min, double max) { - return (Math.random() * (max - min)) + min; + double sin = -dtheta / 2.0; + double cos = Math.abs(Math.cos(dtheta) - 1.0) < 1e-6 + ? 1.0 - ((1.0 / 12.0) * dtheta * dtheta) + : (sin * Math.sin(dtheta)) / (Math.cos(dtheta) - 1.0); + + double dt = dtSeconds; + double dx = speeds.vxMetersPerSecond * dt; + double dy = speeds.vyMetersPerSecond * dt; + + speeds.vxMetersPerSecond = ((dx * cos) - (dy * sin)) / dt; + speeds.vyMetersPerSecond = ((dx * sin) + (dy * cos)) / dt; + + return speeds; } /** @@ -75,6 +68,23 @@ public static boolean isNear(Rotation2d expected, Rotation2d actual, double tole return Math.acos(MathUtil.clamp(dot, -1.0, 1.0)) < tolerance; } + /** + * Returns a random double from {@code 0.0} to {@code max}. + * @param max The maximum value to return. + */ + public static double random(double max) { + return Math.random() * max; + } + + /** + * Returns a random double from {@code min} to {@code max}. + * @param min The minimum value to return. + * @param max The maximum value to return. + */ + public static double random(double min, double max) { + return (Math.random() * (max - min)) + min; + } + /** * Copies values from a source {@link Twist2d} object to another. * @param source The twist to copy from. diff --git a/src/main/java/org/team340/lib/math/PAPFController.java b/src/main/java/org/team340/lib/math/PAPFController.java new file mode 100644 index 0000000..a693c70 --- /dev/null +++ b/src/main/java/org/team340/lib/math/PAPFController.java @@ -0,0 +1,472 @@ +package org.team340.lib.math; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.epilogue.NotLogged; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.team340.lib.util.Tunable; + +/** + * Implements a predictive artificial potential field (P-APF), + * capable of real-time motion planning with obstacle avoidance. + * + *

This implementation improves upon traditional APF algorithms by predicting the + * future movement of the robot to determine intermediate setpoints, with the goal of + * achieving smoother motion and preventing oscillation caused by suboptimal interactions + * with potentials generated by obstacles. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class PAPFController { + + private final Obstacle[] obstacles; + private double resolution; + private double horizon; + private boolean savePrediction; + + // spotless:off + + // Setpoint/goal are Pose2d instead of Translation2d + // due to visualization options in AdvantageScope. + @Logged private final List prediction; + @Logged private Pose2d setpoint = Pose2d.kZero; + @Logged private Pose2d goal = Pose2d.kZero; + + // spotless:on + + /** + * Creates a new P-APF controller. + * @param horizon The horizon at which to predict the robot's motion, in meters. + * Larger horizons increase computational cost, with the benefit + * of generally smoother motion. + * @param resolution The resolution at which to sample the horizon in meters. Higher + * resolutions (i.e. smaller values) will increase accuracy of + * predicted motion, however are more computationally expensive. + * @param savePrediction If the robot's predicted motion should be saved. Useful + * for debugging, however does increase GC pressure. + * @param obstacles The field's obstacles. + */ + public PAPFController(double horizon, double resolution, boolean savePrediction, Obstacle[] obstacles) { + this.horizon = horizon; + this.resolution = resolution; + this.savePrediction = savePrediction; + this.obstacles = obstacles; + + prediction = new ArrayList<>((int) Math.ceil(horizon / resolution)); + } + + /** + * Returns the current setpoint of the controller from the last invocation of + * {@link #calculate(Pose2d, Translation2d, double, double) PAPFController.calculate()}. + */ + @NotLogged + public Translation2d getSetpoint() { + return setpoint.getTranslation(); + } + + /** + * Returns the current goal of the controller from the last invocation of + * {@link #calculate(Pose2d, Translation2d, double, double) PAPFController.calculate()}. + */ + @NotLogged + public Translation2d getGoal() { + return goal.getTranslation(); + } + + /** + * Returns the current predicted motion of the robot from the last invocation of + * {@link #calculate(Pose2d, Translation2d, double, double) PAPFController.calculate()}. + * Note that this list will be empty if {@code savePrediction} is {@code false}. + */ + @NotLogged + public List getPrediction() { + return Collections.unmodifiableList(prediction); + } + + /** + * Returns the next velocity output of the potential field. Note + * that while deceleration is accounted for, acceleration from + * rest is not. If acceleration limiting is desired, it must be + * implemented downstream. + * @param currentPose The current robot pose. + * @param goal The desired position of the robot. + * @param maxVelocity The desired cruise velocity of the robot, in m/s. + * @param maxDeceleration The desired deceleration rate of the robot, in m/s/s. + */ + public ChassisSpeeds calculate(Pose2d currentPose, Translation2d goal, double maxVelocity, double maxDeceleration) { + this.goal = new Pose2d(goal, currentPose.getRotation()); + prediction.clear(); + + // To future readers, apologies for the notation crimes. + + double e_x = goal.getX() - currentPose.getX(); + double e_y = goal.getY() - currentPose.getY(); + double error = Math.hypot(e_x, e_y); + + if (error < 1e-6) { + setpoint = this.goal; + return new ChassisSpeeds(); + } + + double rk_x = goal.getX(); + double rk_y = goal.getY(); + + if (horizon > 0.0 && resolution <= error) { + double accum_x = currentPose.getX(); + double accum_y = currentPose.getY(); + NetForce netForce = new NetForce(); + + double d_max = 0.0; + double seg_c = rk_x * currentPose.getY() - rk_y * currentPose.getX(); + for (int i = 0; i < Math.ceil(horizon / resolution); i++) { + netForce.reset(); + forceAt(accum_x, accum_y, goal, netForce); + double norm = Math.hypot(netForce.x, netForce.y); + if (norm < 1e-6) break; + + double alpha = resolution / norm; + accum_x += netForce.x * alpha; + accum_y += netForce.y * alpha; + + if (savePrediction) { + prediction.add(new Translation2d(accum_x, accum_y)); + } + + double d = Math.abs(e_y * accum_x - e_x * accum_y + seg_c) / error; + if (d > resolution && d >= d_max) { + rk_x = accum_x; + rk_y = accum_y; + d_max = d; + } + + double ex_k_n = goal.getX() - accum_x; + double ey_k_n = goal.getY() - accum_y; + if (ex_k_n * ex_k_n + ey_k_n * ey_k_n <= resolution * resolution) { + break; + } + } + } + + setpoint = new Pose2d(rk_x, rk_y, currentPose.getRotation()); + + NetForce force = new NetForce(); + forceAt(currentPose.getX(), currentPose.getY(), setpoint.getTranslation(), force); + + if (error < (maxVelocity * maxVelocity) / (2.0 * maxDeceleration)) { + maxVelocity = Math.sqrt(2.0 * maxDeceleration * error); + } + + double norm = Math.hypot(force.x, force.y); + return norm > 1e-6 + ? new ChassisSpeeds(maxVelocity * (force.x / norm), maxVelocity * (force.y / norm), 0.0) + : new ChassisSpeeds(); + } + + /** + * Applies the the force generated by all obstacles as well as the + * attraction potential of the goal to the provided accumulator, + * based on the robot's current position and goal. + * @param x The robot's current X position on the field. + * @param y The robot's current Y position on the field. + * @param goal The current target field location. + * @param netForce The force accumulator to apply the force to. + */ + private void forceAt(double x, double y, Translation2d goal, NetForce netForce) { + double e_x = goal.getX() - x; + double e_y = goal.getY() - y; + double error = Math.hypot(e_x, e_y); + + if (error < 1e-6) return; + + double targetForce = 1.0 + (2.0 / error); + netForce.x += targetForce * (e_x / error); + netForce.y += targetForce * (e_y / error); + + for (Obstacle obstacle : obstacles) { + obstacle.applyForce(x, y, goal, netForce); + } + } + + /** + * Enables publishing tunables for adjustment of the controller's parameters. + * @param name The parent name for the tunables in NetworkTables. + */ + public void enableTunables(String name) { + Tunable.doubleValue(name + "/horizon", horizon, v -> horizon = v); + Tunable.doubleValue(name + "/resolution", resolution, v -> resolution = v); + Tunable.booleanValue(name + "/savePrediction", savePrediction, v -> savePrediction = v); + } + + /** + * Returns a list of samples from the potential field for visualization in + * AdvantageScope. Represented as {@link Pose2d}s, the translation of the + * pose is the sample location, and the rotation is the direction of force + * at the sample. Useful for debugging purposes, but is not recommended for + * regular use due to performance implications. + * @param rows The number of rows (X+ direction) to sample. Columns are sampled + * relative to the ratio of {@code fieldWidth / fieldLength}. + * @param padding The distance past the field perimeter to sample in meters. + * @param fieldLength The length of the field in meters. + * @param fieldWidth The width of the field in meters. + */ + public List visualizeField(int rows, double padding, double fieldLength, double fieldWidth) { + fieldLength += (padding * 2.0); + fieldWidth += (padding * 2.0); + + List field = new ArrayList<>(); + NetForce sample = new NetForce(); + int columns = (int) Math.ceil(rows * (fieldWidth / fieldLength)); + for (double x = -padding; x < fieldLength + 1e-3; x += fieldLength / rows) { + for (double y = -padding; y < fieldWidth + 1e-3; y += fieldWidth / columns) { + sample.reset(); + forceAt(x, y, setpoint.getTranslation(), sample); + if (sample.x * sample.x + sample.y * sample.y > 1e-6) { + field.add(new Pose2d(x, y, new Rotation2d(sample.x, sample.y))); + } + } + } + + return field; + } + + /** + * Represents a sum of forces. + */ + private static final class NetForce { + + // TODO (2027 MRC) + // NetForce exists as an optimization to reduce GC pressure, over + // the alternative of returning immutable container objects. With + // the release of the 2027 MRC, this will not be necessary. + + public double x = 0.0; + public double y = 0.0; + + public void reset() { + x = 0.0; + y = 0.0; + } + } + + /** + * A generic obstacle. + */ + public abstract static class Obstacle { + + private final double strength; + private final boolean repulsive; + + /** + * Creates an obstacle. + * @param strength The strength of the obstacle's force. + * @param repulsive If the obstacle is a repulsive potential (pushes away). If + * {@code false}, the obstacle will behave as an attractive + * potential (pulls towards). + */ + public Obstacle(double strength, boolean repulsive) { + this.strength = strength; + this.repulsive = repulsive; + } + + /** + * Applies the force generated by the obstacle to the provided + * accumulator, based on the robot's current position and goal. + * @param x The robot's current X position on the field. + * @param y The robot's current Y position on the field. + * @param goal The current target field location. + * @param netForce The force accumulator to apply the force to. + */ + public abstract void applyForce(double x, double y, Translation2d goal, NetForce netForce); + + /** + * Converts a distance from the obstacle to the strength of the force. + * @param dist The distance from the obstacle. + */ + protected final double getForceMagnitude(double dist) { + double dist_2 = Math.max(1e-6, dist * dist); + return (strength / dist_2) * (repulsive ? 1.0 : -1.0); + } + } + + /** + * A line segment that pushes perpendicular from its length. + */ + public static final class LineObstacle extends Obstacle { + + private final Translation2d start; + private final Translation2d end; + + private final double length; + private final Rotation2d inverse; + private final Rotation2d perpendicular; + + /** + * Creates a line obstacle. + * @param start The start of the line segment. + * @param end The end of the line segment. + * @param strength The strength of the obstacle's force. + * @param repulsive If the obstacle is a repulsive potential (pushes away). If + * {@code false}, the obstacle will behave as an attractive + * potential (pulls towards). + */ + public LineObstacle(Translation2d start, Translation2d end, double strength, boolean repulsive) { + super(strength, repulsive); + this.start = start; + this.end = end; + + Translation2d difference = end.minus(start); + length = difference.getNorm(); + inverse = difference.getAngle().unaryMinus(); + perpendicular = difference.getAngle().rotateBy(Rotation2d.kCCW_Pi_2); + } + + @Override + public void applyForce(double x, double y, Translation2d target, NetForce netForce) { + double startDist_x = x - start.getX(); + double startDist_y = y - start.getY(); + + double proj_x = startDist_x * inverse.getCos() - startDist_y * inverse.getSin(); + double proj_y = startDist_x * inverse.getSin() + startDist_y * inverse.getCos(); + + if (proj_x > 0.0 && proj_x < length) { + double magnitude = Math.copySign(getForceMagnitude(proj_y), proj_y); + netForce.x += magnitude * perpendicular.getCos(); + netForce.y += magnitude * perpendicular.getSin(); + } else { + Translation2d closest = proj_x <= 0.0 ? start : end; + double d_x = x - closest.getX(); + double d_y = y - closest.getY(); + double d_norm = Math.max(1e-6, Math.hypot(d_x, d_y)); + double magnitude = getForceMagnitude(d_norm); + + netForce.x += magnitude * (d_x / d_norm); + netForce.y += magnitude * (d_y / d_norm); + } + } + } + + /** + * A simple point obstacle. Applies a force from the specified field location. + */ + public static final class PointObstacle extends Obstacle { + + private final Translation2d location; + + /** + * Creates a point obstacle. + * @param location The point's location on the field. + * @param strength The strength of the obstacle's force. + * @param repulsive If the obstacle is a repulsive potential (pushes away). If + * {@code false}, the obstacle will behave as an attractive + * potential (pulls towards). + */ + public PointObstacle(Translation2d location, double strength, boolean repulsive) { + super(strength, repulsive); + this.location = location; + } + + @Override + public void applyForce(double x, double y, Translation2d goal, NetForce netForce) { + double d_x = x - location.getX(); + double d_y = y - location.getY(); + double d_norm = Math.max(1e-6, Math.hypot(d_x, d_y)); + double force = getForceMagnitude(d_norm); + + netForce.x += force * (d_x / d_norm); + netForce.y += force * (d_y / d_norm); + } + } + + /** + * A circle with a specified radius. + */ + public static final class CircleObstacle extends Obstacle { + + private final Translation2d location; + private final double radius; + + /** + * Creates a circle obstacle. + * @param location The location of the circle's center. + * @param radius The radius of the circle. + * @param strength The strength of the obstacle's force. + */ + public CircleObstacle(Translation2d location, double radius, double strength) { + super(strength, true); + this.location = location; + this.radius = radius; + } + + @Override + public void applyForce(double x, double y, Translation2d goal, NetForce netForce) { + double d_x = x - location.getX(); + double d_y = y - location.getY(); + double d_norm = Math.max(1e-6, Math.hypot(d_x, d_y)); + double force = getForceMagnitude(Math.max(0.0, d_norm - radius)); + + netForce.x += force * (d_x / d_norm); + netForce.y += force * (d_y / d_norm); + } + } + + /** + * An infinite line that pushes parallel to the X axis, to + * constrain forwards/backwards field-relative movement. + */ + public static final class LongitudinalObstacle extends Obstacle { + + private final double x; + + /** + * Creates a longitudinal obstacle. + * @param x The X coordinate of the line. + * @param strength The strength of the obstacle's force. + * @param repulsive If the obstacle is a repulsive potential (pushes away). If + * {@code false}, the obstacle will behave as an attractive + * potential (pulls towards). + */ + public LongitudinalObstacle(double x, double strength, boolean repulsive) { + super(strength, repulsive); + this.x = x; + } + + @Override + public void applyForce(double x, double y, Translation2d target, NetForce netForce) { + double difference = x - this.x; + netForce.x += Math.copySign(getForceMagnitude(difference), difference); + } + } + + /** + * An infinite line that pushes parallel to the Y axis, to + * constrain left/right field-relative movement. + */ + public static final class LateralObstacle extends Obstacle { + + private final double y; + + /** + * Creates a lateral obstacle. + * @param y The Y coordinate of the line. + * @param strength The strength of the obstacle's force. + * @param repulsive If the obstacle is a repulsive potential (pushes away). If + * {@code false}, the obstacle will behave as an attractive + * potential (pulls towards). + */ + public LateralObstacle(double y, double strength, boolean repulsive) { + super(strength, repulsive); + this.y = y; + } + + @Override + public void applyForce(double x, double y, Translation2d target, NetForce netForce) { + double difference = y - this.y; + netForce.y += Math.copySign(getForceMagnitude(difference), difference); + } + } +} diff --git a/src/main/java/org/team340/lib/swerve/Perspective.java b/src/main/java/org/team340/lib/swerve/Perspective.java index 0f3d281..ad855ba 100644 --- a/src/main/java/org/team340/lib/swerve/Perspective.java +++ b/src/main/java/org/team340/lib/swerve/Perspective.java @@ -13,20 +13,20 @@ public enum Perspective { * 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. */ - kOperator { + OPERATOR { @Override public ChassisSpeeds toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { - return (Alliance.isBlue() ? kBlue : kRed).toRobotSpeeds(speeds, robotAngle); + return (Alliance.isBlue() ? BLUE : RED).toRobotSpeeds(speeds, robotAngle); } @Override public ChassisSpeeds toPerspectiveSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { - return (Alliance.isBlue() ? kBlue : kRed).toPerspectiveSpeeds(speeds, robotAngle); + return (Alliance.isBlue() ? BLUE : RED).toPerspectiveSpeeds(speeds, robotAngle); } @Override public Rotation2d getTareRotation() { - return (Alliance.isBlue() ? kBlue : kRed).getTareRotation(); + return (Alliance.isBlue() ? BLUE : RED).getTareRotation(); } }, @@ -34,7 +34,7 @@ public Rotation2d getTareRotation() { * The speeds are relative to the blue alliance perspective. * X+ drives towards the red alliance. */ - kBlue { + BLUE { @Override public ChassisSpeeds toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { return ChassisSpeeds.fromFieldRelativeSpeeds(speeds, robotAngle); @@ -55,7 +55,7 @@ public Rotation2d getTareRotation() { * The speeds are relative to the red alliance perspective. * X+ drives towards the blue alliance. */ - kRed { + RED { @Override public ChassisSpeeds toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { return ChassisSpeeds.fromFieldRelativeSpeeds(speeds, robotAngle.rotateBy(Rotation2d.kPi)); @@ -76,7 +76,7 @@ public Rotation2d getTareRotation() { * The speeds are relative to the robot's perspective. * X+ drives forwards relative to the chassis. */ - kRobot { + ROBOT { @Override public ChassisSpeeds toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { return speeds; diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPI.java b/src/main/java/org/team340/lib/swerve/SwerveAPI.java index 47f8eaf..cd24e24 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveAPI.java +++ b/src/main/java/org/team340/lib/swerve/SwerveAPI.java @@ -1,6 +1,7 @@ package org.team340.lib.swerve; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusCode; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Strategy; @@ -18,6 +19,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj.Timer; import java.util.ArrayList; @@ -26,9 +28,9 @@ import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.Consumer; +import org.team340.lib.math.Math2; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.hardware.SwerveIMUs.SwerveIMU; -import org.team340.lib.util.Math2; import org.team340.lib.util.Sleep; import org.team340.robot.Robot; @@ -67,6 +69,8 @@ public class SwerveAPI implements AutoCloseable { public SwerveAPI(SwerveConfig config) { this.config = config; + if (RobotBase.isSimulation()) config.phoenixCanBus = new CANBus(); + moduleCount = config.modules.length; modules = new SwerveModule[moduleCount]; moduleLocations = new Translation2d[moduleCount]; @@ -120,6 +124,7 @@ public void refresh() { } state.pose = poseEstimator.getEstimatedPosition(); + state.odometryPose = odometry.getPoseMeters(); state.poseHistory.clear(); state.poseHistory.addAll(odometryThread.poseHistory); @@ -186,8 +191,8 @@ public void resetPose(Pose2d pose) { /** * 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. + * IMU. For most cases, a perspective of {@link Perspective#OPERATOR} is + * desirable. {@link Perspective#ROBOT} will no-op. * @param perspective The perspective to tare the rotation to. */ public void tareRotation(Perspective perspective) { @@ -327,57 +332,45 @@ public void applySpeeds(ChassisSpeeds speeds, Perspective perspective, boolean d if (ratelimit) { double now = Timer.getFPGATimestamp(); - if (now - lastRatelimit < config.period * 2.0) { - ChassisSpeeds lastSpeeds = perspective.toPerspectiveSpeeds(state.targetSpeeds, lastRobotAngle); - - double vx_l = lastSpeeds.vxMetersPerSecond; - double vy_l = lastSpeeds.vyMetersPerSecond; - double v_l = Math.hypot(vx_l, vy_l); - double w_l = lastSpeeds.omegaRadiansPerSecond; - - double dx = speeds.vxMetersPerSecond - vx_l; - double dy = speeds.vyMetersPerSecond - vy_l; - double a_slip = config.slipAccel * config.period; - if (dx * dx + dy * dy > a_slip * a_slip) { - double k = a_slip / Math.hypot(dx, dy); - speeds.vxMetersPerSecond = vx_l + (k * dx); - speeds.vyMetersPerSecond = vy_l + (k * dy); - } - - double v = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - double a_torque = (config.torqueAccel * config.period) * (1.0 - (v_l / config.velocity)); - if (v - v_l > a_torque) { - double k = (v_l + a_torque) / v; - speeds.vxMetersPerSecond *= k; - speeds.vyMetersPerSecond *= k; - } + ChassisSpeeds lastSpeeds = now - lastRatelimit < config.period * 4.0 + ? perspective.toPerspectiveSpeeds(state.targetSpeeds, lastRobotAngle) + : perspective.toPerspectiveSpeeds(state.speeds, state.rotation); + + double vx_l = lastSpeeds.vxMetersPerSecond; + double vy_l = lastSpeeds.vyMetersPerSecond; + double v_l = Math.hypot(vx_l, vy_l); + double w_l = lastSpeeds.omegaRadiansPerSecond; + + double dx = speeds.vxMetersPerSecond - vx_l; + double dy = speeds.vyMetersPerSecond - vy_l; + double a_slip = config.slipAccel * config.period; + if (dx * dx + dy * dy > a_slip * a_slip) { + double k = a_slip / Math.hypot(dx, dy); + speeds.vxMetersPerSecond = vx_l + (k * dx); + speeds.vyMetersPerSecond = vy_l + (k * dy); + } - double dw = speeds.omegaRadiansPerSecond - w_l; - double a_angular = config.angularAccel * config.period; - speeds.omegaRadiansPerSecond = w_l + (Math.min(a_angular / Math.abs(dw), 1.0) * dw); + double v = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + double a_torque = (config.torqueAccel * config.period) * (1.0 - (v_l / config.velocity)); + if (v - v_l > a_torque && v > 1e-6) { + double k = (v_l + a_torque) / v; + speeds.vxMetersPerSecond *= k; + speeds.vyMetersPerSecond *= k; } + double dw = speeds.omegaRadiansPerSecond - w_l; + double a_angular = config.angularAccel * config.period; + speeds.omegaRadiansPerSecond = w_l + (Math.min(a_angular / Math.abs(dw), 1.0) * dw); + lastRatelimit = now; } if (discretize) { - double dtheta = speeds.omegaRadiansPerSecond * config.discretizationPeriod; - - double sin = -dtheta / 2.0; - double cos = Math.abs(Math.cos(dtheta) - 1.0) < 1e-6 - ? 1.0 - ((1.0 / 12.0) * dtheta * dtheta) - : (sin * Math.sin(dtheta)) / (Math.cos(dtheta) - 1.0); - - double dt = config.period; - double dx = speeds.vxMetersPerSecond * dt; - double dy = speeds.vyMetersPerSecond * dt; - - speeds.vxMetersPerSecond = ((dx * cos) - (dy * sin)) / dt; - speeds.vyMetersPerSecond = ((dx * sin) + (dy * cos)) / dt; + Math2.discretize(speeds, config.discretizationPeriod); } - lastRobotAngle = state.pose.getRotation(); - Math2.copyInto(perspective.toRobotSpeeds(speeds, state.pose.getRotation()), state.targetSpeeds); + lastRobotAngle = state.rotation; + Math2.copyInto(perspective.toRobotSpeeds(speeds, state.rotation), state.targetSpeeds); applyStates(kinematics.toSwerveModuleStates(state.targetSpeeds)); } diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java b/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java index c827123..3251e9d 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java +++ b/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java @@ -39,6 +39,7 @@ private void logState(EpilogueBackend backend, SwerveState state) { backend.log("pose", state.pose, Pose2d.struct); backend.log("pitch", state.pitch, Rotation2d.struct); backend.log("roll", state.roll, Rotation2d.struct); + backend.log("odometryPose", state.odometryPose, Pose2d.struct); var modules = backend.getNested("modules"); modules.log("positions", state.modules.positions, SwerveModulePosition.struct); diff --git a/src/main/java/org/team340/lib/swerve/SwerveModule.java b/src/main/java/org/team340/lib/swerve/SwerveModule.java index e6b5e2b..fb8f89d 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveModule.java +++ b/src/main/java/org/team340/lib/swerve/SwerveModule.java @@ -10,12 +10,12 @@ import java.util.List; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import org.team340.lib.math.Math2; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; import org.team340.lib.swerve.hardware.SwerveEncoders.SwerveEncoder; import org.team340.lib.swerve.hardware.SwerveEncoders.SwerveEncoder.HookStatus; import org.team340.lib.swerve.hardware.SwerveMotors.SwerveMotor; -import org.team340.lib.util.Math2; /** * An encapsulation of all hardware for a swerve module. @@ -75,7 +75,7 @@ public List getSignals() { /** * Refreshes the cached position and state of the module. - * @return {@code true} on success, {@code false} if a read error ocurred. + * @return {@code true} on success, {@code false} if a read error occurred. */ public boolean refresh() { double turnPosition = turnMotor.getPosition(); @@ -155,7 +155,7 @@ public void applyState(SwerveModuleState state) { } boolean flipped = false; - if (Math.abs(angleDelta.getRadians()) > Math2.kHalfPi) { + if (Math.abs(angleDelta.getRadians()) > Math.PI / 2.0) { 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 5ca9cbf..0311198 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveState.java +++ b/src/main/java/org/team340/lib/swerve/SwerveState.java @@ -85,6 +85,8 @@ private OdometryThread() {} public Rotation2d pitch; /** The robot's roll. */ public Rotation2d roll; + /** The uncorrected blue origin relative odometry pose of the robot. */ + public Pose2d odometryPose; /** The timestamp of the swerve state in seconds (FPGA time). */ public double timestamp; @@ -99,5 +101,6 @@ private OdometryThread() {} rotation = Rotation2d.kZero; pitch = Rotation2d.kZero; roll = Rotation2d.kZero; + odometryPose = Pose2d.kZero; } } 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 7a7c812..926d1d0 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java @@ -268,28 +268,28 @@ public void close() { * @param offset Offset of the magnet in rotations. * @param inverted If the encoder is inverted. */ - public static SwerveEncoder.Ctor canCoder(int id, double offset, boolean inverted) { + public static SwerveEncoder.Ctor cancoder(int id, double offset, boolean inverted) { return (config, turnMotor) -> { var deviceLogger = new CANcoderLogger(); - CANcoder canCoder = new CANcoder(id, config.phoenixCanBus); + CANcoder cancoder = new CANcoder(id, config.phoenixCanBus); HookStatus tempHookStatus = new HookStatus(false, false); - StatusSignal position = canCoder.getPosition().clone(); - StatusSignal velocity = canCoder.getVelocity().clone(); + StatusSignal position = cancoder.getPosition().clone(); + StatusSignal velocity = cancoder.getVelocity().clone(); - var canCoderConfig = new CANcoderConfiguration(); - canCoderConfig.MagnetSensor.MagnetOffset = offset; - canCoderConfig.MagnetSensor.SensorDirection = inverted + var cancoderConfig = new CANcoderConfiguration(); + cancoderConfig.MagnetSensor.MagnetOffset = offset; + cancoderConfig.MagnetSensor.SensorDirection = inverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; - PhoenixUtil.run("Clear Sticky Faults", () -> canCoder.clearStickyFaults()); - PhoenixUtil.run("Apply CANcoderConfiguration", () -> canCoder.getConfigurator().apply(canCoderConfig)); + PhoenixUtil.run("Clear Sticky Faults", () -> cancoder.clearStickyFaults()); + PhoenixUtil.run("Apply CANcoderConfiguration", () -> cancoder.getConfigurator().apply(cancoderConfig)); PhoenixUtil.run("Set Update Frequency", () -> BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.odometryPeriod, position, velocity) ); PhoenixUtil.run("Optimize Bus Utilization", () -> - canCoder.optimizeBusUtilization(1.0 / config.defaultFramePeriod, 0.05) + cancoder.optimizeBusUtilization(1.0 / config.defaultFramePeriod, 0.05) ); if (turnMotor.getAPI() instanceof TalonFX talonFX) { @@ -329,12 +329,12 @@ public HookStatus hookStatus() { @Override public Object getAPI() { - return canCoder; + return cancoder; } @Override public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, canCoder, errorHandler); + deviceLogger.tryUpdate(backend, cancoder, errorHandler); } @Override @@ -344,7 +344,7 @@ public List getSignals() { @Override public void close() { - canCoder.close(); + cancoder.close(); } }; }; diff --git a/src/main/java/org/team340/lib/util/AutoChooser.java b/src/main/java/org/team340/lib/util/AutoChooser.java new file mode 100644 index 0000000..990939d --- /dev/null +++ b/src/main/java/org/team340/lib/util/AutoChooser.java @@ -0,0 +1,102 @@ +package org.team340.lib.util; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringArrayPublisher; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringSubscriber; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import java.util.HashMap; +import java.util.Map; + +/** + * Similar to {@link SendableChooser}, this class implements a selector over + * NetworkTables for choosing a command to run during the autonomous period. + * + *

For this class to function, you must either bind it to a command + * scheduler using {@link AutoChooser#bind(CommandScheduler)}, or call + * {@link AutoChooser#update()} periodically. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class AutoChooser { + + private static final String DEFAULT = "Do Nothing"; + + private final StringArrayPublisher optionsPub; + private final StringPublisher activePub; + private final StringSubscriber selectedSub; + + private final Map options = new HashMap<>(); + + private String activeName = DEFAULT; + private Command activeCommand = Commands.none(); + + /** + * Create the auto chooser. + * @param name The name for the chooser in NetworkTables. + */ + public AutoChooser(String name) { + NetworkTable nt = NetworkTableInstance.getDefault().getTable(name); + + nt.getStringTopic(".type").publish().set("String Chooser"); + nt.getBooleanTopic(".controllable").publish().set(true); + nt.getStringTopic("default").publish().set(DEFAULT); + + optionsPub = nt.getStringArrayTopic("options").publish(); + activePub = nt.getStringTopic("active").publish(); + selectedSub = nt.getStringTopic("selected").subscribe("Do Nothing"); + + add(activeName, activeCommand); + activePub.set(activeName); + } + + /** + * Returns the command of the currently selected auto. + */ + public Command getSelected() { + return activeCommand; + } + + /** + * Add an option to the chooser. + * @param name The name of the option. Must be unique. + * @param command The option's command. + */ + public void add(String name, Command command) { + options.put(name, command); + optionsPub.set(options.keySet().toArray(String[]::new)); + } + + /** + * Binds the auto chooser to the default button loop of the provided command + * scheduler. This will automatically update the chooser, as well as schedule + * the selected command during the autonomous period. + * @param scheduler The command scheduler to bind to. + */ + public void bind(CommandScheduler scheduler) { + scheduler.getDefaultButtonLoop().bind(this::update); + new Trigger(scheduler.getDefaultButtonLoop(), DriverStation::isAutonomousEnabled).whileTrue( + Commands.deferredProxy(() -> activeCommand) + ); + } + + /** + * Updates the auto chooser. You do not need to call this method + * when using {@link AutoChooser#bind(CommandScheduler)}. + */ + public void update() { + String selected = selectedSub.get(); + if (!selected.equals(activeName)) { + activeName = options.containsKey(selected) ? selected : DEFAULT; + activeCommand = options.get(activeName); + activePub.set(activeName); + } + } +} diff --git a/src/main/java/org/team340/lib/util/Profiler.java b/src/main/java/org/team340/lib/util/Profiler.java deleted file mode 100644 index f9225d4..0000000 --- a/src/main/java/org/team340/lib/util/Profiler.java +++ /dev/null @@ -1,208 +0,0 @@ -package org.team340.lib.util; - -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; -import java.lang.management.GarbageCollectorMXBean; -import java.lang.management.ManagementFactory; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.function.Supplier; - -/** - * A simple pseudo call-graph profiler that publishes timings to NetworkTables. - * - *

Data is published to {@code /Profiling}, with CPU time utilized by the garbage - * collector published at {@code /Profiling/GC}, and time spent publishing values to - * NT at {@code /Profiling/Overhead}. All timings are recorded in milliseconds, - * as the duration of execution. The profiler records the time to execute blocks - * of code, with support for nesting execution times which are reflected in an - * expanding NT tree. - * - *

All invocations of {@link Profiler#start(String)} are expected to be - * followed by a closing {@link Profiler#end()}. Additionally, it is expected - * that a single {@link Profiler#start(String)}/{@link Profiler#end()} pair is - * found at the highest level of the robot's code (i.e. {@code robotPeriodic}), - * with no other "root" pairs. If either of these limitations are left unsatisfied, - * an error will be printed to the Driver Station. Profiling across threads is also - * not supported. - */ -public final class Profiler { - - private Profiler() { - throw new AssertionError("This is a utility class!"); - } - - private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("Profiling"); - - private static final Map callGraph = new HashMap<>(); - private static final List stack = new ArrayList<>(); - - private static final List gcList = ManagementFactory.getGarbageCollectorMXBeans(); - private static final DoublePublisher overheadPub = nt.getDoubleTopic("overhead").publish(); - private static final DoublePublisher gcPub = nt.getDoubleTopic("gc").publish(); - - private static String root = ""; - private static long lastGCTime = 0; - - /** - * Determines if the profiler is currently running. - * @return {@code true} if the profiler is running. - */ - public static boolean isRunning() { - return stack.size() > 0; - } - - /** - * Profiles a section of user code. Serves as a shorthand for calling - * {@link Profiler#start(String)}, {user code}, {@link Profiler#end()}. - * @param name The name of the call. Must be unique. - * @param runnable The user code to be ran. - */ - public static void run(String name, Runnable runnable) { - start(name); - runnable.run(); - end(); - } - - /** - * Profiles a section of user code. Serves as a shorthand for calling - * {@link Profiler#start(String)}, {user code}, {@link Profiler#end()}. - * @param The return type of the supplier. - * @param name The name of the call. Must be unique. - * @param runnable The user code to be ran. - * @return The value returned from the supplier. - */ - public static T run(String name, Supplier supplier) { - start(name); - T v = supplier.get(); - end(); - return v; - } - - /** - * Starts to profile a section of user code. This method should be invoked - * before the user code to be profiled, with {@link Profiler#end()} expected - * to be after. Additional {@link Profiler#start(String)}/{@link Profiler#end()} - * pairs can be nested within the section of user code, and will appear as a - * sub-topic in NetworkTables. Timings of nested pairs are still included in - * the overall time recorded by the parent pair. - * @param name The name of the call. Must be unique. - */ - public static void start(String name) { - if (stack.isEmpty()) { - if (root.isEmpty()) { - root = name; - } else if (name != root) { - DriverStation.reportError( - "[Profiler] Unexpected secondary root with name \"" + - name + - "\", expected primary root \"" + - root + - "\"", - true - ); - } - } - - stack.add(name); - String fullName = String.join("/", stack); - CallData call = callGraph.get(fullName); - if (call == null) { - call = new CallData(fullName); - callGraph.put(fullName, call); - } - - call.onStart(); - } - - /** - * Ends profiling a section of code. This method should be invoked - * after the user code to be profiled. Trailing calls to this method - * will result in an error printed to the Driver Station. - */ - public static void end() { - CallData call = callGraph.get(String.join("/", stack)); - if (call != null) { - call.onEnd(); - stack.remove(stack.size() - 1); - - if (stack.isEmpty()) { - long start = RobotController.getFPGATime(); - var iter = callGraph.entrySet().iterator(); - while (iter.hasNext()) { - CallData entryCall = iter.next().getValue(); - if (!entryCall.done) { - entryCall.close(); - iter.remove(); - continue; - } - - entryCall.pubAndReset(start); - } - - long gcSum = 0; - for (var gc : gcList) { - long gcTime = gc.getCollectionTime(); - if (gcTime != -1) gcSum += gcTime; - } - gcPub.set(gcSum - lastGCTime); - lastGCTime = gcSum; - - overheadPub.set((RobotController.getFPGATime() - start) / 1000.0); - } - } else { - DriverStation.reportError("[Profiler] Unexpected end() call", true); - } - } - - /** - * Manages the data of a call. - */ - private static final class CallData implements AutoCloseable { - - public final String fullName; - public long time = -1; - public boolean done = false; - public DoublePublisher pub; - - public CallData(String fullName) { - this.fullName = fullName; - } - - /** - * Should be invoked when a call starts. - */ - public void onStart() { - time = RobotController.getFPGATime(); - } - - /** - * Should be invoked when a call ends. - */ - public void onEnd() { - time = RobotController.getFPGATime() - time; - done = true; - } - - /** - * Publishes the last timing to NT and resets state. - * @param timestamp The timestamp to publish the NT value to. - */ - public void pubAndReset(long timestamp) { - if (pub == null) pub = nt.getDoubleTopic(fullName).publish(); - pub.set(time / 1000.0, timestamp); - time = -1; - done = false; - } - - @Override - public void close() { - pub.close(); - } - } -} diff --git a/src/main/java/org/team340/lib/util/Tunable.java b/src/main/java/org/team340/lib/util/Tunable.java index a860576..6f78bfd 100644 --- a/src/main/java/org/team340/lib/util/Tunable.java +++ b/src/main/java/org/team340/lib/util/Tunable.java @@ -40,7 +40,7 @@ private Tunable() { throw new AssertionError("This is a utility class!"); } - private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("Tunables"); + private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("/Tunables"); private static final EventLoop pollChanges = new EventLoop(); /** diff --git a/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java b/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java index 503e877..931551e 100644 --- a/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java +++ b/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java @@ -10,6 +10,7 @@ import com.reduxrobotics.sensors.canandmag.CanandmagSettings; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import java.util.Map; @@ -41,10 +42,11 @@ public static boolean applySettings(Canandmag canandmag, CanandmagSettings setti * @return {@code true} if success, {@code false} otherwise. */ public static boolean applySettings(Canandmag canandmag, CanandmagSettings settings, int maxTries) { + if (RobotBase.isSimulation()) return true; + String results = ""; for (int i = 0; i < maxTries; i++) { - boolean result = canandmag.setSettings(settings, 0.05); - if (!result) { + if (!canandmag.setSettings(settings, 0.05)) { results += (results.isEmpty() ? "" : ", ") + "setSettings() Failure"; continue; } @@ -88,10 +90,11 @@ public static boolean applySettings(Canandgyro canandgyro, CanandgyroSettings se * @return {@code true} if success, {@code false} otherwise. */ public static boolean applySettings(Canandgyro canandgyro, CanandgyroSettings settings, int maxTries) { + if (RobotBase.isSimulation()) return true; + String results = ""; for (int i = 0; i < maxTries; i++) { - boolean result = canandgyro.setSettings(settings, 0.05); - if (!result) { + if (!canandgyro.setSettings(settings, 0.05)) { results += (results.isEmpty() ? "" : ", ") + "setSettings() Failure"; continue; } diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java index 98001b4..742cbd3 100644 --- a/src/main/java/org/team340/robot/Constants.java +++ b/src/main/java/org/team340/robot/Constants.java @@ -7,10 +7,11 @@ */ public final class Constants { - public static final double kVoltage = 12.0; + public static final double VOLTAGE = 12.0; - public static final int kDriver = 0; - public static final int kCoDriver = 1; + // Controller ports + public static final int DRIVER = 0; + public static final int CO_DRIVER = 1; /** * The RobotMap class defines CAN IDs, CAN bus names, DIO/PWM/PH/PCM channel @@ -18,22 +19,22 @@ public final class Constants { */ public static final class RobotMap { - public static final String kLowerCANBus = "LowerCAN"; + public static final String LOWER_CAN = "LowerCAN"; - 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_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 kFlEncoder = 10; - public static final int kFrEncoder = 11; - public static final int kBlEncoder = 12; - public static final int kBrEncoder = 13; + 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 kCanandgyro = 14; + public static final int CANANDGYRO = 14; } } diff --git a/src/main/java/org/team340/robot/Robot.java b/src/main/java/org/team340/robot/Robot.java index 3ab41d0..a4f0b4e 100644 --- a/src/main/java/org/team340/robot/Robot.java +++ b/src/main/java/org/team340/robot/Robot.java @@ -12,9 +12,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import org.team340.lib.util.DisableWatchdog; -import org.team340.lib.util.Profiler; import org.team340.lib.util.Tunable; import org.team340.robot.commands.Autos; import org.team340.robot.commands.Routines; @@ -23,7 +21,7 @@ @Logged public final class Robot extends TimedRobot { - private final CommandScheduler scheduler = CommandScheduler.getInstance(); + public final CommandScheduler scheduler = CommandScheduler.getInstance(); public final Swerve swerve; @@ -52,11 +50,8 @@ public Robot() { autos = new Autos(this); // Initialize controllers - driver = new CommandXboxController(Constants.kDriver); - coDriver = new CommandXboxController(Constants.kCoDriver); - - // Create triggers - RobotModeTriggers.autonomous().whileTrue(autos.runSelectedAuto()); + driver = new CommandXboxController(Constants.DRIVER); + coDriver = new CommandXboxController(Constants.CO_DRIVER); // Set default commands swerve.setDefaultCommand(swerve.drive(this::driverX, this::driverY, this::driverAngular)); @@ -78,7 +73,7 @@ public Robot() { * Returns the current match time in seconds. */ public double matchTime() { - return Math.max(DriverStation.getMatchTime(), 0.0); + return Math.max(0.0, DriverStation.getMatchTime()); } @NotLogged @@ -98,11 +93,9 @@ public double driverAngular() { @Override public void robotPeriodic() { - Profiler.start("robotPeriodic"); - Profiler.run("scheduler", scheduler::run); - Profiler.run("epilogue", () -> Epilogue.update(this)); - Profiler.run("tunables", Tunable::update); - Profiler.end(); + scheduler.run(); + Epilogue.update(this); + Tunable.update(); } @Override diff --git a/src/main/java/org/team340/robot/commands/Autos.java b/src/main/java/org/team340/robot/commands/Autos.java index 755f677..33f485e 100644 --- a/src/main/java/org/team340/robot/commands/Autos.java +++ b/src/main/java/org/team340/robot/commands/Autos.java @@ -2,15 +2,12 @@ import static edu.wpi.first.wpilibj2.command.Commands.*; -import choreo.auto.AutoChooser; -import choreo.auto.AutoChooser; -import choreo.auto.AutoFactory; -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Strategy; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import org.team340.lib.util.AutoChooser; import org.team340.robot.Robot; import org.team340.robot.subsystems.Swerve; @@ -25,35 +22,26 @@ public final class Autos { private final Swerve swerve; private final Routines routines; - private final AutoFactory factory; private final AutoChooser chooser; public Autos(Robot robot) { swerve = robot.swerve; routines = robot.routines; - // Create the auto factory - factory = new AutoFactory(swerve::getPose, swerve::resetPose, swerve::followTrajectory, true, swerve); - chooser = new AutoChooser(); + // Create the auto chooser + chooser = new AutoChooser("Autos"); + chooser.bind(robot.scheduler); // Add autonomous modes to the dashboard - chooser.addRoutine("Example", this::example); - SmartDashboard.putData("autos", chooser); + chooser.add("Example", example()); } - /** - * Returns a command that when scheduled will run the currently selected auto. - */ - public Command runSelectedAuto() { - return chooser.selectedCommandScheduler(); - } - - private AutoRoutine example() { - AutoRoutine routine = factory.newRoutine("Example"); - AutoTrajectory exampleTraj = routine.trajectory("ExampleTrajectory"); - - routine.active().onTrue(sequence(exampleTraj.resetOdometry(), exampleTraj.spawnCmd())); - - return routine; + private Command example() { + return sequence( + swerve.resetPose(() -> new Pose2d(1.0, 1.0, Rotation2d.kZero)), + swerve.apfDrive(() -> new Pose2d(2.0, 5.0, Rotation2d.kPi), () -> 6.0, () -> 0.02), + swerve.apfDrive(() -> new Pose2d(6.0, 2.5, Rotation2d.kCW_Pi_2), () -> 6.0, () -> 0.02), + swerve.stop(false) + ); } } diff --git a/src/main/java/org/team340/robot/subsystems/Swerve.java b/src/main/java/org/team340/robot/subsystems/Swerve.java index 4bc821d..b3edc3d 100644 --- a/src/main/java/org/team340/robot/subsystems/Swerve.java +++ b/src/main/java/org/team340/robot/subsystems/Swerve.java @@ -1,17 +1,19 @@ package org.team340.robot.subsystems; -import choreo.auto.AutoFactory; -import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.CANBus; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; -import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.team340.lib.math.Math2; +import org.team340.lib.math.PAPFController; +import org.team340.lib.math.PAPFController.Obstacle; import org.team340.lib.swerve.Perspective; import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.SwerveState; @@ -31,39 +33,37 @@ @Logged public final class Swerve extends GRRSubsystem { - private static final double kMoveRatio = (54.0 / 10.0) * (18.0 / 38.0) * (45.0 / 15.0); - private static final double kTurnRatio = (22.0 / 10.0) * (88.0 / 16.0); - private static final double kModuleOffset = Units.inchesToMeters(12.5); + private static final double OFFSET = Units.inchesToMeters(12.5); - private static final SwerveModuleConfig kFrontLeft = new SwerveModuleConfig() + private final SwerveModuleConfig frontLeft = new SwerveModuleConfig() .setName("frontLeft") - .setLocation(kModuleOffset, kModuleOffset) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.kFlMove, true)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.kFlTurn, true)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.kFlEncoder, 0.0, false)); + .setLocation(OFFSET, OFFSET) + .setMoveMotor(SwerveMotors.talonFX(RobotMap.FL_MOVE, true)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.FL_TURN, true)) + .setEncoder(SwerveEncoders.cancoder(RobotMap.FL_ENCODER, 0.0, false)); - private static final SwerveModuleConfig kFrontRight = new SwerveModuleConfig() + private final SwerveModuleConfig frontRight = new SwerveModuleConfig() .setName("frontRight") - .setLocation(kModuleOffset, -kModuleOffset) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.kFrMove, true)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.kFrTurn, true)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.kFrEncoder, 0.0, false)); + .setLocation(OFFSET, -OFFSET) + .setMoveMotor(SwerveMotors.talonFX(RobotMap.FR_MOVE, true)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.FR_TURN, true)) + .setEncoder(SwerveEncoders.cancoder(RobotMap.FR_ENCODER, 0.0, false)); - private static final SwerveModuleConfig kBackLeft = new SwerveModuleConfig() + private final SwerveModuleConfig backLeft = new SwerveModuleConfig() .setName("backLeft") - .setLocation(-kModuleOffset, kModuleOffset) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.kBlMove, true)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.kBlTurn, true)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.kBlEncoder, 0.0, false)); + .setLocation(-OFFSET, OFFSET) + .setMoveMotor(SwerveMotors.talonFX(RobotMap.BL_MOVE, true)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.BL_TURN, true)) + .setEncoder(SwerveEncoders.cancoder(RobotMap.BL_ENCODER, 0.0, false)); - private static final SwerveModuleConfig kBackRight = new SwerveModuleConfig() + private final SwerveModuleConfig backRight = new SwerveModuleConfig() .setName("backRight") - .setLocation(-kModuleOffset, -kModuleOffset) - .setMoveMotor(SwerveMotors.talonFX(RobotMap.kBrMove, true)) - .setTurnMotor(SwerveMotors.talonFX(RobotMap.kBrTurn, true)) - .setEncoder(SwerveEncoders.canCoder(RobotMap.kBrEncoder, 0.0, false)); + .setLocation(-OFFSET, -OFFSET) + .setMoveMotor(SwerveMotors.talonFX(RobotMap.BR_MOVE, true)) + .setTurnMotor(SwerveMotors.talonFX(RobotMap.BR_TURN, true)) + .setEncoder(SwerveEncoders.cancoder(RobotMap.BR_ENCODER, 0.0, false)); - private static final SwerveConfig kConfig = new SwerveConfig() + private final SwerveConfig config = new SwerveConfig() .setTimings(TimedRobot.kDefaultPeriod, 0.004, 0.02, 0.01) .setMovePID(0.25, 0.0, 0.0) .setMoveFF(0.0, 0.125) @@ -71,38 +71,30 @@ public final class Swerve extends GRRSubsystem { .setBrakeMode(false, true) .setLimits(4.5, 0.05, 17.5, 14.0, 30.0) .setDriverProfile(4.0, 1.5, 0.15, 4.75, 2.0, 0.05) - .setPowerProperties(Constants.kVoltage, 100.0, 80.0, 60.0, 60.0) - .setMechanicalProperties(kMoveRatio, kTurnRatio, 0.0, Units.inchesToMeters(4.0)) + .setPowerProperties(Constants.VOLTAGE, 100.0, 80.0, 60.0, 60.0) + .setMechanicalProperties(729.0 / 95.0, 12.1, 0.0, Units.inchesToMeters(4.0)) .setOdometryStd(0.1, 0.1, 0.1) - .setIMU(SwerveIMUs.canandgyro(RobotMap.kCanandgyro)) - .setPhoenixFeatures(new CANBus(RobotMap.kLowerCANBus), true, true, true) - .setModules(kFrontLeft, kFrontRight, kBackLeft, kBackRight); + .setIMU(SwerveIMUs.canandgyro(RobotMap.CANANDGYRO)) + .setPhoenixFeatures(new CANBus(RobotMap.LOWER_CAN), true, true, true) + .setModules(frontLeft, frontRight, backLeft, backRight); private final SwerveAPI api; private final SwerveState state; - private final PIDController autoPIDx; - private final PIDController autoPIDy; - private final PIDController autoPIDangular; - - @SuppressWarnings("unused") - private Pose2d autoLast = null; - - private Pose2d autoNext = null; + private final PAPFController apf; + private final ProfiledPIDController angularPID; public Swerve() { - api = new SwerveAPI(kConfig); + api = new SwerveAPI(config); state = api.state; - autoPIDx = new PIDController(10.0, 0.0, 0.0); - autoPIDy = new PIDController(10.0, 0.0, 0.0); - autoPIDangular = new PIDController(10.0, 0.0, 0.0); - autoPIDangular.enableContinuousInput(-Math.PI, Math.PI); + apf = new PAPFController(4.0, 0.5, true, new Obstacle[0]); + angularPID = new ProfiledPIDController(10.0, 0.0, 0.0, new Constraints(10.0, 25.0)); + angularPID.enableContinuousInput(-Math.PI, Math.PI); api.enableTunables("swerve/api"); - Tunable.pidController("swerve/autoPID", autoPIDx); - Tunable.pidController("swerve/autoPID", autoPIDy); - Tunable.pidController("swerve/autoPIDangular", autoPIDangular); + apf.enableTunables("swerve/apf"); + Tunable.pidController("swerve/angularPID", angularPID); } @Override @@ -124,7 +116,18 @@ public Pose2d getPose() { */ public Command tareRotation() { return commandBuilder("Swerve.tareRotation()") - .onInitialize(() -> api.tareRotation(Perspective.kOperator)) + .onInitialize(() -> api.tareRotation(Perspective.OPERATOR)) + .isFinished(true) + .ignoringDisable(true); + } + + /** + * Resets the pose of the robot, inherently seeding field-relative movement. + * @param pose A supplier that returns the new blue origin relative pose to apply to the pose estimator. + */ + public Command resetPose(Supplier pose) { + return commandBuilder("Swerve.resetPose()") + .onInitialize(() -> api.resetPose(pose.get())) .isFinished(true) .ignoringDisable(true); } @@ -141,7 +144,7 @@ public Command drive(DoubleSupplier x, DoubleSupplier y, DoubleSupplier angular) x.getAsDouble(), y.getAsDouble(), angular.getAsDouble(), - Perspective.kOperator, + Perspective.OPERATOR, true, true ) @@ -149,45 +152,49 @@ public Command drive(DoubleSupplier x, DoubleSupplier y, DoubleSupplier angular) } /** - * 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. + * Drives the robot to a target position using the P-APF, until the + * robot is positioned within a specified tolerance of the target. + * @param goal A supplier that returns the target blue-origin relative field location. + * @param maxDeceleration A supplier that returns the desired deceleration rate of the robot, in m/s/s. + * @param endTolerance The tolerance in meters at which to end the command. */ - public Command stop(boolean lock) { - return commandBuilder("Swerve.stop(" + lock + ")").onExecute(() -> api.applyStop(lock)); + public Command apfDrive(Supplier goal, DoubleSupplier maxDeceleration, DoubleSupplier endTolerance) { + return apfDrive(goal, maxDeceleration) + .until(() -> Math2.isNear(goal.get().getTranslation(), state.translation, endTolerance.getAsDouble())) + .withName("Swerve.apfDrive()"); } /** - * Resets the pose of the robot, inherently seeding field-relative movement. This - * method is not intended for use outside of creating an {@link AutoFactory}. - * @param pose The new blue origin relative pose to apply to the pose estimator. + * Drives the robot to a target position using the P-APF. This command does not end. + * @param goal A supplier that returns the target blue-origin relative field location. + * @param maxDeceleration A supplier that returns the desired deceleration rate of the robot, in m/s/s. */ - public void resetPose(Pose2d pose) { - api.resetPose(pose); - - autoPIDx.reset(); - autoPIDy.reset(); - autoPIDangular.reset(); + public Command apfDrive(Supplier goal, DoubleSupplier maxDeceleration) { + return commandBuilder("Swerve.apfDrive()") + .onInitialize(() -> angularPID.reset(state.rotation.getRadians(), state.speeds.omegaRadiansPerSecond)) + .onExecute(() -> { + Pose2d next = goal.get(); + var speeds = apf.calculate( + state.pose, + next.getTranslation(), + config.velocity, + maxDeceleration.getAsDouble() + ); + + speeds.omegaRadiansPerSecond = angularPID.calculate( + state.rotation.getRadians(), + next.getRotation().getRadians() + ); + + api.applySpeeds(speeds, Perspective.BLUE, true, true); + }); } /** - * Follows a Choreo trajectory by moving towards the next sample. This method - * is not intended for use outside of creating an {@link AutoFactory}. - * @param sample The next trajectory sample. + * 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 followTrajectory(SwerveSample sample) { - autoLast = autoNext; - autoNext = sample.getPose(); - - Pose2d pose = state.pose; - api.applySpeeds( - new ChassisSpeeds( - sample.vx + autoPIDx.calculate(pose.getX(), sample.x), - sample.vy + autoPIDy.calculate(pose.getY(), sample.y), - sample.omega + autoPIDangular.calculate(pose.getRotation().getRadians(), sample.heading) - ), - Perspective.kBlue, - true, - false - ); + public Command stop(boolean lock) { + return commandBuilder("Swerve.stop(" + lock + ")").onExecute(() -> api.applyStop(lock)); } } diff --git a/vendordeps/ChoreoLib-2025.0.3.json b/vendordeps/ChoreoLib-2025.0.3.json deleted file mode 100644 index 5a8cd54..0000000 --- a/vendordeps/ChoreoLib-2025.0.3.json +++ /dev/null @@ -1,44 +0,0 @@ -{ - "fileName": "ChoreoLib-2025.0.3.json", - "name": "ChoreoLib", - "version": "2025.0.3", - "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", - "frcYear": "2025", - "mavenUrls": [ - "https://lib.choreo.autos/dep", - "https://repo1.maven.org/maven2" - ], - "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json", - "javaDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-java", - "version": "2025.0.3" - }, - { - "groupId": "com.google.code.gson", - "artifactId": "gson", - "version": "2.11.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-cpp", - "version": "2025.0.3", - "libName": "ChoreoLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index d7bea57..6f40c84 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.3.2", + "version": "25.4.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.3.2" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +382,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +398,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +446,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "25.3.2", + "version": "25.4.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -444,6 +458,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index 2d7b1d8..0000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2025.3.1", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2025", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2025.3.1", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2025.3.1" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.3.1" - } - ] -} \ No newline at end of file From c9f52699b84095f8b29104a7b2fdb3bfe99cd4ee Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Tue, 13 May 2025 21:03:28 -0400 Subject: [PATCH 5/7] Update AllianceFlip.java --- src/main/java/org/team340/lib/math/AllianceFlip.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team340/lib/math/AllianceFlip.java b/src/main/java/org/team340/lib/math/AllianceFlip.java index 07eac6d..25c6197 100644 --- a/src/main/java/org/team340/lib/math/AllianceFlip.java +++ b/src/main/java/org/team340/lib/math/AllianceFlip.java @@ -23,7 +23,7 @@ public final class AllianceFlip { */ public AllianceFlip(double fieldLength, double fieldWidth, FlipType flipType) { this.fieldLength = fieldLength; - this.fieldWidth = fieldLength; + this.fieldWidth = fieldWidth; this.flipType = flipType; } From d41d5238c30f6851ba3a38d9e583ad351aee7fb7 Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Fri, 6 Jun 2025 23:25:22 -0400 Subject: [PATCH 6/7] Upstream project layout experiments from 2025 repo --- .vscode/settings.json | 32 ++- build.gradle | 12 +- .../team340/lib/logging/EpilogueProxy.java | 88 ++++++++ .../org/team340/lib/logging/LoggedRobot.java | 124 +++++++++++ .../org/team340/lib/logging/Profiler.java | 208 ++++++++++++++++++ .../lib/logging/phoenix/CANcoderLogger.java | 11 +- .../lib/logging/phoenix/TalonFXLogger.java | 3 + .../lib/logging/phoenix/TalonFXSLogger.java | 3 + .../SparkClosedLoopControllerLogger.java | 19 -- .../lib/logging/wpimath/DebouncerLogger.java | 19 -- .../ExponentialProfileConstraintsLogger.java | 19 -- .../wpimath/ExponentialProfileLogger.java | 19 -- .../TrapezoidProfileConstraintsLogger.java | 19 -- .../wpimath/TrapezoidProfileLogger.java | 19 -- .../org/team340/lib/math/AllianceFlip.java | 164 -------------- .../org/team340/lib/swerve/SwerveAPI.java | 2 +- .../team340/lib/swerve/SwerveAPILogger.java | 40 +++- .../org/team340/lib/swerve/SwerveModule.java | 6 +- .../team340/lib/swerve/SwerveTunables.java | 46 ++-- .../lib/swerve/config/SwerveConfig.java | 24 +- .../lib/swerve/config/SwerveModuleConfig.java | 14 +- .../swerve/hardware/SwerveBaseHardware.java | 9 - .../lib/swerve/hardware/SwerveEncoders.java | 38 +--- .../lib/swerve/hardware/SwerveIMUs.java | 34 +-- .../lib/swerve/hardware/SwerveMotors.java | 33 +-- .../java/org/team340/lib/util/Alliance.java | 2 +- .../org/team340/lib/util/DisableWatchdog.java | 2 +- .../java/org/team340/lib/util/FieldFlip.java | 135 ++++++++++++ .../java/org/team340/lib/util/FieldInfo.java | 64 ++++++ .../org/team340/lib/{math => util}/Math2.java | 21 +- .../lib/{math => util}/PAPFController.java | 24 +- src/main/java/org/team340/lib/util/Sleep.java | 2 +- .../java/org/team340/lib/util/Tunable.java | 137 ++++++------ .../lib/util/{ => command}/AutoChooser.java | 65 ++++-- .../team340/lib/util/vendors/PhoenixUtil.java | 2 +- .../team340/lib/util/vendors/ReduxUtil.java | 2 +- .../org/team340/lib/util/vendors/RevUtil.java | 2 +- src/main/java/org/team340/robot/Robot.java | 50 +---- .../org/team340/robot/commands/Autos.java | 21 +- .../org/team340/robot/commands/Routines.java | 4 +- .../org/team340/robot/subsystems/Swerve.java | 17 +- 41 files changed, 919 insertions(+), 636 deletions(-) create mode 100644 src/main/java/org/team340/lib/logging/EpilogueProxy.java create mode 100644 src/main/java/org/team340/lib/logging/LoggedRobot.java create mode 100644 src/main/java/org/team340/lib/logging/Profiler.java delete mode 100644 src/main/java/org/team340/lib/logging/revlib/SparkClosedLoopControllerLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/wpimath/DebouncerLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileConstraintsLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileConstraintsLogger.java delete mode 100644 src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileLogger.java delete mode 100644 src/main/java/org/team340/lib/math/AllianceFlip.java create mode 100644 src/main/java/org/team340/lib/util/FieldFlip.java create mode 100644 src/main/java/org/team340/lib/util/FieldInfo.java rename src/main/java/org/team340/lib/{math => util}/Math2.java (85%) rename src/main/java/org/team340/lib/{math => util}/PAPFController.java (96%) rename src/main/java/org/team340/lib/util/{ => command}/AutoChooser.java (62%) diff --git a/.vscode/settings.json b/.vscode/settings.json index 3b046a9..9d1f165 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -49,7 +49,6 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "svelte.enable-ts-plugin": true, "files.exclude": { "**/.git": true, "**/.svn": true, @@ -63,27 +62,29 @@ "**/.factorypath": true, "**/*~": true }, + "wpilib.autoStartRioLog": false, "cSpell.words": [ + "ADIS", "Accl", "Accum", - "ADIS", "Autoset", "Backports", "Bezier", "Botpose", "Brushless", + "CANcoder", + "CANdi", + "CANrange", + "CTRE", "Canandcolor", "Canandgyro", "Canandmag", "Canbus", - "CANcoder", - "CANdi", - "CANrange", "Checkstyle", "Choreo", "ChoreoLib", "Cnfg", - "CTRE", + "DTheta", "Deadband", "Deadbands", "Deadzone", @@ -100,40 +101,39 @@ "Discretize", "Discretizing", "Doppel", - "DTheta", "Dunkin", "EEPROM", "Expdelta", "Falsi", "Feedforward", "Fullscreen", + "GSON", "Gradlew", "Grav", - "GRRDashboard", - "GSON", "Holonomic", "Intaking", "Integ", "Interpolatable", "Itor", - "JoystickProfiles", "Kalman", "Keepalive", "Lerp", - "Motorcontrol", "Msgpack", "Mult", "Multiturn", - "NetworkTables", "NTURI", + "NetworkTables", "Odometry", "Overcurrent", "PAPF", "PIDF", + "PhotonVision", "Powerup", "Protobuf", "Pubuid", "Quasistatic", + "REVPH", + "REVRobotics", "Ratelimit", "Ratelimited", "Ratelimiter", @@ -142,9 +142,8 @@ "ReduxLib", "Reefscape", "Regula", + "Repulsor", "RevLib", - "REVPH", - "REVRobotics", "RoboRIO", "Sendables", "Setpoint", @@ -156,9 +155,7 @@ "Subuid", "TalonFX", "TalonSRX", - "Tauri", "Teleop", - "Timestep", "Timesync", "Topicsonly", "Traj", @@ -169,14 +166,11 @@ "Unsubscriber", "Unsubscribers", "Unused", - "URCL", "Vbat", "Vmax", - "WPIBlue", "WPILib", "WPILibJ", "WPIMath", - "WPIRed", "Writables" ] } diff --git a/build.gradle b/build.gradle index a98f347..834d562 100644 --- a/build.gradle +++ b/build.gradle @@ -28,7 +28,17 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - // Uncomment to enable VisualVM connections + jvmArgs.add("-XX:+UnlockExperimentalVMOptions") + jvmArgs.add("-XX:GCTimeRatio=5") + jvmArgs.add("-XX:+UseSerialGC") + jvmArgs.add("-XX:MaxGCPauseMillis=50") + + // Only enable these arguments when using a RIO 2 + jvmArgs.add("-Xmx100M") + jvmArgs.add("-Xms100M") + jvmArgs.add("-XX:+AlwaysPreTouch") + + // Enable VisualVM connection // jvmArgs.add("-Dcom.sun.management.jmxremote=true") // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") diff --git a/src/main/java/org/team340/lib/logging/EpilogueProxy.java b/src/main/java/org/team340/lib/logging/EpilogueProxy.java new file mode 100644 index 0000000..b73a114 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/EpilogueProxy.java @@ -0,0 +1,88 @@ +package org.team340.lib.logging; + +import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.epilogue.EpilogueConfiguration; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; +import edu.wpi.first.wpilibj.DriverStation; +import java.lang.reflect.Field; +import java.lang.reflect.Modifier; +import java.util.function.Consumer; +import org.team340.lib.util.Mutable; + +// +// Do not fret if your IDE reports errors in this file! +// +// Epilogue is a generated class and may not have been seen yet by your +// editor, but your code will still build and function as normal. +// + +/** + * Utility for interfacing with the generated {@code Epilogue} + * class. Keeps the occasional false negatives from the language + * server in one file. + */ +public final class EpilogueProxy { + + private EpilogueProxy() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Configures Epilogue. + * @param configurator A consumer that mutates the provided {@link EpilogueConfiguration}. + */ + public static void configure(Consumer configurator) { + Epilogue.configure(configurator); + } + + /** + * Gets the current epilogue configuration. + */ + public static EpilogueConfiguration getConfig() { + return Epilogue.getConfig(); + } + + /** + * Gets the configured root backend. + */ + public static EpilogueBackend getRootBackend() { + return getConfig().backend.getNested(getConfig().root); + } + + /** + * Generates a logging function for the provided object via reflection. This + * method should not be invoked periodically due to performance implications. + * @param obj The object to generate the logger for. + */ + @SuppressWarnings("unchecked") + public static Consumer getLogger(Object obj) { + Field[] fields = Epilogue.class.getDeclaredFields(); + Mutable> objLogger = new Mutable<>(null); + + for (Field field : fields) { + if (Modifier.isStatic(field.getModifiers())) { + try { + Object value = field.get(obj); + if (value instanceof ClassSpecificLogger logger) { + Class type = logger.getLoggedType(); + if (type.equals(obj.getClass())) { + objLogger.value = logger; + break; + } + } + } catch (Exception e) {} + } + } + + if (objLogger.value != null) { + return backend -> objLogger.value.tryUpdate(backend, obj, Epilogue.getConfig().errorHandler); + } else { + DriverStation.reportWarning( + "[EpilogueProxy] Unable to find logger for class \"" + obj.getClass().getSimpleName() + "\"", + true + ); + return backend -> {}; + } + } +} diff --git a/src/main/java/org/team340/lib/logging/LoggedRobot.java b/src/main/java/org/team340/lib/logging/LoggedRobot.java new file mode 100644 index 0000000..549a2a3 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/LoggedRobot.java @@ -0,0 +1,124 @@ +package org.team340.lib.logging; + +import com.ctre.phoenix6.SignalLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; +import edu.wpi.first.hal.DriverStationJNI; +import edu.wpi.first.hal.NotifierJNI; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.IterativeRobotBase; +import edu.wpi.first.wpilibj.RobotController; +import java.util.function.Consumer; +import org.team340.lib.util.DisableWatchdog; +import org.team340.lib.util.Tunable; + +/** + * LoggedRobot implements the IterativeRobotBase robot program framework, + * and is intended to be subclassed by a user creating a robot program. + * + *

The LoggedRobot class configures the program for logging data to NetworkTables + * via Epilogue, which is captured by the {@link DataLogManager} alongside Driver + * Station values and saved to file. Loop timings are also recorded using the + * {@link Profiler} class. + * + *

{@code periodic()} functions from the base class are called on an interval + * by a Notifier instance. Additionally, utility classes with periodic methods + * such as {@link Tunable} are updated on the same interval. + */ +public class LoggedRobot extends IterativeRobotBase { + + public static final double DEFAULT_PERIOD = 0.02; + + private final int notifier = NotifierJNI.initializeNotifier(); + private final Consumer logger; + + private long nextCycle = 0L; + + /** + * Constructor for LoggedRobot. + */ + protected LoggedRobot() { + this(DEFAULT_PERIOD); + } + + /** + * Constructor for LoggedRobot. + * @param period Period in seconds. + */ + protected LoggedRobot(double period) { + super(period); + NotifierJNI.setNotifierName(notifier, "LoggedRobot"); + + DriverStation.silenceJoystickConnectionWarning(true); + DisableWatchdog.in(this, "m_watchdog"); + + DataLogManager.start(); + DriverStation.startDataLog(DataLogManager.getLog()); + SignalLogger.enableAutoLogging(false); + + EpilogueProxy.getConfig().root = "/Telemetry"; + logger = EpilogueProxy.getLogger(this); + } + + @Override + public void startCompetition() { + robotInit(); + if (isSimulation()) { + simulationInit(); + } + + // Tell the DS that the robot is ready to be enabled + System.out.println("********** Robot program startup complete **********"); + DriverStationJNI.observeUserProgramStarting(); + + // Loop forever, calling the appropriate mode-dependent function + while (true) { + long now = RobotController.getFPGATime(); + if (nextCycle < now) { + nextCycle = now; + } else { + NotifierJNI.updateNotifierAlarm(notifier, nextCycle); + if (NotifierJNI.waitForNotifierAlarm(notifier) == 0L) { + break; + } + } + + nextCycle += (long) (getPeriod() * 1000000.0); + + Profiler.start("robot"); + Profiler.run("loopFunction", () -> loopFunc()); + Profiler.run("epilogue", () -> logger.accept(EpilogueProxy.getRootBackend())); + Profiler.run("tunables", Tunable::update); + Profiler.end(); + } + } + + @Override + public void endCompetition() { + NotifierJNI.stopNotifier(notifier); + } + + @Override + public void close() { + NotifierJNI.stopNotifier(notifier); + NotifierJNI.cleanNotifier(notifier); + } + + @Override + public void robotPeriodic() {} + + @Override + public void simulationPeriodic() {} + + @Override + public void disabledPeriodic() {} + + @Override + public void autonomousPeriodic() {} + + @Override + public void teleopPeriodic() {} + + @Override + public void testPeriodic() {} +} diff --git a/src/main/java/org/team340/lib/logging/Profiler.java b/src/main/java/org/team340/lib/logging/Profiler.java new file mode 100644 index 0000000..280c73b --- /dev/null +++ b/src/main/java/org/team340/lib/logging/Profiler.java @@ -0,0 +1,208 @@ +package org.team340.lib.logging; + +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import java.lang.management.GarbageCollectorMXBean; +import java.lang.management.ManagementFactory; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.function.Supplier; + +/** + * A simple pseudo call-graph profiler that publishes timings to NetworkTables. + * + *

Data is published to {@code /Profiling}, with CPU time utilized by the garbage + * collector published at {@code /Profiling/gc}, and time spent publishing values to + * NT at {@code /Profiling/overhead}. All timings are recorded in milliseconds, + * as the duration of execution. The profiler records the time to execute blocks + * of code, with support for nesting execution times which are reflected in an + * expanding NT tree. + * + *

All invocations of {@link Profiler#start(String)} are expected to be + * followed by a closing {@link Profiler#end()}. Additionally, it is expected + * that a single {@link Profiler#start(String)}/{@link Profiler#end()} pair is + * found at the highest level of the robot's code, with no other "root" pairs + * (this is already done if utilizing {@link LoggedRobot}. If either of these + * limitations are left unsatisfied, an error will be printed to the Driver Station + * console. Profiling across threads is also not supported. + */ +public final class Profiler { + + private Profiler() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("Profiling"); + + private static final Map callGraph = new HashMap<>(); + private static final List stack = new ArrayList<>(); + + private static final List gcList = ManagementFactory.getGarbageCollectorMXBeans(); + private static final DoublePublisher overheadPub = nt.getDoubleTopic("overhead").publish(); + private static final DoublePublisher gcPub = nt.getDoubleTopic("gc").publish(); + + private static String root = ""; + private static long lastGCTime = 0L; + + /** + * Determines if the profiler is currently running. + * @return {@code true} if the profiler is running. + */ + public static boolean isRunning() { + return stack.size() > 0; + } + + /** + * Profiles a section of user code. Serves as a shorthand for calling + * {@link Profiler#start(String)}, {user code}, {@link Profiler#end()}. + * @param name The name of the call. Must be unique. + * @param runnable The user code to be ran. + */ + public static void run(String name, Runnable runnable) { + start(name); + runnable.run(); + end(); + } + + /** + * Profiles a section of user code. Serves as a shorthand for calling + * {@link Profiler#start(String)}, {user code}, {@link Profiler#end()}. + * @param The return type of the supplier. + * @param name The name of the call. Must be unique. + * @param runnable The user code to be ran. + * @return The value returned from the supplier. + */ + public static T run(String name, Supplier supplier) { + start(name); + T v = supplier.get(); + end(); + return v; + } + + /** + * Starts to profile a section of user code. This method should be invoked + * before the user code to be profiled, with {@link Profiler#end()} expected + * to be after. Additional {@link Profiler#start(String)}/{@link Profiler#end()} + * pairs can be nested within the section of user code, and will appear as a + * sub-topic in NetworkTables. Timings of nested pairs are still included in + * the overall time recorded by the parent pair. + * @param name The name of the call. Must be unique. + */ + public static void start(String name) { + if (stack.isEmpty()) { + if (root.isEmpty()) { + root = name; + } else if (name != root) { + DriverStation.reportError( + "[Profiler] Unexpected secondary root with name \"" + + name + + "\", expected primary root \"" + + root + + "\"", + true + ); + } + } + + stack.add(name); + String fullName = String.join("/", stack); + CallData call = callGraph.get(fullName); + if (call == null) { + call = new CallData(fullName); + callGraph.put(fullName, call); + } + + call.onStart(); + } + + /** + * Ends profiling a section of code. This method should be invoked + * after the user code to be profiled. Trailing calls to this method + * will result in an error printed to the Driver Station console. + */ + public static void end() { + CallData call = callGraph.get(String.join("/", stack)); + if (call != null) { + call.onEnd(); + stack.remove(stack.size() - 1); + + if (stack.isEmpty()) { + long start = RobotController.getFPGATime(); + var iter = callGraph.entrySet().iterator(); + while (iter.hasNext()) { + CallData entryCall = iter.next().getValue(); + if (!entryCall.done) { + entryCall.close(); + iter.remove(); + continue; + } + + entryCall.pubAndReset(start); + } + + long gcSum = 0L; + for (var gc : gcList) { + long gcTime = gc.getCollectionTime(); + if (gcTime != -1L) gcSum += gcTime; + } + gcPub.set(gcSum - lastGCTime); + lastGCTime = gcSum; + + overheadPub.set((RobotController.getFPGATime() - start) / 1000.0); + } + } else { + DriverStation.reportError("[Profiler] Unexpected end() call", true); + } + } + + /** + * Manages the data of a call. + */ + private static final class CallData implements AutoCloseable { + + public final String fullName; + public long time = -1L; + public boolean done = false; + public DoublePublisher pub; + + public CallData(String fullName) { + this.fullName = fullName; + } + + /** + * Should be invoked when a call starts. + */ + public void onStart() { + time = RobotController.getFPGATime(); + } + + /** + * Should be invoked when a call ends. + */ + public void onEnd() { + time = RobotController.getFPGATime() - time; + done = true; + } + + /** + * Publishes the last timing to NT and resets state. + * @param timestamp The timestamp to publish the NT value to. + */ + public void pubAndReset(long timestamp) { + if (pub == null) pub = nt.getDoubleTopic(fullName).publish(); + pub.set(time / 1000.0, timestamp); + time = -1L; + done = false; + } + + @Override + public void close() { + pub.close(); + } + } +} diff --git a/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java b/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java index a56d696..f3167f8 100644 --- a/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java +++ b/src/main/java/org/team340/lib/logging/phoenix/CANcoderLogger.java @@ -1,14 +1,10 @@ package org.team340.lib.logging.phoenix; -import static edu.wpi.first.util.struct.StructGenerator.genEnum; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.MagnetHealthValue; import edu.wpi.first.epilogue.CustomLoggerFor; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.util.struct.Struct; import java.util.HashMap; import java.util.Map; import java.util.function.Consumer; @@ -17,7 +13,6 @@ public class CANcoderLogger extends ClassSpecificLogger { private static final Map> cache = new HashMap<>(); - private static final Struct magnetHealthStruct = genEnum(MagnetHealthValue.class); public CANcoderLogger() { super(CANcoder.class); @@ -27,17 +22,13 @@ public CANcoderLogger() { public void update(EpilogueBackend backend, CANcoder cancoder) { cache .computeIfAbsent(cancoder, key -> { - var absolutePosition = cancoder.getAbsolutePosition(false); - var magnetHealth = cancoder.getMagnetHealth(false); var position = cancoder.getPosition(false); var velocity = cancoder.getVelocity(false); - BaseStatusSignal[] signals = { absolutePosition, magnetHealth, position, velocity }; + BaseStatusSignal[] signals = { position, velocity }; return b -> { BaseStatusSignal.refreshAll(signals); - b.log("absolutePosition", absolutePosition.getValueAsDouble()); - b.log("magnetHealth", magnetHealth.getValue(), magnetHealthStruct); b.log("position", position.getValueAsDouble()); b.log("velocity", velocity.getValueAsDouble()); }; diff --git a/src/main/java/org/team340/lib/logging/phoenix/TalonFXLogger.java b/src/main/java/org/team340/lib/logging/phoenix/TalonFXLogger.java index b9e744a..5ae7c48 100644 --- a/src/main/java/org/team340/lib/logging/phoenix/TalonFXLogger.java +++ b/src/main/java/org/team340/lib/logging/phoenix/TalonFXLogger.java @@ -27,6 +27,7 @@ public void update(EpilogueBackend backend, TalonFX talonFX) { var motorVoltage = talonFX.getMotorVoltage(false); var position = talonFX.getPosition(false); var statorCurrent = talonFX.getStatorCurrent(false); + var supplyCurrent = talonFX.getSupplyCurrent(false); var velocity = talonFX.getVelocity(false); BaseStatusSignal[] signals = { @@ -35,6 +36,7 @@ public void update(EpilogueBackend backend, TalonFX talonFX) { motorVoltage, position, statorCurrent, + supplyCurrent, velocity }; @@ -45,6 +47,7 @@ public void update(EpilogueBackend backend, TalonFX talonFX) { b.log("motorVoltage", motorVoltage.getValueAsDouble()); b.log("position", position.getValueAsDouble()); b.log("statorCurrent", statorCurrent.getValueAsDouble()); + b.log("supplyCurrent", supplyCurrent.getValueAsDouble()); b.log("velocity", velocity.getValueAsDouble()); }; }) diff --git a/src/main/java/org/team340/lib/logging/phoenix/TalonFXSLogger.java b/src/main/java/org/team340/lib/logging/phoenix/TalonFXSLogger.java index b6d3a1f..c7897e0 100644 --- a/src/main/java/org/team340/lib/logging/phoenix/TalonFXSLogger.java +++ b/src/main/java/org/team340/lib/logging/phoenix/TalonFXSLogger.java @@ -27,6 +27,7 @@ public void update(EpilogueBackend backend, TalonFXS talonFXS) { var motorVoltage = talonFXS.getMotorVoltage(false); var position = talonFXS.getPosition(false); var statorCurrent = talonFXS.getStatorCurrent(false); + var supplyCurrent = talonFXS.getSupplyCurrent(false); var velocity = talonFXS.getVelocity(false); BaseStatusSignal[] signals = { @@ -35,6 +36,7 @@ public void update(EpilogueBackend backend, TalonFXS talonFXS) { motorVoltage, position, statorCurrent, + supplyCurrent, velocity }; @@ -45,6 +47,7 @@ public void update(EpilogueBackend backend, TalonFXS talonFXS) { b.log("motorVoltage", motorVoltage.getValueAsDouble()); b.log("position", position.getValueAsDouble()); b.log("statorCurrent", statorCurrent.getValueAsDouble()); + b.log("supplyCurrent", supplyCurrent.getValueAsDouble()); b.log("velocity", velocity.getValueAsDouble()); }; }) diff --git a/src/main/java/org/team340/lib/logging/revlib/SparkClosedLoopControllerLogger.java b/src/main/java/org/team340/lib/logging/revlib/SparkClosedLoopControllerLogger.java deleted file mode 100644 index 3512e89..0000000 --- a/src/main/java/org/team340/lib/logging/revlib/SparkClosedLoopControllerLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -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.EpilogueBackend; - -@CustomLoggerFor(SparkClosedLoopController.class) -public class SparkClosedLoopControllerLogger extends ClassSpecificLogger { - - public SparkClosedLoopControllerLogger() { - super(SparkClosedLoopController.class); - } - - @Override - public void update(EpilogueBackend backend, SparkClosedLoopController pidController) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpimath/DebouncerLogger.java b/src/main/java/org/team340/lib/logging/wpimath/DebouncerLogger.java deleted file mode 100644 index 5383818..0000000 --- a/src/main/java/org/team340/lib/logging/wpimath/DebouncerLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpimath; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.math.filter.Debouncer; - -@CustomLoggerFor(Debouncer.class) -public class DebouncerLogger extends ClassSpecificLogger { - - public DebouncerLogger() { - super(Debouncer.class); - } - - @Override - public void update(EpilogueBackend backend, Debouncer debouncer) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileConstraintsLogger.java b/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileConstraintsLogger.java deleted file mode 100644 index d60f648..0000000 --- a/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileConstraintsLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpimath; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.math.trajectory.ExponentialProfile; - -@CustomLoggerFor(ExponentialProfile.Constraints.class) -public class ExponentialProfileConstraintsLogger extends ClassSpecificLogger { - - public ExponentialProfileConstraintsLogger() { - super(ExponentialProfile.Constraints.class); - } - - @Override - public void update(EpilogueBackend backend, ExponentialProfile.Constraints constraints) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileLogger.java b/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileLogger.java deleted file mode 100644 index ffddec7..0000000 --- a/src/main/java/org/team340/lib/logging/wpimath/ExponentialProfileLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpimath; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.math.trajectory.ExponentialProfile; - -@CustomLoggerFor(ExponentialProfile.class) -public class ExponentialProfileLogger extends ClassSpecificLogger { - - public ExponentialProfileLogger() { - super(ExponentialProfile.class); - } - - @Override - public void update(EpilogueBackend backend, ExponentialProfile profile) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileConstraintsLogger.java b/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileConstraintsLogger.java deleted file mode 100644 index 98fb578..0000000 --- a/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileConstraintsLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpimath; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.math.trajectory.TrapezoidProfile; - -@CustomLoggerFor(TrapezoidProfile.Constraints.class) -public class TrapezoidProfileConstraintsLogger extends ClassSpecificLogger { - - public TrapezoidProfileConstraintsLogger() { - super(TrapezoidProfile.Constraints.class); - } - - @Override - public void update(EpilogueBackend backend, TrapezoidProfile.Constraints constraints) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileLogger.java b/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileLogger.java deleted file mode 100644 index de4789a..0000000 --- a/src/main/java/org/team340/lib/logging/wpimath/TrapezoidProfileLogger.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.team340.lib.logging.wpimath; - -import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.logging.ClassSpecificLogger; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.math.trajectory.TrapezoidProfile; - -@CustomLoggerFor(TrapezoidProfile.class) -public class TrapezoidProfileLogger extends ClassSpecificLogger { - - public TrapezoidProfileLogger() { - super(TrapezoidProfile.class); - } - - @Override - public void update(EpilogueBackend backend, TrapezoidProfile profile) { - // No-op - } -} diff --git a/src/main/java/org/team340/lib/math/AllianceFlip.java b/src/main/java/org/team340/lib/math/AllianceFlip.java deleted file mode 100644 index 25c6197..0000000 --- a/src/main/java/org/team340/lib/math/AllianceFlip.java +++ /dev/null @@ -1,164 +0,0 @@ -package org.team340.lib.math; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import java.util.function.Supplier; -import org.team340.lib.util.Alliance; - -/** - * Utility class for flipping field locations based on the robot's alliance. - */ -public final class AllianceFlip { - - private final double fieldLength; - private final double fieldWidth; - private final FlipType flipType; - - /** - * Create the alliance flipper. - * @param fieldLength The length of the field, in meters. - * @param fieldWidth The width of the field, in meters. - * @param flipType The field's symmetry. - */ - public AllianceFlip(double fieldLength, double fieldWidth, FlipType flipType) { - this.fieldLength = fieldLength; - this.fieldWidth = fieldWidth; - this.flipType = flipType; - } - - /** - * Flips the provided translation to the opposite alliance. - * @param translation The translation to flip. - */ - public Translation2d flip(Translation2d translation) { - return new Translation2d( - flipType.x(translation.getX(), fieldLength), - flipType.y(translation.getY(), fieldWidth) - ); - } - - /** - * Flips the provided rotation to the opposite alliance. - * @param rotation The rotation to flip. - */ - public Rotation2d flip(Rotation2d rotation) { - return switch (flipType) { - case MIRROR -> new Rotation2d(-rotation.getCos(), rotation.getSin()); - case ROTATE -> new Rotation2d(-rotation.getCos(), -rotation.getSin()); - }; - } - - /** - * Flips the provided pose to the opposite alliance. - * @param pose The pose to flip. - */ - public Pose2d flip(Pose2d pose) { - return new Pose2d(flip(pose.getTranslation()), flip(pose.getRotation())); - } - - /** - * Mirrors the provided translation across the width of the field. - * @param translation The translation to mirror. - */ - public Translation2d mirror(Translation2d translation) { - return new Translation2d(translation.getX(), fieldWidth - translation.getY()); - } - - /** - * Mirrors the provided rotation across the width of the field. - * @param rotation The rotation to mirror. - */ - public Rotation2d mirror(Rotation2d rotation) { - return rotation.unaryMinus(); - } - - /** - * Mirrors the provided pose across the width of the field. - * @param pose The pose to mirror. - */ - public Pose2d mirror(Pose2d pose) { - return new Pose2d(mirror(pose.getTranslation()), mirror(pose.getRotation())); - } - - /** - * Creates a supplier that returns the provided {@link Pose2d}, - * flipped based on the robot's current alliance. - * @param pose The blue origin relative pose. - */ - public Supplier flipped(Pose2d pose) { - return flipped(pose, false); - } - - /** - * Creates a supplier that returns the provided {@link Pose2d}, - * flipped based on the robot's current alliance. - * @param pose The blue origin relative pose. - * @param mirrored If the pose should also be mirrored - * across the width of the field. - */ - public Supplier flipped(Pose2d pose, boolean mirrored) { - Pose2d blue = mirrored ? mirror(pose) : pose; - Pose2d red = flip(pose); - return () -> Alliance.isBlue() ? blue : red; - } - - /** - * Symmetry types for a field. - */ - public static enum FlipType { - MIRROR { - @Override - protected double x(double x, double fieldLength) { - return fieldLength - x; - } - - @Override - protected double y(double y, double fieldWidth) { - return y; - } - - @Override - protected double heading(double heading) { - return Math.PI - heading; - } - }, - - ROTATE { - @Override - protected double x(double x, double fieldLength) { - return fieldLength - x; - } - - @Override - protected double y(double y, double fieldWidth) { - return fieldWidth - y; - } - - @Override - protected double heading(double heading) { - return Math.PI + heading; - } - }; - - /** - * Flips the X coordinate. - * @param x The X coordinate to flip. - * @param fieldLength The field's length. - */ - protected abstract double x(double x, double fieldLength); - - /** - * Flips the Y coordinate. - * @param y The Y coordinate to flip. - * @param fieldWidth the field's width. - */ - protected abstract double y(double y, double fieldWidth); - - /** - * Flips the heading. - * @param heading The heading to flip. - */ - protected abstract double heading(double heading); - } -} diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPI.java b/src/main/java/org/team340/lib/swerve/SwerveAPI.java index cd24e24..03d3a03 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveAPI.java +++ b/src/main/java/org/team340/lib/swerve/SwerveAPI.java @@ -28,9 +28,9 @@ import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.Consumer; -import org.team340.lib.math.Math2; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.hardware.SwerveIMUs.SwerveIMU; +import org.team340.lib.util.Math2; import org.team340.lib.util.Sleep; import org.team340.robot.Robot; diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java b/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java index 3251e9d..e5f1a90 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java +++ b/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java @@ -1,7 +1,6 @@ package org.team340.lib.swerve; import edu.wpi.first.epilogue.CustomLoggerFor; -import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.logging.ClassSpecificLogger; import edu.wpi.first.epilogue.logging.EpilogueBackend; import edu.wpi.first.math.geometry.Pose2d; @@ -9,10 +8,16 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import org.team340.lib.logging.EpilogueProxy; @CustomLoggerFor(SwerveAPI.class) public class SwerveAPILogger extends ClassSpecificLogger { + private static final Map> cache = new HashMap<>(); + public SwerveAPILogger() { super(SwerveAPI.class); } @@ -21,16 +26,31 @@ public SwerveAPILogger() { public void update(EpilogueBackend backend, SwerveAPI api) { logState(backend.getNested("state"), api.state); - var hardware = backend.getNested("hardware"); - var errorHandler = Epilogue.getConfig().errorHandler; + cache + .computeIfAbsent(api, key -> { + var imu = EpilogueProxy.getLogger(api.imu.getAPI()); + + Map> modules = new HashMap<>(); + for (SwerveModule module : api.modules) { + var move = EpilogueProxy.getLogger(module.moveMotor.getAPI()); + var turn = EpilogueProxy.getLogger(module.turnMotor.getAPI()); + var encoder = EpilogueProxy.getLogger(module.encoder.getAPI()); + + modules.put(module.getName(), b -> { + move.accept(b.getNested("moveMotor")); + turn.accept(b.getNested("turnMotor")); + encoder.accept(b.getNested("encoder")); + }); + } - api.imu.log(hardware.getNested("imu"), errorHandler); - for (var m : api.modules) { - var module = hardware.getNested(m.getName()); - m.moveMotor.log(module.getNested("moveMotor"), errorHandler); - m.turnMotor.log(module.getNested("turnMotor"), errorHandler); - m.encoder.log(module.getNested("encoder"), errorHandler); - } + return b -> { + imu.accept(b.getNested("imu")); + for (var module : modules.entrySet()) { + module.getValue().accept(b.getNested(module.getKey())); + } + }; + }) + .accept(backend.getNested("hardware")); } private void logState(EpilogueBackend backend, SwerveState state) { diff --git a/src/main/java/org/team340/lib/swerve/SwerveModule.java b/src/main/java/org/team340/lib/swerve/SwerveModule.java index fb8f89d..7717323 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveModule.java +++ b/src/main/java/org/team340/lib/swerve/SwerveModule.java @@ -10,12 +10,12 @@ import java.util.List; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; -import org.team340.lib.math.Math2; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; import org.team340.lib.swerve.hardware.SwerveEncoders.SwerveEncoder; import org.team340.lib.swerve.hardware.SwerveEncoders.SwerveEncoder.HookStatus; import org.team340.lib.swerve.hardware.SwerveMotors.SwerveMotor; +import org.team340.lib.util.Math2; /** * An encapsulation of all hardware for a swerve module. @@ -139,7 +139,7 @@ public SwerveModuleState getNextTarget() { * @param state The state to apply to the module. */ public void applyState(SwerveModuleState state) { - if (state.speedMetersPerSecond < config.velDeadband) { + if (Math.abs(state.speedMetersPerSecond) < config.velDeadband) { state.speedMetersPerSecond = 0.0; state.angle = nextTarget.angle; } @@ -155,7 +155,7 @@ public void applyState(SwerveModuleState state) { } boolean flipped = false; - if (Math.abs(angleDelta.getRadians()) > Math.PI / 2.0) { + if (Math.abs(angleDelta.getRadians()) > Math2.HALF_PI) { state.speedMetersPerSecond *= -1.0; state.angle = state.angle.rotateBy(Rotation2d.kPi); flipped = true; diff --git a/src/main/java/org/team340/lib/swerve/SwerveTunables.java b/src/main/java/org/team340/lib/swerve/SwerveTunables.java index 2d33988..4311322 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveTunables.java +++ b/src/main/java/org/team340/lib/swerve/SwerveTunables.java @@ -9,7 +9,7 @@ final class SwerveTunables { private SwerveTunables() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** @@ -21,68 +21,64 @@ public static void initialize(String name, SwerveAPI api) { SwerveConfig config = api.config; // setTimings() - Tunable.doubleValue(name + "/timings/discretization", config.discretizationPeriod, v -> + Tunable.value(name + "/timings/discretization", config.discretizationPeriod, v -> config.discretizationPeriod = v ); // setMovePID() - Tunable.doubleValue(name + "/movePID/kP", config.movePID[0], v -> { + Tunable.value(name + "/movePID/kP", config.movePID[0], v -> { config.movePID[0] = v; reapplyGains(true, api); }); - Tunable.doubleValue(name + "/movePID/kI", config.movePID[1], v -> { + Tunable.value(name + "/movePID/kI", config.movePID[1], v -> { config.movePID[1] = v; reapplyGains(true, api); }); - Tunable.doubleValue(name + "/movePID/kD", config.movePID[2], v -> { + Tunable.value(name + "/movePID/kD", config.movePID[2], v -> { config.movePID[2] = v; reapplyGains(true, api); }); // setMoveFF() - Tunable.doubleValue(name + "/moveFF/kS", config.moveFF[0], v -> { + Tunable.value(name + "/moveFF/kS", config.moveFF[0], v -> { config.moveFF[0] = v; reapplyGains(true, api); }); - Tunable.doubleValue(name + "/moveFF/kV", config.moveFF[1], v -> { + Tunable.value(name + "/moveFF/kV", config.moveFF[1], v -> { config.moveFF[1] = v; reapplyGains(true, api); }); // setTurnPID() - Tunable.doubleValue(name + "/turnPID/kP", config.turnPID[0], v -> { + Tunable.value(name + "/turnPID/kP", config.turnPID[0], v -> { config.turnPID[0] = v; reapplyGains(false, api); }); - Tunable.doubleValue(name + "/turnPID/kI", config.turnPID[1], v -> { + Tunable.value(name + "/turnPID/kI", config.turnPID[1], v -> { config.turnPID[1] = v; reapplyGains(false, api); }); - Tunable.doubleValue(name + "/turnPID/kD", config.turnPID[2], v -> { + Tunable.value(name + "/turnPID/kD", config.turnPID[2], v -> { config.turnPID[2] = v; reapplyGains(false, api); }); // setLimits() - Tunable.doubleValue(name + "/limits/velocity", config.velocity, v -> config.velocity = v); - Tunable.doubleValue(name + "/limits/velDeadband", config.velDeadband, v -> config.velDeadband = v); - Tunable.doubleValue(name + "/limits/slipAccel", config.slipAccel, v -> config.slipAccel = v); - Tunable.doubleValue(name + "/limits/torqueAccel", config.torqueAccel, v -> config.torqueAccel = v); - Tunable.doubleValue(name + "/limits/angularAccel", config.angularAccel, v -> config.angularAccel = v); + Tunable.value(name + "/limits/velocity", config.velocity, v -> config.velocity = v); + Tunable.value(name + "/limits/velDeadband", config.velDeadband, v -> config.velDeadband = v); + Tunable.value(name + "/limits/slipAccel", config.slipAccel, v -> config.slipAccel = v); + Tunable.value(name + "/limits/torqueAccel", config.torqueAccel, v -> config.torqueAccel = v); + Tunable.value(name + "/limits/angularAccel", config.angularAccel, v -> config.angularAccel = v); // setDriverProfile() - Tunable.doubleValue(name + "/driverProfile/vel", config.driverVel, v -> config.driverVel = v); - Tunable.doubleValue(name + "/driverProfile/velExp", config.driverVelExp, v -> config.driverVelExp = v); - Tunable.doubleValue(name + "/driverProfile/velDeadband", config.driverVelDeadband, v -> - config.driverVelDeadband = v - ); - Tunable.doubleValue(name + "/driverProfile/angularVel", config.driverAngularVel, v -> - config.driverAngularVel = v - ); - Tunable.doubleValue(name + "/driverProfile/angularVelExp", config.driverAngularVelExp, v -> + Tunable.value(name + "/driverProfile/vel", config.driverVel, v -> config.driverVel = v); + Tunable.value(name + "/driverProfile/velExp", config.driverVelExp, v -> config.driverVelExp = v); + Tunable.value(name + "/driverProfile/velDeadband", config.driverVelDeadband, v -> config.driverVelDeadband = v); + Tunable.value(name + "/driverProfile/angularVel", config.driverAngularVel, v -> config.driverAngularVel = v); + Tunable.value(name + "/driverProfile/angularVelExp", config.driverAngularVelExp, v -> config.driverAngularVelExp = v ); - Tunable.doubleValue(name + "/driverProfile/angularVelDeadband", config.driverAngularVelDeadband, v -> + Tunable.value(name + "/driverProfile/angularVelDeadband", config.driverAngularVelDeadband, v -> config.driverAngularVelDeadband = v ); } diff --git a/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java b/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java index 9b27413..35c4c98 100644 --- a/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java +++ b/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java @@ -297,6 +297,16 @@ public SwerveConfig setModules(SwerveModuleConfig... modules) { * Throws an error if an issue is found. */ public void verify() { + var missing = missing(); + if (!missing.isEmpty()) { + throw new IllegalArgumentException("SwerveConfig missing values: " + String.join(", ", missing)); + } + } + + /** + * Collects a list of missing elements. + */ + private List missing() { List missing = new ArrayList<>(); if (period == -1.0) missing.add("Period"); if (odometryPeriod == -1.0) missing.add("Odometry Period"); @@ -327,14 +337,16 @@ public void verify() { if (wheelDiameter == -1.0) missing.add("Wheel Diameter"); if (odometryStdDevs == null) missing.add("Odometry Standard Deviations"); if (imu == null) missing.add("IMU"); - if (modules == null) missing.add("Modules"); - if (!missing.isEmpty()) { - throw new IllegalArgumentException("SwerveConfig missing values: " + String.join(", ", missing)); + if (modules == null || modules.length == 0) { + missing.add("Modules"); + } else { + for (int i = 0; i < modules.length; i++) { + String name = modules[i].name == null ? "Module " + i : modules[i].name; + missing.addAll(modules[i].missing().stream().map(s -> name + ": " + s).toList()); + } } - for (SwerveModuleConfig module : modules) { - module.verify(); - } + return missing; } } diff --git a/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java b/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java index e63c6e9..f7fe80b 100644 --- a/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java +++ b/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java @@ -75,6 +75,16 @@ public SwerveModuleConfig setEncoder(SwerveEncoder.Ctor encoder) { * Verifies the config. */ public void verify() { + var missing = missing(); + if (!missing.isEmpty()) { + throw new IllegalArgumentException("SwerveModuleConfig missing values: " + String.join(", ", missing)); + } + } + + /** + * Collects a list of missing elements. + */ + List missing() { List missing = new ArrayList<>(); if (name == null) missing.add("Name"); if (location == null) missing.add("Position"); @@ -82,8 +92,6 @@ public void verify() { if (turnMotor == null) missing.add("Turn Motor"); if (encoder == null) missing.add("Encoder"); - if (!missing.isEmpty()) { - throw new IllegalArgumentException("SwerveModuleConfig missing values: " + String.join(", ", missing)); - } + return missing; } } 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 866ab97..ac46a99 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java @@ -1,8 +1,6 @@ package org.team340.lib.swerve.hardware; import com.ctre.phoenix6.BaseStatusSignal; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.epilogue.logging.errors.ErrorHandler; import java.util.List; import org.team340.lib.swerve.SwerveAPI; @@ -12,13 +10,6 @@ interface SwerveBaseHardware extends AutoCloseable { */ public abstract Object getAPI(); - /** - * Logs the device via Epilogue. - * @param logger The logger to log data to. - * @param errorHandler The handler to use if logging raised an exception. - */ - public abstract void log(EpilogueBackend backend, ErrorHandler errorHandler); - /** * Returns all Phoenix status signals in use by the hardware. Phoenix * hardware should not invoke {@code .refresh()} on their status 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 926d1d0..0befdbe 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java @@ -17,16 +17,11 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.epilogue.logging.errors.ErrorHandler; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotBase; import java.util.List; import java.util.function.BiFunction; -import org.team340.lib.logging.phoenix.CANcoderLogger; -import org.team340.lib.logging.reduxlib.CanandmagLogger; -import org.team340.lib.logging.revlib.SparkAbsoluteEncoderLogger; import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.hardware.SwerveEncoders.SwerveEncoder.HookStatus; @@ -41,7 +36,7 @@ public final class SwerveEncoders { private SwerveEncoders() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** @@ -101,7 +96,6 @@ public static SwerveEncoder.Ctor sparkMaxEncoder(double offset, boolean inverted throw new UnsupportedOperationException("Turn motor is not a Spark Max"); } - var deviceLogger = new SparkAbsoluteEncoderLogger(); SparkAbsoluteEncoder encoder = sparkMax.getAbsoluteEncoder(); HookStatus hookStatus = new HookStatus(false, true); @@ -142,11 +136,6 @@ public Object getAPI() { return encoder; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, encoder, errorHandler); - } - @Override public void close() {} }; @@ -164,7 +153,6 @@ public static SwerveEncoder.Ctor sparkFlexEncoder(double offset, boolean inverte throw new UnsupportedOperationException("Turn motor is not a Spark Flex"); } - var deviceLogger = new SparkAbsoluteEncoderLogger(); SparkAbsoluteEncoder encoder = sparkFlex.getAbsoluteEncoder(); HookStatus hookStatus = new HookStatus(false, true); @@ -205,11 +193,6 @@ public Object getAPI() { return encoder; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, encoder, errorHandler); - } - @Override public void close() {} }; @@ -224,7 +207,6 @@ public void close() {} */ public static SwerveEncoder.Ctor canandmag(int id, double offset, boolean inverted) { return (config, turnMotor) -> { - var deviceLogger = new CanandmagLogger(); Canandmag canandmag = new Canandmag(id); var settings = new CanandmagSettings() @@ -249,11 +231,6 @@ public Object getAPI() { return canandmag; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, canandmag, errorHandler); - } - @Override public void close() { canandmag.close(); @@ -270,7 +247,6 @@ public void close() { */ public static SwerveEncoder.Ctor cancoder(int id, double offset, boolean inverted) { return (config, turnMotor) -> { - var deviceLogger = new CANcoderLogger(); CANcoder cancoder = new CANcoder(id, config.phoenixCanBus); HookStatus tempHookStatus = new HookStatus(false, false); @@ -332,11 +308,6 @@ public Object getAPI() { return cancoder; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, cancoder, errorHandler); - } - @Override public List getSignals() { return List.of(position, velocity); @@ -373,13 +344,6 @@ public Object getAPI() { return encoder.getAPI(); } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - encoder.log(backend, errorHandler); - var sim = backend.getNested(".sim"); - sim.log("position", getPosition()); - } - @Override public List getSignals() { return encoder.getSignals(); 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 d05da7e..1a16e49 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java @@ -5,8 +5,6 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.reduxrobotics.sensors.canandgyro.Canandgyro; import com.reduxrobotics.sensors.canandgyro.CanandgyroSettings; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.epilogue.logging.errors.ErrorHandler; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; @@ -19,9 +17,6 @@ import java.util.List; import java.util.function.Consumer; import java.util.function.Function; -import org.team340.lib.logging.phoenix.Pigeon2Logger; -import org.team340.lib.logging.reduxlib.CanandgyroLogger; -import org.team340.lib.logging.wpilibj.ADIS16470Logger; import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.hardware.SwerveIMUs.SwerveIMU.IMUSimHook; @@ -35,7 +30,7 @@ public final class SwerveIMUs { private SwerveIMUs() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** @@ -100,7 +95,6 @@ public static SwerveIMU.Ctor adis16470( CalibrationTime calibrationTime ) { return config -> { - var deviceLogger = new ADIS16470Logger(); ADIS16470_IMU adis16470 = new ADIS16470_IMU(yawAxis, pitchAxis, rollAxis, port, calibrationTime); return new SwerveIMU() { @@ -124,11 +118,6 @@ public Object getAPI() { return adis16470; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, adis16470, errorHandler); - } - @Override public void close() { adis16470.close(); @@ -143,7 +132,6 @@ public void close() { */ public static SwerveIMU.Ctor canandgyro(int id) { return config -> { - var deviceLogger = new CanandgyroLogger(); Canandgyro canandgyro = new Canandgyro(id); var settings = new CanandgyroSettings() @@ -189,11 +177,6 @@ public Object getAPI() { return canandgyro; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, canandgyro, errorHandler); - } - @Override public void close() { canandgyro.close(); @@ -208,7 +191,6 @@ public void close() { */ public static SwerveIMU.Ctor pigeon2(int id) { return config -> { - var deviceLogger = new Pigeon2Logger(); Pigeon2 pigeon2 = new Pigeon2(id, config.phoenixCanBus); StatusSignal yaw = pigeon2.getYaw().clone(); @@ -261,11 +243,6 @@ public Object getAPI() { return pigeon2; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, pigeon2, errorHandler); - } - @Override public List getSignals() { return List.of(yaw, yawVelocity); @@ -312,15 +289,6 @@ public Object getAPI() { return imu.getAPI(); } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - imu.log(backend, errorHandler); - var sim = backend.getNested(".sim"); - sim.log("yaw", getYaw(), Rotation2d.struct); - sim.log("pitch", getPitch(), Rotation2d.struct); - sim.log("roll", getRoll(), Rotation2d.struct); - } - @Override public List getSignals() { return imu.getSignals(); 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 585f6b5..8f21834 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java @@ -22,16 +22,11 @@ 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.EpilogueBackend; -import edu.wpi.first.epilogue.logging.errors.ErrorHandler; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotBase; import java.util.List; import java.util.function.BiFunction; -import org.team340.lib.logging.phoenix.TalonFXLogger; -import org.team340.lib.logging.revlib.SparkFlexLogger; -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.vendors.PhoenixUtil; @@ -43,7 +38,7 @@ public final class SwerveMotors { private SwerveMotors() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** @@ -112,7 +107,6 @@ public static SwerveMotor construct(Ctor ctor, SwerveConfig config, boolean isMo */ public static SwerveMotor.Ctor sparkMax(int id, boolean inverted) { return (config, isMoveMotor) -> { - var deviceLogger = new SparkMaxLogger(); SparkMax sparkMax = new SparkMax(id, MotorType.kBrushless); RelativeEncoder relativeEncoder = sparkMax.getEncoder(); SparkClosedLoopController pid = sparkMax.getClosedLoopController(); @@ -202,11 +196,6 @@ public Object getAPI() { return sparkMax; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, sparkMax, errorHandler); - } - @Override public boolean readError() { return !sparkMax.getLastError().equals(REVLibError.kOk); @@ -227,7 +216,6 @@ public void close() { */ public static SwerveMotor.Ctor sparkFlex(int id, boolean inverted) { return (config, isMoveMotor) -> { - var deviceLogger = new SparkFlexLogger(); SparkFlex sparkFlex = new SparkFlex(id, MotorType.kBrushless); RelativeEncoder relativeEncoder = sparkFlex.getEncoder(); SparkClosedLoopController pid = sparkFlex.getClosedLoopController(); @@ -321,11 +309,6 @@ public Object getAPI() { return sparkFlex; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, sparkFlex, errorHandler); - } - @Override public boolean readError() { return !sparkFlex.getLastError().equals(REVLibError.kOk); @@ -346,7 +329,6 @@ public void close() { */ public static SwerveMotor.Ctor talonFX(int id, boolean inverted) { return (config, isMoveMotor) -> { - var deviceLogger = new TalonFXLogger(); TalonFX talonFX = new TalonFX(id, config.phoenixCanBus); StatusSignal position = talonFX.getPosition().clone(); @@ -448,11 +430,6 @@ public Object getAPI() { return talonFX; } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - deviceLogger.tryUpdate(backend, talonFX, errorHandler); - } - @Override public List getSignals() { return List.of(position, velocity); @@ -519,14 +496,6 @@ public Object getAPI() { return motor.getAPI(); } - @Override - public void log(EpilogueBackend backend, ErrorHandler errorHandler) { - motor.log(backend, errorHandler); - var sim = backend.getNested(".sim"); - sim.log("position", getPosition()); - sim.log("velocity", getVelocity()); - } - @Override public List getSignals() { return motor.getSignals(); diff --git a/src/main/java/org/team340/lib/util/Alliance.java b/src/main/java/org/team340/lib/util/Alliance.java index 4a44229..3329def 100644 --- a/src/main/java/org/team340/lib/util/Alliance.java +++ b/src/main/java/org/team340/lib/util/Alliance.java @@ -9,7 +9,7 @@ public final class Alliance { private Alliance() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** diff --git a/src/main/java/org/team340/lib/util/DisableWatchdog.java b/src/main/java/org/team340/lib/util/DisableWatchdog.java index 9ae11ec..77b48f1 100644 --- a/src/main/java/org/team340/lib/util/DisableWatchdog.java +++ b/src/main/java/org/team340/lib/util/DisableWatchdog.java @@ -10,7 +10,7 @@ public final class DisableWatchdog { private DisableWatchdog() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** diff --git a/src/main/java/org/team340/lib/util/FieldFlip.java b/src/main/java/org/team340/lib/util/FieldFlip.java new file mode 100644 index 0000000..0e35784 --- /dev/null +++ b/src/main/java/org/team340/lib/util/FieldFlip.java @@ -0,0 +1,135 @@ +package org.team340.lib.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.function.Supplier; + +/** + * Utility class for flipping field locations across lines of symmetry. + * By default, {@link FieldInfo} is utilized to retrieve field dimensions. + */ +public final class FieldFlip { + + private FieldFlip() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + private static double length = FieldInfo.length(); + private static double width = FieldInfo.width(); + + /** + * Overrides the default field dimensions. This method + * should be called before all other user code. + * @param length The field's length, in meters. + * @param width The field's width, in meters. + */ + public static void setFieldDimensions(double length, double width) { + FieldFlip.length = length; + FieldFlip.width = width; + } + + /** + * Flips the provided {@link Translation2d} over the + * length of the field (i.e. hamburger style). + * @param translation The translation to flip. + */ + public static Translation2d overLength(Translation2d translation) { + return new Translation2d(length - translation.getX(), translation.getY()); + } + + /** + * Flips the provided {@link Rotation2d} over the + * length of the field (i.e. hamburger style). + * @param rotation The rotation to flip. + */ + public static Rotation2d overLength(Rotation2d rotation) { + return new Rotation2d(-rotation.getCos(), rotation.getSin()); + } + + /** + * Flips the provided {@link Pose2d} over the + * length of the field (i.e. hamburger style). + * @param pose The pose to flip. + */ + public static Pose2d overLength(Pose2d pose) { + return new Pose2d(overLength(pose.getTranslation()), overLength(pose.getRotation())); + } + + /** + * Flips the provided {@link Translation2d} over the + * width of the field (i.e. hotdog style). + * @param translation The translation to flip. + */ + public static Translation2d overWidth(Translation2d translation) { + return new Translation2d(translation.getX(), width - translation.getY()); + } + + /** + * Flips the provided {@link Rotation2d} over the + * width of the field (i.e. hotdog style). + * @param rotation The rotation to flip. + */ + public static Rotation2d overWidth(Rotation2d rotation) { + return rotation.unaryMinus(); + } + + /** + * Flips the provided {@link Pose2d} over the + * width of the field (i.e. hotdog style). + * @param pose The pose to flip. + */ + public static Pose2d overWidth(Pose2d pose) { + return new Pose2d(overWidth(pose.getTranslation()), overWidth(pose.getRotation())); + } + + /** + * Flips the provided {@link Translation2d} over the field's + * diagonal (i.e. rotated 180deg around the field's center). + * @param translation The translation to flip. + */ + public static Translation2d overDiagonal(Translation2d translation) { + return new Translation2d(length - translation.getX(), width - translation.getY()); + } + + /** + * Flips the provided {@link Rotation2d} over the field's + * diagonal (i.e. rotated 180deg around the field's center). + * @param rotation The rotation to flip. + */ + public static Rotation2d overDiagonal(Rotation2d rotation) { + return new Rotation2d(-rotation.getCos(), -rotation.getSin()); + } + + /** + * Flips the provided {@link Pose2d} over the field's + * diagonal (i.e. rotated 180deg around the field's center). + * @param pose The pose to flip. + */ + public static Pose2d overDiagonal(Pose2d pose) { + return new Pose2d(overDiagonal(pose.getTranslation()), overDiagonal(pose.getRotation())); + } + + /** + * Creates a supplier that returns the provided blue origin relative + * {@link Pose2d}, optionally flipped over the length of the field + * (i.e. hamburger style) if the robot is on the red alliance. + * @param pose The blue origin relative pose. + */ + public static Supplier allianceLength(Pose2d pose) { + Pose2d flipped = overLength(pose); + return () -> Alliance.isBlue() ? pose : flipped; + } + + /** + * Creates a supplier that returns the provided blue origin relative + * {@link Pose2d}, optionally flipped over the field's diagonal + * (i.e. rotated 180deg around the field's center) if the robot + * is on the red alliance. + * @param pose The blue origin relative pose. + */ + public static Supplier allianceDiagonal(Pose2d pose) { + Pose2d flipped = overDiagonal(pose); + return () -> Alliance.isBlue() ? pose : flipped; + } +} diff --git a/src/main/java/org/team340/lib/util/FieldInfo.java b/src/main/java/org/team340/lib/util/FieldInfo.java new file mode 100644 index 0000000..3cf47f0 --- /dev/null +++ b/src/main/java/org/team340/lib/util/FieldInfo.java @@ -0,0 +1,64 @@ +package org.team340.lib.util; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.wpilibj.DriverStation; + +/** + * Retrieves information about the playing field from an AprilTag field + * layout. By default, {@link AprilTagFields#kDefaultField} is used. + */ +public final class FieldInfo { + + private FieldInfo() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + private static AprilTagFieldLayout layout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + /** + * Loads a new AprilTag layout to retrieve field information from. + * This method should be called before all other user code. + * @param field The loadable AprilTag field layout. + */ + public static void setLayout(AprilTagFields field) { + layout = AprilTagFieldLayout.loadField(field); + } + + /** + * Loads a new AprilTag layout to retrieve field information from. + * This method should be called before all other user code. + * @param resourcePath The absolute path of the resource. + */ + public static void setLayout(String resourcePath) { + try { + layout = AprilTagFieldLayout.loadFromResource(resourcePath); + } catch (Exception e) { + DriverStation.reportError( + "[AprilTags] Unable to load layout from resource \"" + resourcePath + "\": " + e.getMessage(), + true + ); + } + } + + /** + * Returns the length of the field in meters. + */ + public static double length() { + return layout.getFieldLength(); + } + + /** + * Returns thr width of the field in meters. + */ + public static double width() { + return layout.getFieldWidth(); + } + + /** + * Returns the current field's AprilTag layout. + */ + public static AprilTagFieldLayout aprilTags() { + return layout; + } +} diff --git a/src/main/java/org/team340/lib/math/Math2.java b/src/main/java/org/team340/lib/util/Math2.java similarity index 85% rename from src/main/java/org/team340/lib/math/Math2.java rename to src/main/java/org/team340/lib/util/Math2.java index 720dc4c..b46a9d0 100644 --- a/src/main/java/org/team340/lib/math/Math2.java +++ b/src/main/java/org/team340/lib/util/Math2.java @@ -1,4 +1,4 @@ -package org.team340.lib.math; +package org.team340.lib.util; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; @@ -15,10 +15,25 @@ public final class Math2 { private Math2() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } - public static final double PI2 = 6.283185307179586; + /** {@code PI/6} (30deg) */ + public static final double SIXTH_PI = 0.5235987755982989; + /** {@code PI/4} (45deg) */ + public static final double QUARTER_PI = 0.7853981633974483; + /** {@code PI/3} (60deg) */ + public static final double THIRD_PI = 1.0471975511965979; + /** {@code PI/2} (90deg) */ + public static final double HALF_PI = 1.5707963267948966; + /** {@code 2PI/3} (120deg) */ + public static final double TWO_THIRDS_PI = 2.0943951023931957; + /** {@code 3PI/4} (135deg) */ + public static final double THREE_QUARTERS_PI = 2.356194490192345; + /** {@code 5PI/6} (150deg) */ + public static final double FIVE_SIXTHS_PI = 2.6179938779914944; + /** {@code PI*2} (360deg) */ + public static final double TWO_PI = 6.283185307179586; /** * Discretizes a continuous-time chassis speed in place. diff --git a/src/main/java/org/team340/lib/math/PAPFController.java b/src/main/java/org/team340/lib/util/PAPFController.java similarity index 96% rename from src/main/java/org/team340/lib/math/PAPFController.java rename to src/main/java/org/team340/lib/util/PAPFController.java index a693c70..c4f8592 100644 --- a/src/main/java/org/team340/lib/math/PAPFController.java +++ b/src/main/java/org/team340/lib/util/PAPFController.java @@ -1,4 +1,4 @@ -package org.team340.lib.math; +package org.team340.lib.util; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Strategy; @@ -10,7 +10,6 @@ import java.util.ArrayList; import java.util.Collections; import java.util.List; -import org.team340.lib.util.Tunable; /** * Implements a predictive artificial potential field (P-APF), @@ -27,6 +26,7 @@ public final class PAPFController { private final Obstacle[] obstacles; private double resolution; private double horizon; + private double tolerance; private boolean savePrediction; // spotless:off @@ -47,13 +47,21 @@ public final class PAPFController { * @param resolution The resolution at which to sample the horizon in meters. Higher * resolutions (i.e. smaller values) will increase accuracy of * predicted motion, however are more computationally expensive. + * @param tolerance The distance from the goal in meters at which to return a velocity output of 0. * @param savePrediction If the robot's predicted motion should be saved. Useful * for debugging, however does increase GC pressure. * @param obstacles The field's obstacles. */ - public PAPFController(double horizon, double resolution, boolean savePrediction, Obstacle[] obstacles) { + public PAPFController( + double horizon, + double resolution, + double tolerance, + boolean savePrediction, + Obstacle[] obstacles + ) { this.horizon = horizon; this.resolution = resolution; + this.tolerance = tolerance; this.savePrediction = savePrediction; this.obstacles = obstacles; @@ -97,6 +105,7 @@ public List getPrediction() { * @param goal The desired position of the robot. * @param maxVelocity The desired cruise velocity of the robot, in m/s. * @param maxDeceleration The desired deceleration rate of the robot, in m/s/s. + * */ public ChassisSpeeds calculate(Pose2d currentPose, Translation2d goal, double maxVelocity, double maxDeceleration) { this.goal = new Pose2d(goal, currentPose.getRotation()); @@ -108,7 +117,7 @@ public ChassisSpeeds calculate(Pose2d currentPose, Translation2d goal, double ma double e_y = goal.getY() - currentPose.getY(); double error = Math.hypot(e_x, e_y); - if (error < 1e-6) { + if (error <= tolerance) { setpoint = this.goal; return new ChassisSpeeds(); } @@ -197,9 +206,10 @@ private void forceAt(double x, double y, Translation2d goal, NetForce netForce) * @param name The parent name for the tunables in NetworkTables. */ public void enableTunables(String name) { - Tunable.doubleValue(name + "/horizon", horizon, v -> horizon = v); - Tunable.doubleValue(name + "/resolution", resolution, v -> resolution = v); - Tunable.booleanValue(name + "/savePrediction", savePrediction, v -> savePrediction = v); + Tunable.value(name + "/horizon", horizon, v -> horizon = v); + Tunable.value(name + "/resolution", resolution, v -> resolution = v); + Tunable.value(name + "/tolerance", tolerance, v -> tolerance = v); + Tunable.value(name + "/savePrediction", savePrediction, v -> savePrediction = v); } /** diff --git a/src/main/java/org/team340/lib/util/Sleep.java b/src/main/java/org/team340/lib/util/Sleep.java index b0b0227..8b2289d 100644 --- a/src/main/java/org/team340/lib/util/Sleep.java +++ b/src/main/java/org/team340/lib/util/Sleep.java @@ -12,7 +12,7 @@ public final class Sleep { private Sleep() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** diff --git a/src/main/java/org/team340/lib/util/Tunable.java b/src/main/java/org/team340/lib/util/Tunable.java index 6f78bfd..0a678ce 100644 --- a/src/main/java/org/team340/lib/util/Tunable.java +++ b/src/main/java/org/team340/lib/util/Tunable.java @@ -28,6 +28,7 @@ import java.util.function.Consumer; import java.util.function.DoubleConsumer; import java.util.function.IntConsumer; +import org.team340.lib.logging.LoggedRobot; import org.team340.lib.util.vendors.RevUtil; /** @@ -37,15 +38,15 @@ public final class Tunable { private Tunable() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("/Tunables"); private static final EventLoop pollChanges = new EventLoop(); /** - * Polls changes from NetworkTables. Must be called - * periodically in order for this class to function. + * Polls changes from NetworkTables. Must be called periodically in order for + * this class to function (this is already done if utilizing {@link LoggedRobot}). */ public static void update() { pollChanges.poll(); @@ -56,8 +57,8 @@ public static void update() { * @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); + public static TunableBoolean value(String name, boolean defaultValue) { + return value(name, defaultValue, null); } /** @@ -66,7 +67,7 @@ public static TunableBoolean booleanValue(String name, boolean defaultValue) { * @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) { + public static TunableBoolean value(String name, boolean defaultValue, BooleanConsumer onChange) { return new TunableBoolean(name, defaultValue, onChange); } @@ -96,7 +97,7 @@ private TunableBoolean(String name, boolean defaultValue, BooleanConsumer onChan /** * Returns the value of the tunable. */ - public boolean value() { + public boolean get() { return value; } @@ -120,8 +121,8 @@ public void close() { * @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); + public static TunableInteger value(String name, int defaultValue) { + return value(name, defaultValue, (IntConsumer) null); } /** @@ -130,7 +131,7 @@ public static TunableInteger intValue(String name, int defaultValue) { * @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) { + public static TunableInteger value(String name, int defaultValue, IntConsumer onChange) { return new TunableInteger(name, defaultValue, onChange); } @@ -160,7 +161,7 @@ private TunableInteger(String name, int defaultValue, IntConsumer onChange) { /** * Returns the value of the tunable. */ - public int value() { + public int get() { return value; } @@ -184,8 +185,8 @@ public void close() { * @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); + public static TunableFloat value(String name, float defaultValue) { + return value(name, defaultValue, (FloatConsumer) null); } /** @@ -194,7 +195,7 @@ public static TunableFloat floatValue(String name, float defaultValue) { * @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) { + public static TunableFloat value(String name, float defaultValue, FloatConsumer onChange) { return new TunableFloat(name, defaultValue, onChange); } @@ -224,7 +225,7 @@ private TunableFloat(String name, float defaultValue, FloatConsumer onChange) { /** * Returns the value of the tunable. */ - public float value() { + public float get() { return value; } @@ -248,8 +249,8 @@ public void close() { * @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); + public static TunableDouble value(String name, double defaultValue) { + return value(name, defaultValue, (DoubleConsumer) null); } /** @@ -258,7 +259,7 @@ public static TunableDouble doubleValue(String name, double defaultValue) { * @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) { + public static TunableDouble value(String name, double defaultValue, DoubleConsumer onChange) { return new TunableDouble(name, defaultValue, onChange); } @@ -288,7 +289,7 @@ private TunableDouble(String name, double defaultValue, DoubleConsumer onChange) /** * Returns the value of the tunable. */ - public double value() { + public double get() { return value; } @@ -312,8 +313,8 @@ public void close() { * @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); + public static TunableString value(String name, String defaultValue) { + return value(name, defaultValue, null); } /** @@ -322,7 +323,7 @@ public static TunableString stringValue(String name, String defaultValue) { * @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) { + public static TunableString value(String name, String defaultValue, Consumer onChange) { return new TunableString(name, defaultValue, onChange); } @@ -352,7 +353,7 @@ private TunableString(String name, String defaultValue, Consumer onChang /** * Returns the value of the tunable. */ - public String value() { + public String get() { return value; } @@ -377,10 +378,10 @@ public void close() { * @param controller The PID controller. */ public static void pidController(String name, PIDController controller) { - doubleValue(name + "/kP", controller.getP(), controller::setP); - doubleValue(name + "/kI", controller.getI(), controller::setI); - doubleValue(name + "/kD", controller.getD(), controller::setD); - doubleValue(name + "/iZone", controller.getIZone(), controller::setIZone); + value(name + "/kP", controller.getP(), controller::setP); + value(name + "/kI", controller.getI(), controller::setI); + value(name + "/kD", controller.getD(), controller::setD); + value(name + "/iZone", controller.getIZone(), controller::setIZone); } /** @@ -389,14 +390,14 @@ public static void pidController(String name, PIDController controller) { * @param controller The PID controller. */ public static void pidController(String name, ProfiledPIDController controller) { - doubleValue(name + "/kP", controller.getP(), controller::setP); - doubleValue(name + "/kI", controller.getI(), controller::setI); - doubleValue(name + "/kD", controller.getD(), controller::setD); - doubleValue(name + "/iZone", controller.getIZone(), controller::setIZone); - doubleValue(name + "/maxVelocity", controller.getConstraints().maxVelocity, v -> + value(name + "/kP", controller.getP(), controller::setP); + value(name + "/kI", controller.getI(), controller::setI); + value(name + "/kD", controller.getD(), controller::setD); + value(name + "/iZone", controller.getIZone(), controller::setIZone); + value(name + "/maxVelocity", controller.getConstraints().maxVelocity, v -> controller.setConstraints(new TrapezoidProfile.Constraints(v, controller.getConstraints().maxAcceleration)) ); - doubleValue(name + "/maxAcceleration", controller.getConstraints().maxAcceleration, v -> + value(name + "/maxAcceleration", controller.getConstraints().maxAcceleration, v -> controller.setConstraints(new TrapezoidProfile.Constraints(controller.getConstraints().maxVelocity, v)) ); } @@ -419,42 +420,42 @@ public static void pidController(String name, SparkMax spark) { public static void pidController(String name, SparkMax spark, ClosedLoopSlot slot) { var config = spark.configAccessor.closedLoop; - doubleValue(name + "/kP", config.getP(slot), v -> { + value(name + "/kP", config.getP(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.p(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kI", config.getI(slot), v -> { + value(name + "/kI", config.getI(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.i(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kD", config.getD(slot), v -> { + value(name + "/kD", config.getD(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.d(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kV", config.getFF(slot), v -> { + value(name + "/kV", config.getFF(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.velocityFF(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/iZone", config.getIZone(slot), v -> { + value(name + "/iZone", config.getIZone(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.iZone(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/dFilter", config.getDFilter(slot), v -> { + value(name + "/dFilter", config.getDFilter(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.dFilter(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/minOutput", config.getMinOutput(slot), v -> { + value(name + "/minOutput", config.getMinOutput(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.minOutput(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/maxOutput", config.getMaxOutput(slot), v -> { + value(name + "/maxOutput", config.getMaxOutput(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.maxOutput(v, slot); RevUtil.configEphemeral(spark, newConfig); @@ -479,42 +480,42 @@ public static void pidController(String name, SparkFlex spark) { public static void pidController(String name, SparkFlex spark, ClosedLoopSlot slot) { var config = spark.configAccessor.closedLoop; - doubleValue(name + "/kP", config.getP(slot), v -> { + value(name + "/kP", config.getP(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.p(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kI", config.getI(slot), v -> { + value(name + "/kI", config.getI(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.i(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kD", config.getD(slot), v -> { + value(name + "/kD", config.getD(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.d(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/kV", config.getFF(slot), v -> { + value(name + "/kV", config.getFF(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.velocityFF(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/iZone", config.getIZone(slot), v -> { + value(name + "/iZone", config.getIZone(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.iZone(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/dFilter", config.getDFilter(slot), v -> { + value(name + "/dFilter", config.getDFilter(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.dFilter(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/minOutput", config.getMinOutput(slot), v -> { + value(name + "/minOutput", config.getMinOutput(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.minOutput(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/maxOutput", config.getMaxOutput(slot), v -> { + value(name + "/maxOutput", config.getMaxOutput(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.maxOutput(v, slot); RevUtil.configEphemeral(spark, newConfig); @@ -531,37 +532,37 @@ public static void pidController(String name, TalonFX talonFX) { Slot0Configs config = new Slot0Configs(); talonFX.getConfigurator().refresh(config); - doubleValue(name + "/kP", config.kP, v -> { + value(name + "/kP", config.kP, v -> { talonFX.getConfigurator().refresh(config); config.kP = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kI", config.kI, v -> { + value(name + "/kI", config.kI, v -> { talonFX.getConfigurator().refresh(config); config.kI = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kD", config.kD, v -> { + value(name + "/kD", config.kD, v -> { talonFX.getConfigurator().refresh(config); config.kD = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kS", config.kS, v -> { + value(name + "/kS", config.kS, v -> { talonFX.getConfigurator().refresh(config); config.kS = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kV", config.kV, v -> { + value(name + "/kV", config.kV, v -> { talonFX.getConfigurator().refresh(config); config.kV = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kA", config.kA, v -> { + value(name + "/kA", config.kA, v -> { talonFX.getConfigurator().refresh(config); config.kA = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/kG", config.kG, v -> { + value(name + "/kG", config.kG, v -> { talonFX.getConfigurator().refresh(config); config.kG = v; talonFX.getConfigurator().apply(config); @@ -586,17 +587,17 @@ public static void motionProfile(String name, SparkMax spark) { public static void motionProfile(String name, SparkMax spark, ClosedLoopSlot slot) { var config = spark.configAccessor.closedLoop.maxMotion; - doubleValue(name + "/velocity", config.getMaxVelocity(slot), v -> { + value(name + "/velocity", config.getMaxVelocity(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.maxMotion.maxVelocity(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/acceleration", config.getMaxAcceleration(slot), v -> { + value(name + "/acceleration", config.getMaxAcceleration(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.maxMotion.maxAcceleration(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/allowedClosedLoopError", config.getAllowedClosedLoopError(slot), v -> { + value(name + "/allowedClosedLoopError", config.getAllowedClosedLoopError(slot), v -> { var newConfig = new SparkMaxConfig(); newConfig.closedLoop.maxMotion.allowedClosedLoopError(v, slot); RevUtil.configEphemeral(spark, newConfig); @@ -621,17 +622,17 @@ public static void motionProfile(String name, SparkFlex spark) { public static void motionProfile(String name, SparkFlex spark, ClosedLoopSlot slot) { var config = spark.configAccessor.closedLoop.maxMotion; - doubleValue(name + "/velocity", config.getMaxVelocity(slot), v -> { + value(name + "/velocity", config.getMaxVelocity(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.maxMotion.maxVelocity(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/acceleration", config.getMaxAcceleration(slot), v -> { + value(name + "/acceleration", config.getMaxAcceleration(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.maxMotion.maxAcceleration(v, slot); RevUtil.configEphemeral(spark, newConfig); }); - doubleValue(name + "/allowedClosedLoopError", config.getAllowedClosedLoopError(slot), v -> { + value(name + "/allowedClosedLoopError", config.getAllowedClosedLoopError(slot), v -> { var newConfig = new SparkFlexConfig(); newConfig.closedLoop.maxMotion.allowedClosedLoopError(v, slot); RevUtil.configEphemeral(spark, newConfig); @@ -647,27 +648,27 @@ public static void motionProfile(String name, TalonFX talonFX) { MotionMagicConfigs config = new MotionMagicConfigs(); talonFX.getConfigurator().refresh(config); - doubleValue(name + "/velocity", config.MotionMagicCruiseVelocity, v -> { + value(name + "/velocity", config.MotionMagicCruiseVelocity, v -> { talonFX.getConfigurator().refresh(config); config.MotionMagicCruiseVelocity = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/acceleration", config.MotionMagicAcceleration, v -> { + value(name + "/acceleration", config.MotionMagicAcceleration, v -> { talonFX.getConfigurator().refresh(config); config.MotionMagicAcceleration = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/jerk", config.MotionMagicJerk, v -> { + value(name + "/jerk", config.MotionMagicJerk, v -> { talonFX.getConfigurator().refresh(config); config.MotionMagicJerk = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/expoKv", config.MotionMagicExpo_kV, v -> { + value(name + "/expoKv", config.MotionMagicExpo_kV, v -> { talonFX.getConfigurator().refresh(config); config.MotionMagicExpo_kV = v; talonFX.getConfigurator().apply(config); }); - doubleValue(name + "/expoKa", config.MotionMagicExpo_kA, v -> { + value(name + "/expoKa", config.MotionMagicExpo_kA, v -> { talonFX.getConfigurator().refresh(config); config.MotionMagicExpo_kA = v; talonFX.getConfigurator().apply(config); @@ -680,6 +681,6 @@ public static void motionProfile(String name, TalonFX talonFX) { * @param debouncer The debouncer to tune. */ public static void debounce(String name, Debouncer debouncer) { - doubleValue(name, debouncer.getDebounceTime(), debouncer::setDebounceTime); + value(name, debouncer.getDebounceTime(), debouncer::setDebounceTime); } } diff --git a/src/main/java/org/team340/lib/util/AutoChooser.java b/src/main/java/org/team340/lib/util/command/AutoChooser.java similarity index 62% rename from src/main/java/org/team340/lib/util/AutoChooser.java rename to src/main/java/org/team340/lib/util/command/AutoChooser.java index 990939d..c438050 100644 --- a/src/main/java/org/team340/lib/util/AutoChooser.java +++ b/src/main/java/org/team340/lib/util/command/AutoChooser.java @@ -1,4 +1,4 @@ -package org.team340.lib.util; +package org.team340.lib.util.command; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Strategy; @@ -7,11 +7,11 @@ import edu.wpi.first.networktables.StringArrayPublisher; import edu.wpi.first.networktables.StringPublisher; import edu.wpi.first.networktables.StringSubscriber; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import java.util.HashMap; import java.util.Map; @@ -20,9 +20,9 @@ * Similar to {@link SendableChooser}, this class implements a selector over * NetworkTables for choosing a command to run during the autonomous period. * - *

For this class to function, you must either bind it to a command - * scheduler using {@link AutoChooser#bind(CommandScheduler)}, or call - * {@link AutoChooser#update()} periodically. + *

The AutoChooser binds itself to the default {@link CommandScheduler} + * instance, and will automatically schedule the selected command at the + * beginning of autonomous. */ @Logged(strategy = Strategy.OPT_IN) public final class AutoChooser { @@ -34,15 +34,29 @@ public final class AutoChooser { private final StringSubscriber selectedSub; private final Map options = new HashMap<>(); + private final boolean renameCommands; private String activeName = DEFAULT; private Command activeCommand = Commands.none(); + private boolean newSelection = false; /** - * Create the auto chooser. + * Creates an auto chooser. + */ + public AutoChooser() { + this("Autos", true); + } + + /** + * Creates an auto chooser. * @param name The name for the chooser in NetworkTables. + * @param renameCommands If the specified names of autonomous options should + * be applied to their corresponding commands when + * adding them to the chooser. */ - public AutoChooser(String name) { + public AutoChooser(String name, boolean renameCommands) { + this.renameCommands = renameCommands; + NetworkTable nt = NetworkTableInstance.getDefault().getTable(name); nt.getStringTopic(".type").publish().set("String Chooser"); @@ -51,10 +65,13 @@ public AutoChooser(String name) { optionsPub = nt.getStringArrayTopic("options").publish(); activePub = nt.getStringTopic("active").publish(); - selectedSub = nt.getStringTopic("selected").subscribe("Do Nothing"); + selectedSub = nt.getStringTopic("selected").subscribe(DEFAULT); add(activeName, activeCommand); activePub.set(activeName); + + CommandScheduler.getInstance().getDefaultButtonLoop().bind(this::update); + RobotModeTriggers.autonomous().whileTrue(Commands.deferredProxy(() -> activeCommand)); } /** @@ -64,39 +81,41 @@ public Command getSelected() { return activeCommand; } + /** + * Returns a {@link Trigger} that rises {@code true} + * for one cycle after an auto is selected. + */ + public Trigger newSelection() { + return new Trigger(() -> newSelection); + } + /** * Add an option to the chooser. * @param name The name of the option. Must be unique. * @param command The option's command. + * @return A {@link Trigger} that is {@code true} when the option is selected. */ - public void add(String name, Command command) { + public Trigger add(String name, Command command) { + if (renameCommands) command.setName(name); + options.put(name, command); optionsPub.set(options.keySet().toArray(String[]::new)); - } - - /** - * Binds the auto chooser to the default button loop of the provided command - * scheduler. This will automatically update the chooser, as well as schedule - * the selected command during the autonomous period. - * @param scheduler The command scheduler to bind to. - */ - public void bind(CommandScheduler scheduler) { - scheduler.getDefaultButtonLoop().bind(this::update); - new Trigger(scheduler.getDefaultButtonLoop(), DriverStation::isAutonomousEnabled).whileTrue( - Commands.deferredProxy(() -> activeCommand) - ); + return new Trigger(() -> activeName.equals(name)); } /** * Updates the auto chooser. You do not need to call this method * when using {@link AutoChooser#bind(CommandScheduler)}. */ - public void update() { + private void update() { String selected = selectedSub.get(); if (!selected.equals(activeName)) { activeName = options.containsKey(selected) ? selected : DEFAULT; activeCommand = options.get(activeName); activePub.set(activeName); + newSelection = true; + } else { + newSelection = false; } } } diff --git a/src/main/java/org/team340/lib/util/vendors/PhoenixUtil.java b/src/main/java/org/team340/lib/util/vendors/PhoenixUtil.java index 3e490c8..3407fa9 100644 --- a/src/main/java/org/team340/lib/util/vendors/PhoenixUtil.java +++ b/src/main/java/org/team340/lib/util/vendors/PhoenixUtil.java @@ -10,7 +10,7 @@ public final class PhoenixUtil { private PhoenixUtil() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** diff --git a/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java b/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java index 931551e..62ae6cc 100644 --- a/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java +++ b/src/main/java/org/team340/lib/util/vendors/ReduxUtil.java @@ -20,7 +20,7 @@ public final class ReduxUtil { private ReduxUtil() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** diff --git a/src/main/java/org/team340/lib/util/vendors/RevUtil.java b/src/main/java/org/team340/lib/util/vendors/RevUtil.java index 37902a9..a460de5 100644 --- a/src/main/java/org/team340/lib/util/vendors/RevUtil.java +++ b/src/main/java/org/team340/lib/util/vendors/RevUtil.java @@ -17,7 +17,7 @@ public final class RevUtil { private RevUtil() { - throw new AssertionError("This is a utility class!"); + throw new UnsupportedOperationException("This is a utility class!"); } /** diff --git a/src/main/java/org/team340/robot/Robot.java b/src/main/java/org/team340/robot/Robot.java index a4f0b4e..f3a3a83 100644 --- a/src/main/java/org/team340/robot/Robot.java +++ b/src/main/java/org/team340/robot/Robot.java @@ -2,24 +2,20 @@ import static edu.wpi.first.wpilibj2.command.Commands.*; -import com.ctre.phoenix6.SignalLogger; -import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; -import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Threads; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import org.team340.lib.logging.LoggedRobot; +import org.team340.lib.logging.Profiler; import org.team340.lib.util.DisableWatchdog; -import org.team340.lib.util.Tunable; import org.team340.robot.commands.Autos; import org.team340.robot.commands.Routines; import org.team340.robot.subsystems.Swerve; @Logged -public final class Robot extends TimedRobot { +public final class Robot extends LoggedRobot { public final CommandScheduler scheduler = CommandScheduler.getInstance(); @@ -32,16 +28,6 @@ public final class Robot extends TimedRobot { private final CommandXboxController coDriver; public Robot() { - DriverStation.silenceJoystickConnectionWarning(true); - DisableWatchdog.in(scheduler, "m_watchdog"); - DisableWatchdog.in(this, "m_watchdog"); - - // Configure logging - DataLogManager.start(); - DriverStation.startDataLog(DataLogManager.getLog()); - SignalLogger.enableAutoLogging(false); - Epilogue.getConfig().root = "/Telemetry"; - // Initialize subsystems swerve = new Swerve(); @@ -57,23 +43,22 @@ public Robot() { swerve.setDefaultCommand(swerve.drive(this::driverX, this::driverY, this::driverAngular)); // Driver bindings + driver.a().onTrue(none()); driver.povLeft().onTrue(swerve.tareRotation()); // Co-driver bindings coDriver.a().onTrue(none()); - // Set thread priority - waitSeconds(10.0) - .until(DriverStation::isEnabled) - .andThen(() -> Threads.setCurrentThreadPriority(true, 10)) - .schedule(); + // Disable loop overrun warnings from the command + // scheduler, since we already log loop timings + DisableWatchdog.in(scheduler, "m_watchdog"); } /** * Returns the current match time in seconds. */ public double matchTime() { - return Math.max(0.0, DriverStation.getMatchTime()); + return Math.max(DriverStation.getMatchTime(), 0.0); } @NotLogged @@ -93,23 +78,6 @@ public double driverAngular() { @Override public void robotPeriodic() { - scheduler.run(); - Epilogue.update(this); - Tunable.update(); + Profiler.run("scheduler", scheduler::run); } - - @Override - public void simulationPeriodic() {} - - @Override - public void disabledPeriodic() {} - - @Override - public void autonomousPeriodic() {} - - @Override - public void teleopPeriodic() {} - - @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 33f485e..483fd28 100644 --- a/src/main/java/org/team340/robot/commands/Autos.java +++ b/src/main/java/org/team340/robot/commands/Autos.java @@ -7,7 +7,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; -import org.team340.lib.util.AutoChooser; +import org.team340.lib.util.FieldFlip; +import org.team340.lib.util.Tunable; +import org.team340.lib.util.Tunable.TunableDouble; +import org.team340.lib.util.command.AutoChooser; import org.team340.robot.Robot; import org.team340.robot.subsystems.Swerve; @@ -19,6 +22,9 @@ @Logged(strategy = Strategy.OPT_IN) public final class Autos { + private static final TunableDouble deceleration = Tunable.value("autos/deceleration", 6.0); + private static final TunableDouble tolerance = Tunable.value("autos/tolerance", 0.05); + private final Swerve swerve; private final Routines routines; @@ -29,18 +35,21 @@ public Autos(Robot robot) { routines = robot.routines; // Create the auto chooser - chooser = new AutoChooser("Autos"); - chooser.bind(robot.scheduler); + chooser = new AutoChooser(); // Add autonomous modes to the dashboard chooser.add("Example", example()); } private Command example() { + Pose2d start = new Pose2d(1.0, 1.0, Rotation2d.kZero); + Pose2d firstGoal = new Pose2d(2.0, 5.0, Rotation2d.k180deg); + Pose2d secondGoal = new Pose2d(6.0, 2.5, Rotation2d.kCW_90deg); + return sequence( - swerve.resetPose(() -> new Pose2d(1.0, 1.0, Rotation2d.kZero)), - swerve.apfDrive(() -> new Pose2d(2.0, 5.0, Rotation2d.kPi), () -> 6.0, () -> 0.02), - swerve.apfDrive(() -> new Pose2d(6.0, 2.5, Rotation2d.kCW_Pi_2), () -> 6.0, () -> 0.02), + swerve.resetPose(FieldFlip.allianceDiagonal(start)), + swerve.apfDrive(FieldFlip.allianceDiagonal(firstGoal), deceleration::get, tolerance::get), + swerve.apfDrive(FieldFlip.allianceDiagonal(secondGoal), deceleration::get, tolerance::get), swerve.stop(false) ); } diff --git a/src/main/java/org/team340/robot/commands/Routines.java b/src/main/java/org/team340/robot/commands/Routines.java index 7eea19a..f97f641 100644 --- a/src/main/java/org/team340/robot/commands/Routines.java +++ b/src/main/java/org/team340/robot/commands/Routines.java @@ -9,8 +9,8 @@ import org.team340.robot.subsystems.Swerve; /** - * The Routines class contains command compositions, such as sequences - * or parallel command groups, that require multiple subsystems. + * The Routines class contains command compositions that require + * multiple subsystems, such as sequences or parallel command groups. */ @Logged(strategy = Strategy.OPT_IN) public final class Routines { diff --git a/src/main/java/org/team340/robot/subsystems/Swerve.java b/src/main/java/org/team340/robot/subsystems/Swerve.java index b3edc3d..4905224 100644 --- a/src/main/java/org/team340/robot/subsystems/Swerve.java +++ b/src/main/java/org/team340/robot/subsystems/Swerve.java @@ -2,7 +2,6 @@ import com.ctre.phoenix6.CANBus; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; @@ -11,9 +10,6 @@ import edu.wpi.first.wpilibj2.command.Command; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import org.team340.lib.math.Math2; -import org.team340.lib.math.PAPFController; -import org.team340.lib.math.PAPFController.Obstacle; import org.team340.lib.swerve.Perspective; import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.SwerveState; @@ -22,6 +18,9 @@ import org.team340.lib.swerve.hardware.SwerveEncoders; import org.team340.lib.swerve.hardware.SwerveIMUs; import org.team340.lib.swerve.hardware.SwerveMotors; +import org.team340.lib.util.Math2; +import org.team340.lib.util.PAPFController; +import org.team340.lib.util.PAPFController.Obstacle; import org.team340.lib.util.Tunable; import org.team340.lib.util.command.GRRSubsystem; import org.team340.robot.Constants; @@ -88,7 +87,7 @@ public Swerve() { api = new SwerveAPI(config); state = api.state; - apf = new PAPFController(4.0, 0.5, true, new Obstacle[0]); + apf = new PAPFController(4.0, 0.5, 0.01, true, new Obstacle[0]); angularPID = new ProfiledPIDController(10.0, 0.0, 0.0, new Constraints(10.0, 25.0)); angularPID.enableContinuousInput(-Math.PI, Math.PI); @@ -102,14 +101,6 @@ public void periodic() { api.refresh(); } - /** - * Returns the current blue origin relative pose of the robot. - */ - @NotLogged - public Pose2d getPose() { - return state.pose; - } - /** * Tares the rotation of the robot. Useful for * fixing an out of sync or drifting IMU. From 3c624e0637a92a5c56a57211384315cc3451319d Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Fri, 6 Jun 2025 23:28:32 -0400 Subject: [PATCH 7/7] Update README.md --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 32fc374..8e1c2a3 100644 --- a/README.md +++ b/README.md @@ -39,19 +39,19 @@ Node.js is required to support linting via [Spotless](https://github.com/diffplu ```diff diff --git a/build.gradle b/build.gradle -index 155f017..7670ad8 100644 +index 834d562..a2837c8 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,6 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2025.3.2" -- id "com.diffplug.spotless" version "7.0.2" +- id "com.diffplug.spotless" version "7.0.3" } java { -@@ -83,30 +82,6 @@ dependencies { - implementation 'com.google.code.gson:gson:2.11.0' +@@ -91,30 +90,6 @@ dependencies { + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } -// Code formatting via spotless @@ -81,7 +81,7 @@ index 155f017..7670ad8 100644 test { useJUnitPlatform() systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' -@@ -134,5 +109,4 @@ wpi.java.configureTestTasks(test) +@@ -142,5 +117,4 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline'