Merge remote-tracking branch 'origin/ramsete-controller' into ramsete…

…-controller

src/main/java/frc/robot/AStarTrajectoryGenerator.java

@@ -27,7 +27,8 @@ public class AStarTrajectoryGenerator {
    * @param endingRotation The rotation the bot should start in
    * @return The generated Trajectory
    */
-  public static Trajectory generateTrajectory(Path path, Rotation2d startingRotation, Rotation2d endingRotation) {
+  public static Trajectory generateTrajectory(
+      Path path, Rotation2d startingRotation, Rotation2d endingRotation) {
     ArrayList<Double[]> pathList = path.getPathList();
 
     /* stealing max accel and velocity from holonomicController */

src/main/java/frc/robot/Constants.java

@@ -90,6 +90,7 @@ public static class Constraints {
       }
     }
   }
+
   /** IDs for the controller buttons. */
   public static class ContollerButtons {
     public static final int A_NUMBER = 1;
@@ -288,6 +289,7 @@ public static class Cube {
   public static class AStar {
     /** Half the length of the field. Positive for Max, Negative for Min. Measured in cm. */
     public static final int FIELD_X = 1654 / 2;
+
     /** Half the width of the field. Positive for Max, Negative for Min. Measured in cm. */
     public static final int FIELD_Y = 802 / 2;
   }

src/main/java/frc/robot/commands/TrackTrajectory.java

@@ -47,8 +47,7 @@ public void initialize() {
   /** Called each scheduler run while the command is scheduled. Tracks the trajectory. */
   @Override
   public void execute() {
-    drive.trackTrajectory(
-        trajectory.sample(Timer.getFPGATimestamp() - FPGAOffset));
+    drive.trackTrajectory(trajectory.sample(Timer.getFPGATimestamp() - FPGAOffset));
   }
 
   /** Called once the command ends or is interrupted. Not used in this class. */

src/main/java/frc/robot/commands/autotasks/AutoTask.java

@@ -21,6 +21,7 @@ public abstract class AutoTask {
   private Stack<CommandBase> initCommands = new Stack<CommandBase>();
   private Stack<CommandBase> arrivedCommands = new Stack<CommandBase>();
   private CommandBase runningCommand;
+
   /**
    * Creates a new AutoTask. Dont create instances of commands. Each command should be a parameter
    * instead of created

src/main/java/frc/robot/commands/autotasks/ExampleAutoTask.java

@@ -6,6 +6,7 @@
 public class ExampleAutoTask extends AutoTask {
 
   private ExampleCommand exampleCommand;
+
   /**
    * Use the constructor to configure the autotask arrival and init commands The order you run
    * {@link #addArrivedCommand(edu.wpi.first.wpilibj2.command.CommandBase) addArrivedCommand} and
@@ -26,6 +27,7 @@ public ExampleAutoTask(ExampleCommand exampleCommand) {
     setTaskPosition(
         new Pose()); // Normaly the pose would not be empty but this is a example so its okay.
   }
+
   /**
    * The methods below this are mainly for updates, like if you wanted to put somthing on smartdash
    * you would put that in initTask.

src/main/java/frc/robot/subsystems/Drive.java

@@ -28,7 +28,6 @@ public class Drive extends SubsystemBase {
 
   /* Motor controllers */
   private WPI_TalonSRX frontLeft, frontRight, rearLeft, rearRight;
-
   private MotorControllerGroup left, right;
 
   private DifferentialDriveKinematics kinematics;
@@ -72,8 +71,7 @@ public Drive(Limelight limelight) {
     /* Instantiates the MecanumDrive drivetrain controller. */
     drivetrain = new DifferentialDrive(left, right);
 
-    kinematics =
-        new DifferentialDriveKinematics(Constants.Drive.TRACK_WIDTH);
+    kinematics = new DifferentialDriveKinematics(Constants.Drive.TRACK_WIDTH);
 
     /* Instantiates the gyroscope. */
     gyroscope = new AHRS(SPI.Port.kMXP);
@@ -94,7 +92,6 @@ public Drive(Limelight limelight) {
     ramseteController = new RamseteController();
   }
 
-
   /*
    * *looks at commented-out code*
    * *cries in programmer*
@@ -132,9 +129,9 @@ public Drive(Limelight limelight) {
 
   /**
    * Moves the robot in arcade drive mode.
-   * 
+   *
    * <p>All params are -1.0 to 1.0.
-   * 
+   *
    * @param x Robot speed along X axis, which is forward-backward. Forward is positive.
    * @param z Robot rotation speed around Z axis. Counterclockwise is positive.
    */
@@ -144,9 +141,9 @@ public void moveArcade(double x, double z) {
 
   /**
    * Moves the robot in tank drive mode.
-   * 
+   *
    * <p>All params are -1.0 to 1.0.
-   * 
+   *
    * @param left Speed of left side of robot drivetrain. Forward is positive.
    * @param right Speed of right side of robot drivetrain. Forward is positive.
    */
@@ -175,36 +172,45 @@ public void stop() {
 
   /**
    * Get the average position of the two encoders on the left side of the drivetrain.
-   * 
-   * This should be pretty close to the actual distance travelled, since the motors & encoders should theoretically travel the same or very similar distances
+   *
+   * <p>This should be pretty close to the actual distance travelled, since the motors & encoders
+   * should theoretically travel the same or very similar distances
+   *
    * @return Average position of the two encoders on the left side of the drivetrain
    */
   private double getAverageLeftPosition() {
-    double averageLeftPosition = (frontLeft.getSelectedSensorPosition() + rearLeft.getSelectedSensorPosition()) / 2;
-    double averageLeftPositionInches = (averageLeftPosition * Constants.Drive.DRIVE_ENCODER_PPR) / (Constants.Drive.WHEEL_SIZE_INCHES * Math.PI);
+    double averageLeftPosition =
+        (frontLeft.getSelectedSensorPosition() + rearLeft.getSelectedSensorPosition()) / 2;
+    double averageLeftPositionInches =
+        (averageLeftPosition * Constants.Drive.DRIVE_ENCODER_PPR)
+            / (Constants.Drive.WHEEL_SIZE_INCHES * Math.PI);
     double averageLeftPositionMeters = Units.inchesToMeters(averageLeftPositionInches);
     return averageLeftPositionMeters;
   }
 
   /**
    * Get the average position of the two encoders on the right side of the drivetrain.
-   * 
-   * This should be pretty close to the actual distance travelled, since the motors & encoders should theoretically travel the same or very similar distances
+   *
+   * <p>This should be pretty close to the actual distance travelled, since the motors & encoders
+   * should theoretically travel the same or very similar distances
+   *
    * @return Average position of the two encoders on the right side of the drivetrain
    */
   private double getAverageRightPosition() {
-    double averageRightPosition = (frontRight.getSelectedSensorPosition() + rearRight.getSelectedSensorPosition()) / 2;
-    double averageRightPositionInches = (averageRightPosition * Constants.Drive.DRIVE_ENCODER_PPR) / (Constants.Drive.WHEEL_SIZE_INCHES * Math.PI);
+    double averageRightPosition =
+        (frontRight.getSelectedSensorPosition() + rearRight.getSelectedSensorPosition()) / 2;
+    double averageRightPositionInches =
+        (averageRightPosition * Constants.Drive.DRIVE_ENCODER_PPR)
+            / (Constants.Drive.WHEEL_SIZE_INCHES * Math.PI);
     double averageRightPositionMeters = Units.inchesToMeters(averageRightPositionInches);
     return averageRightPositionMeters;
   }
 
-  /**
-   * Resets the gyroscope.
-   */
+  /** Resets the gyroscope. */
   public void resetGyro() {
     gyroscope.reset();
   }
+
   /**
    * Gets the roll of the robot in degrees
    *
@@ -213,6 +219,7 @@ public void resetGyro() {
   public double getRoll() {
     return gyroscope.getRoll();
   }
+
   /**
    * Gets the pitch of the robot in degrees
    *
@@ -239,7 +246,9 @@ public double getPitch() {
   public void resetPosition(Pose2d currentPose) {
     whereTheHeckAreWe.resetPosition(
         gyroscope.getRotation2d(),
-        this.getAverageLeftPosition(), // we miiiiight need to create an offset and zero these, which would be horrible, but doable
+        this
+            .getAverageLeftPosition(), // we miiiiight need to create an offset and zero these,
+                                       // which would be horrible, but doable
         this.getAverageRightPosition(),
         currentPose);
   }