Reformatted code, updated javadocs.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
BroncBotz3481 · Oct 24, 2024 · 86595fd · 86595fd
1 parent 330e69f
commit 86595fd
Show file tree

Hide file tree

Showing 17 changed files with 241 additions and 229 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -5,7 +5,6 @@
 package frc.robot;
 
 import com.pathplanner.lib.util.PIDConstants;
-
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import swervelib.math.Matter;
@@ -25,7 +24,7 @@ public final class Constants
   public static final Matter CHASSIS    = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
   public static final double LOOP_TIME  = 0.13; //s, 20ms + 110ms sprk max velocity lag
   public static final double MAX_SPEED  = Units.feetToMeters(14.5);
-      // Maximum speed of the robot in meters per second, used to limit acceleration.
+  // Maximum speed of the robot in meters per second, used to limit acceleration.
 
   public static final class AutonConstants
   {

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -4,14 +4,10 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import java.io.File;
-import java.io.IOException;
-import swervelib.parser.SwerveParser;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to each mode, as
@@ -125,7 +121,8 @@ public void teleopInit()
     if (m_autonomousCommand != null)
     {
       m_autonomousCommand.cancel();
-    } else {
+    } else
+    {
       CommandScheduler.getInstance().cancelAll();
     }
     m_robotContainer.setDriveMode();

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -8,11 +8,9 @@
 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 edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -31,10 +29,10 @@ public class RobotContainer
 {
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
-  final CommandXboxController driverXbox = new CommandXboxController(0);
+  final         CommandXboxController driverXbox = new CommandXboxController(0);
   // The robot's subsystems and commands are defined here...
-  private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
-                                                                         "swerve/neo"));
+  private final SwerveSubsystem       drivebase  = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
+                                                                                "swerve/neo"));
   // Applies deadbands and inverts controls because joysticks
   // are back-right positive while robot
   // controls are front-left positive
@@ -43,16 +41,16 @@ public class RobotContainer
   // buttons are quick rotation positions to different ways to face
   // WARNING: default buttons are on the same buttons as the ones defined in configureBindings
   AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase,
-                                                                  () -> -MathUtil.applyDeadband(driverXbox.getLeftY(),
-                                                                                                OperatorConstants.LEFT_Y_DEADBAND),
-                                                                  () -> -MathUtil.applyDeadband(driverXbox.getLeftX(),
-                                                                                                OperatorConstants.LEFT_X_DEADBAND),
-                                                                  () -> -MathUtil.applyDeadband(driverXbox.getRightX(),
-                                                                                                OperatorConstants.RIGHT_X_DEADBAND),
-                                                                  driverXbox.getHID()::getYButtonPressed,
-                                                                  driverXbox.getHID()::getAButtonPressed,
-                                                                  driverXbox.getHID()::getXButtonPressed,
-                                                                  driverXbox.getHID()::getBButtonPressed);
+                                                                 () -> -MathUtil.applyDeadband(driverXbox.getLeftY(),
+                                                                                               OperatorConstants.LEFT_Y_DEADBAND),
+                                                                 () -> -MathUtil.applyDeadband(driverXbox.getLeftX(),
+                                                                                               OperatorConstants.LEFT_X_DEADBAND),
+                                                                 () -> -MathUtil.applyDeadband(driverXbox.getRightX(),
+                                                                                               OperatorConstants.RIGHT_X_DEADBAND),
+                                                                 driverXbox.getHID()::getYButtonPressed,
+                                                                 driverXbox.getHID()::getAButtonPressed,
+                                                                 driverXbox.getHID()::getXButtonPressed,
+                                                                 driverXbox.getHID()::getBButtonPressed);
 
   // Applies deadbands and inverts controls because joysticks
   // are back-right positive while robot
@@ -98,7 +96,8 @@ public RobotContainer()
    */
   private void configureBindings()
   {
-    if (DriverStation.isTest()) {
+    if (DriverStation.isTest())
+    {
       driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand());
       driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
       driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2));
@@ -107,21 +106,22 @@ private void configureBindings()
       driverXbox.leftBumper().onTrue(Commands.none());
       driverXbox.rightBumper().onTrue(Commands.none());
       drivebase.setDefaultCommand(
-        !RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity : driveFieldOrientedDirectAngleSim);
-    } else {
+          !RobotBase.isSimulation() ? driveFieldOrientedAnglularVelocity : driveFieldOrientedDirectAngleSim);
+    } else
+    {
       driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
       driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
       driverXbox.b().whileTrue(
           Commands.deferredProxy(() -> drivebase.driveToPose(
-                                    new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
+                                     new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
                                 ));
       driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2));
       driverXbox.start().whileTrue(Commands.none());
       driverXbox.back().whileTrue(Commands.none());
       driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
       driverXbox.rightBumper().onTrue(Commands.none());
       drivebase.setDefaultCommand(
-        !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim);
+          !RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim);
     }
   }
 

diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java
@@ -27,7 +27,7 @@ public class AbsoluteDriveAdv extends Command
   private final DoubleSupplier  vX, vY;
   private final DoubleSupplier  headingAdjust;
   private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight;
-  private       boolean         resetHeading = false;
+  private boolean resetHeading = false;
 
   /**
    * Used to drive a swerve robot in full field-centric mode.  vX and vY supply translation inputs, where x is

diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -4,20 +4,12 @@
 
 package frc.robot.subsystems.swervedrive;
 
-import java.io.File;
-import java.util.Arrays;
-import java.util.function.DoubleSupplier;
-
-import org.photonvision.PhotonCamera;
-import org.photonvision.targeting.PhotonPipelineResult;
-
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.ReplanningConfig;
-
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -32,18 +24,21 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import frc.robot.Constants;
 import frc.robot.Constants.AutonConstants;
+import java.io.File;
+import java.util.Arrays;
+import java.util.function.DoubleSupplier;
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonPipelineResult;
 import swervelib.SwerveController;
 import swervelib.SwerveDrive;
 import swervelib.SwerveDriveTest;
 import swervelib.math.SwerveMath;
-import swervelib.parser.PIDFConfig;
 import swervelib.parser.SwerveControllerConfiguration;
 import swervelib.parser.SwerveDriveConfiguration;
 import swervelib.parser.SwerveParser;
@@ -53,10 +48,6 @@
 public class SwerveSubsystem extends SubsystemBase
 {
 
-  /**
-   * PhotonVision class to keep an accurate odometry.
-   */
-  private       Vision  vision;
   /**
    * Swerve drive object.
    */
@@ -69,6 +60,10 @@ public class SwerveSubsystem extends SubsystemBase
    * Enable vision odometry updates while driving.
    */
   private final boolean visionDriveTest = false;
+  /**
+   * PhotonVision class to keep an accurate odometry.
+   */
+  private       Vision  vision;
 
   /**
    * Initialize {@link SwerveDrive} with the directory provided.
@@ -104,8 +99,11 @@ public SwerveSubsystem(File directory)
     }
     swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle.
     swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life.
-    swerveDrive.setAngularVelocityCompensation(true, true, 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1.
-    swerveDrive.setModuleEncoderAutoSynchronize(false, 1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving.
+    swerveDrive.setAngularVelocityCompensation(true,
+                                               true,
+                                               0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1.
+    swerveDrive.setModuleEncoderAutoSynchronize(false,
+                                                1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving.
     swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible
     if (visionDriveTest)
     {
@@ -367,42 +365,49 @@ public Command sysIdAngleMotorCommand()
    *
    * @return a Command that centers the modules of the SwerveDrive subsystem
    */
-  public Command centerModulesCommand() {
+  public Command centerModulesCommand()
+  {
     return run(() -> Arrays.asList(swerveDrive.getModules())
-      .forEach(it -> it.setAngle(0.0)));
+                           .forEach(it -> it.setAngle(0.0)));
   }
 
   /**
    * Returns a Command that drives the swerve drive to a specific distance at a given speed.
    *
-   * @param distanceInMeters the distance to drive in meters
+   * @param distanceInMeters       the distance to drive in meters
    * @param speedInMetersPerSecond the speed at which to drive in meters per second
    * @return a Command that drives the swerve drive to a specific distance at a given speed
    */
-  public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) {
+  public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond)
+  {
     return Commands.deferredProxy(
         () -> Commands.run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)), this)
-        .until(() -> swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) > distanceInMeters)
-        );
+                      .until(() -> swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) >
+                                   distanceInMeters)
+                                 );
   }
 
   /**
    * Sets the maximum speed of the swerve drive.
    *
    * @param maximumSpeedInMetersPerSecond the maximum speed to set for the swerve drive in meters per second
    */
-  public void setMaximumSpeed(double maximumSpeedInMetersPerSecond) {
-    swerveDrive.setMaximumSpeed(maximumSpeedInMetersPerSecond, false, swerveDrive.swerveDriveConfiguration.physicalCharacteristics.optimalVoltage);
+  public void setMaximumSpeed(double maximumSpeedInMetersPerSecond)
+  {
+    swerveDrive.setMaximumSpeed(maximumSpeedInMetersPerSecond,
+                                false,
+                                swerveDrive.swerveDriveConfiguration.physicalCharacteristics.optimalVoltage);
   }
 
   /**
    * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object.
    *
-   * @param  kS  the static gain of the feedforward
-   * @param  kV  the velocity gain of the feedforward
-   * @param  kA  the acceleration gain of the feedforward
+   * @param kS the static gain of the feedforward
+   * @param kV the velocity gain of the feedforward
+   * @param kA the acceleration gain of the feedforward
    */
-  public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) {
+  public void replaceSwerveModuleFeedforward(double kS, double kV, double kA)
+  {
     swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA));
   }
 

diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java
@@ -17,9 +17,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import frc.robot.Robot;
 import java.awt.Desktop;
-import java.io.IOException;
-import java.net.URI;
-import java.net.URISyntaxException;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
@@ -52,6 +49,10 @@ public class Vision
    */
   public static final AprilTagFieldLayout fieldLayout                     = AprilTagFieldLayout.loadField(
       AprilTagFields.k2024Crescendo);
+  /**
+   * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}.
+   */
+  private final       double              maximumAmbiguity = 0.25;
   /**
    * Photon Vision Simulation
    */
@@ -64,10 +65,6 @@ public class Vision
    * Current pose from the pose estimator using wheel odometry.
    */
   private             Supplier<Pose2d>    currentPose;
-  /**
-   * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}.
-   */
-  private final double maximumAmbiguity = 0.25;
   /**
    * Field from {@link swervelib.SwerveDrive#field}
    */