From 5b92f427843c58e3b305b7b91634cf8d87d8dd0d Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 6 Dec 2023 20:41:46 -0500 Subject: [PATCH] Review fixes --- src/main/java/frc/robot/RobotContainer.java | 12 ++++-------- .../commands/drive/FeedForwardCharacterization.java | 10 +++++----- src/main/java/frc/robot/constants/Constants.java | 2 +- .../java/frc/robot/subsystems/drive/DriveBase.java | 2 +- .../frc/robot/subsystems/drive/GyroIOPigeon2.java | 4 +--- .../frc/robot/subsystems/drive/ModuleIOSparkMax.java | 2 +- .../subsystems/drive/SparkMaxOdometryThread.java | 4 ++-- .../java/frc/robot/util/PolynomialRegression.java | 6 +++--- 8 files changed, 18 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c9a079e..59fea17 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,7 +73,7 @@ public RobotContainer() { } } - // Confiure PathPlanner + // Configure PathPlanner AutoBuilder.configureHolonomic( () -> PoseEstimator.getInstance().getPose(), (pose) -> PoseEstimator.getInstance().resetPose(pose), @@ -86,14 +86,10 @@ public RobotContainer() { m_drive); Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); + (activePath) -> + Logger.recordOutput("Odometry/Trajectory", activePath.toArray(new Pose2d[0]))); PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); - }); + (targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); diff --git a/src/main/java/frc/robot/commands/drive/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/drive/FeedForwardCharacterization.java index 8f5e314..5be69a0 100644 --- a/src/main/java/frc/robot/commands/drive/FeedForwardCharacterization.java +++ b/src/main/java/frc/robot/commands/drive/FeedForwardCharacterization.java @@ -80,7 +80,7 @@ public void add(double velocity, double voltage) { } public void print() { - if (velocityData.size() == 0 || voltageData.size() == 0) { + if (velocityData.isEmpty() || voltageData.isEmpty()) { return; } @@ -91,10 +91,10 @@ public void print() { 1); System.out.println("FF Characterization Results:"); - System.out.println("\tCount=" + Integer.toString(velocityData.size()) + ""); - System.out.println(String.format("\tR2=%.5f", regression.R2())); - System.out.println(String.format("\tkS=%.5f", regression.beta(0))); - System.out.println(String.format("\tkV=%.5f", regression.beta(1))); + System.out.println("\tCount=" + velocityData.size()); + System.out.printf("\tR2=%.5f%n", regression.R2()); + System.out.printf("\tkS=%.5f%n", regression.beta(0)); + System.out.printf("\tkV=%.5f%n", regression.beta(1)); } } } diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index ce735ec..ae4ddb0 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -9,7 +9,7 @@ public final class Constants { private static RobotType kRobotType = RobotType.ROBOT_SIMBOT; - public static double kLoopPeriodSecs = 0.02; + public static final double kLoopPeriodSecs = 0.02; public enum RobotMode { REAL, diff --git a/src/main/java/frc/robot/subsystems/drive/DriveBase.java b/src/main/java/frc/robot/subsystems/drive/DriveBase.java index 8c60a83..5404497 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveBase.java @@ -14,7 +14,7 @@ import org.littletonrobotics.junction.Logger; public class DriveBase extends SubsystemBase { - public static double ODOMETRY_FREQUENCY = 250.0; + public static final double ODOMETRY_FREQUENCY = 250.0; private final GyroIO m_gyroIO; private final GyroIOInputsAutoLogged m_gyroInputs = new GyroIOInputsAutoLogged(); diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 82e8205..411a6fc 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -75,9 +75,7 @@ public void updateInputs(GyroIOInputs inputs) { inputs.accelZ = m_accelZ.getValueAsDouble(); inputs.odometryYawPositions = - yawPositionQueue.stream() - .map((Double value) -> Rotation2d.fromDegrees(value)) - .toArray(Rotation2d[]::new); + yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new); this.yawPositionQueue.clear(); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index ec755fe..5805974 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -83,7 +83,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.driveAppliedVolts = m_driveMotor.getAppliedOutput() * m_driveMotor.getBusVoltage(); inputs.driveCurrentAmps = new double[] {m_driveMotor.getOutputCurrent()}; - // Refresh the Encoder data becasue it is cached. This is non-blocking. + // Refresh the Encoder data because it is cached. This is non-blocking. m_turnAbsoluteEncoder.refresh(); inputs.turnAbsolutePosition = Rotation2d.fromRotations(m_turnAbsoluteEncoder.getValueAsDouble()); diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index 38fbb2e..be60bfb 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -15,8 +15,8 @@ * blocking thread. A Notifier thread is used to gather samples with consistent timing. */ public class SparkMaxOdometryThread { - private List signals = new ArrayList<>(); - private List> queues = new ArrayList<>(); + private final List signals = new ArrayList<>(); + private final List> queues = new ArrayList<>(); private final Notifier notifier; private static SparkMaxOdometryThread instance = null; diff --git a/src/main/java/frc/robot/util/PolynomialRegression.java b/src/main/java/frc/robot/util/PolynomialRegression.java index 28c5796..d006ff6 100644 --- a/src/main/java/frc/robot/util/PolynomialRegression.java +++ b/src/main/java/frc/robot/util/PolynomialRegression.java @@ -7,7 +7,7 @@ // http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html /** - * The {@code PolynomialRegression} class performs a polynomial regression on an set of N + * The {@code PolynomialRegression} class performs a polynomial regression on a set of N * data points (yi, xi). That is, it fits a polynomial * y = β0 + β1 x + β2 * x2 + ... + βd xd (where @@ -154,12 +154,12 @@ public double predict(double x) { * polynomial and the coefficient of determination R2 */ public String toString() { - StringBuilder s = new StringBuilder(); int j = degree; // ignoring leading zero coefficients while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; + StringBuilder s = new StringBuilder(); // create remaining terms while (j >= 0) { if (j == 0) s.append(String.format("%.10f ", beta(j))); @@ -167,7 +167,7 @@ public String toString() { else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); j--; } - s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); + s.append(" (R^2 = ").append(String.format("%.3f", R2())).append(")"); // replace "+ -2n" with "- 2n" return s.toString().replace("+ -", "- ");