Docs and refactoring (#361)

* java docs & refactoring

* cpp docs and refactoring

pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathFollowingCommand.java → pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java

@@ -14,7 +14,7 @@
 import java.util.function.Consumer;
 import java.util.function.Supplier;
 
-public class PathFollowingCommand extends Command {
+public class FollowPathCommand extends Command {
   private final Timer timer = new Timer();
   private final PathPlannerPath path;
   private final Supplier<Pose2d> poseSupplier;
@@ -25,7 +25,19 @@ public class PathFollowingCommand extends Command {
 
   private PathPlannerTrajectory generatedTrajectory;
 
-  public PathFollowingCommand(
+  /**
+   * Construct a base path following command
+   *
+   * @param path The path to follow
+   * @param poseSupplier Function that supplies the current field-relative pose of the robot
+   * @param speedsSupplier Function that supplies the current robot-relative chassis speeds
+   * @param outputRobotRelative Function that will apply the robot-relative output speeds of this
+   *     command
+   * @param controller Path following controller that will be used to follow the path
+   * @param replanningConfig Path replanning configuration
+   * @param requirements Subsystems required by this command, usually just the drive subsystem
+   */
+  public FollowPathCommand(
       PathPlannerPath path,
       Supplier<Pose2d> poseSupplier,
       Supplier<ChassisSpeeds> speedsSupplier,

pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathHolonomic.java

@@ -11,7 +11,26 @@
 import java.util.function.Consumer;
 import java.util.function.Supplier;
 
-public class FollowPathHolonomic extends PathFollowingCommand {
+public class FollowPathHolonomic extends FollowPathCommand {
+  /**
+   * Construct a path following command that will use a holonomic drive controller for holonomic
+   * drive trains
+   *
+   * @param path The path to follow
+   * @param poseSupplier Function that supplies the current field-relative pose of the robot
+   * @param speedsSupplier Function that supplies the current robot-relative chassis speeds
+   * @param outputRobotRelative Function that will apply the robot-relative output speeds of this
+   *     command
+   * @param translationConstants PID constants for the translation PID controllers
+   * @param rotationConstants PID constants for the rotation controller
+   * @param maxModuleSpeed The max speed of a drive module in meters/sec
+   * @param driveBaseRadius The radius of the drive base in meters. For swerve drive, this is the
+   *     distance from the center of the robot to the furthest module. For mecanum, this is the
+   *     drive base width / 2
+   * @param period Period of the control loop in seconds, default is 0.02s
+   * @param replanningConfig Path replanning configuration
+   * @param requirements Subsystems required by this command, usually just the drive subsystem
+   */
   public FollowPathHolonomic(
       PathPlannerPath path,
       Supplier<Pose2d> poseSupplier,
@@ -35,6 +54,24 @@ public FollowPathHolonomic(
         requirements);
   }
 
+  /**
+   * Construct a path following command that will use a holonomic drive controller for holonomic
+   * drive trains
+   *
+   * @param path The path to follow
+   * @param poseSupplier Function that supplies the current field-relative pose of the robot
+   * @param speedsSupplier Function that supplies the current robot-relative chassis speeds
+   * @param outputRobotRelative Function that will apply the robot-relative output speeds of this
+   *     command
+   * @param translationConstants PID constants for the translation PID controllers
+   * @param rotationConstants PID constants for the rotation controller
+   * @param maxModuleSpeed The max speed of a drive module in meters/sec
+   * @param driveBaseRadius The radius of the drive base in meters. For swerve drive, this is the
+   *     distance from the center of the robot to the furthest module. For mecanum, this is the
+   *     drive base width / 2
+   * @param replanningConfig Path replanning configuration
+   * @param requirements Subsystems required by this command, usually just the drive subsystem
+   */
   public FollowPathHolonomic(
       PathPlannerPath path,
       Supplier<Pose2d> poseSupplier,
@@ -60,6 +97,18 @@ public FollowPathHolonomic(
         requirements);
   }
 
+  /**
+   * Construct a path following command that will use a holonomic drive controller for holonomic
+   * drive trains
+   *
+   * @param path The path to follow
+   * @param poseSupplier Function that supplies the current field-relative pose of the robot
+   * @param speedsSupplier Function that supplies the current robot-relative chassis speeds
+   * @param outputRobotRelative Function that will apply the robot-relative output speeds of this
+   *     command
+   * @param config Holonomic path follower configuration
+   * @param requirements Subsystems required by this command, usually just the drive subsystem
+   */
   public FollowPathHolonomic(
       PathPlannerPath path,
       Supplier<Pose2d> poseSupplier,

pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathLTV.java

@@ -12,7 +12,19 @@
 import java.util.function.Consumer;
 import java.util.function.Supplier;
 
-public class FollowPathLTV extends PathFollowingCommand {
+public class FollowPathLTV extends FollowPathCommand {
+  /**
+   * Create a path following command that will use an LTV unicycle controller for differential drive
+   * trains
+   *
+   * @param path The path to follow
+   * @param poseSupplier Function that supplies the current field-relative pose of the robot
+   * @param speedsSupplier Function that supplies the current robot-relative chassis speeds
+   * @param output Function that will apply the robot-relative output speeds of this command
+   * @param dt The amount of time between each robot control loop, default is 0.02s
+   * @param replanningConfig Path replanning configuration
+   * @param requirements Subsystems required by this command, usually just the drive subsystem
+   */
   public FollowPathLTV(
       PathPlannerPath path,
       Supplier<Pose2d> poseSupplier,
@@ -31,6 +43,20 @@ public FollowPathLTV(
         requirements);
   }
 
+  /**
+   * Create a path following command that will use an LTV unicycle controller for differential drive
+   * trains
+   *
+   * @param path The path to follow
+   * @param poseSupplier Function that supplies the current field-relative pose of the robot
+   * @param speedsSupplier Function that supplies the current robot-relative chassis speeds
+   * @param output Function that will apply the robot-relative output speeds of this command
+   * @param qelems The maximum desired error tolerance for each state.
+   * @param relems The maximum desired control effort for each input.
+   * @param dt The amount of time between each robot control loop, default is 0.02s
+   * @param replanningConfig Path replanning configuration
+   * @param requirements Subsystems required by this command, usually just the drive subsystem
+   */
   public FollowPathLTV(
       PathPlannerPath path,
       Supplier<Pose2d> poseSupplier,

pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathRamsete.java

@@ -9,7 +9,22 @@
 import java.util.function.Consumer;
 import java.util.function.Supplier;
 
-public class FollowPathRamsete extends PathFollowingCommand {
+public class FollowPathRamsete extends FollowPathCommand {
+  /**
+   * Construct a path following command that will use a Ramsete path following controller for
+   * differential drive trains
+   *
+   * @param path The path to follow
+   * @param poseSupplier Function that supplies the current field-relative pose of the robot
+   * @param speedsSupplier Function that supplies the current robot-relative chassis speeds
+   * @param output Function that will apply the robot-relative output speeds of this command
+   * @param b Tuning parameter (b > 0 rad^2/m^2) for which larger values make convergence more
+   *     aggressive like a proportional term.
+   * @param zeta Tuning parameter (0 rad^-1 < zeta < 1 rad^-1) for which larger values provide
+   *     more damping in response.
+   * @param replanningConfig Path replanning configuration
+   * @param requirements Subsystems required by this command, usually just the drive subsystem
+   */
   public FollowPathRamsete(
       PathPlannerPath path,
       Supplier<Pose2d> poseSupplier,
@@ -29,6 +44,17 @@ public FollowPathRamsete(
         requirements);
   }
 
+  /**
+   * Construct a path following command that will use a Ramsete path following controller for
+   * differential drive trains
+   *
+   * @param path The path to follow
+   * @param poseSupplier Function that supplies the current field-relative pose of the robot
+   * @param speedsSupplier Function that supplies the current robot-relative chassis speeds
+   * @param output Function that will apply the robot-relative output speeds of this command
+   * @param replanningConfig Path replanning configuration
+   * @param requirements Subsystems required by this command, usually just the drive subsystem
+   */
   public FollowPathRamsete(
       PathPlannerPath path,
       Supplier<Pose2d> poseSupplier,

pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java

@@ -32,6 +32,19 @@ public class PathfindingCommand extends Command {
   private PathPlannerTrajectory currentTrajectory;
   private Pose2d startingPose;
 
+  /**
+   * Constructs a new base pathfinding command that will generate a path towards the given path.
+   *
+   * @param targetPath the path to pathfind to
+   * @param constraints the path constraints to use while pathfinding
+   * @param poseSupplier a supplier for the robot's current pose
+   * @param speedsSupplier a supplier for the robot's current robot relative speeds
+   * @param outputRobotRelative a consumer for the output speeds (robot relative)
+   * @param controller Path following controller that will be used to follow the path
+   * @param rotationDelayDistance How far the robot should travel before attempting to rotate to the
+   *     final rotation
+   * @param requirements the subsystems required by this command
+   */
   public PathfindingCommand(
       PathPlannerPath targetPath,
       PathConstraints constraints,
@@ -66,6 +79,20 @@ public PathfindingCommand(
     this.rotationDelayDistance = rotationDelayDistance;
   }
 
+  /**
+   * Constructs a new base pathfinding command that will generate a path towards the given pose.
+   *
+   * @param targetPose the pose to pathfind to, the rotation component is only relevant for
+   *     holonomic drive trains
+   * @param constraints the path constraints to use while pathfinding
+   * @param poseSupplier a supplier for the robot's current pose
+   * @param speedsSupplier a supplier for the robot's current robot relative speeds
+   * @param outputRobotRelative a consumer for the output speeds (robot relative)
+   * @param controller Path following controller that will be used to follow the path
+   * @param rotationDelayDistance How far the robot should travel before attempting to rotate to the
+   *     final rotation
+   * @param requirements the subsystems required by this command
+   */
   public PathfindingCommand(
       Pose2d targetPose,
       PathConstraints constraints,

pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java

@@ -89,48 +89,66 @@ public void setEnabled(boolean enabled) {
     this.isEnabled = enabled;
   }
 
+  /**
+   * Resets the controller based on the current state of the robot
+   *
+   * @param currentPose Current robot pose
+   * @param currentSpeeds Current robot relative chassis speeds
+   */
   @Override
   public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) {
     rotationController.reset(
         currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond);
   }
 
+  /**
+   * Calculates the next output of the path following controller
+   *
+   * @param currentPose The current robot pose
+   * @param targetState The desired trajectory state
+   * @return The next robot relative output of the path following controller
+   */
   @Override
   public ChassisSpeeds calculateRobotRelativeSpeeds(
-      Pose2d currentPose, PathPlannerTrajectory.State referenceState) {
-    double xFF = referenceState.velocityMps * referenceState.heading.getCos();
-    double yFF = referenceState.velocityMps * referenceState.heading.getSin();
+      Pose2d currentPose, PathPlannerTrajectory.State targetState) {
+    double xFF = targetState.velocityMps * targetState.heading.getCos();
+    double yFF = targetState.velocityMps * targetState.heading.getSin();
 
-    this.translationError = currentPose.getTranslation().minus(referenceState.positionMeters);
+    this.translationError = currentPose.getTranslation().minus(targetState.positionMeters);
 
     if (!this.isEnabled) {
       return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, 0, currentPose.getRotation());
     }
 
     double xFeedback =
-        this.xController.calculate(currentPose.getX(), referenceState.positionMeters.getX());
+        this.xController.calculate(currentPose.getX(), targetState.positionMeters.getX());
     double yFeedback =
-        this.yController.calculate(currentPose.getY(), referenceState.positionMeters.getY());
+        this.yController.calculate(currentPose.getY(), targetState.positionMeters.getY());
 
-    double angVelConstraint = referenceState.constraints.getMaxAngularVelocityRps();
+    double angVelConstraint = targetState.constraints.getMaxAngularVelocityRps();
     // Approximation of available module speed to do rotation with
-    double maxAngVelModule = Math.max(0, maxModuleSpeed - referenceState.velocityMps) * mpsToRps;
+    double maxAngVelModule = Math.max(0, maxModuleSpeed - targetState.velocityMps) * mpsToRps;
     double maxAngVel = Math.min(angVelConstraint, maxAngVelModule);
 
     var rotationConstraints =
         new TrapezoidProfile.Constraints(
-            maxAngVel, referenceState.constraints.getMaxAngularAccelerationRpsSq());
+            maxAngVel, targetState.constraints.getMaxAngularAccelerationRpsSq());
 
     double targetRotationVel =
         rotationController.calculate(
             currentPose.getRotation().getRadians(),
-            new TrapezoidProfile.State(referenceState.targetHolonomicRotation.getRadians(), 0),
+            new TrapezoidProfile.State(targetState.targetHolonomicRotation.getRadians(), 0),
             rotationConstraints);
 
     return ChassisSpeeds.fromFieldRelativeSpeeds(
         xFF + xFeedback, yFF + yFeedback, targetRotationVel, currentPose.getRotation());
   }
 
+  /**
+   * Get the current positional error between the robot's actual and target positions
+   *
+   * @return Positional error, in meters
+   */
   @Override
   public double getPositionalError() {
     return translationError.getNorm();

pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPLTVController.java

@@ -69,6 +69,13 @@ public PPLTVController(Vector<N3> qelems, Vector<N2> relems, double dt, double m
     super(qelems, relems, dt, maxVelocity);
   }
 
+  /**
+   * Calculates the next output of the path following controller
+   *
+   * @param currentPose The current robot pose
+   * @param targetState The desired trajectory state
+   * @return The next robot relative output of the path following controller
+   */
   @Override
   public ChassisSpeeds calculateRobotRelativeSpeeds(
       Pose2d currentPose, PathPlannerTrajectory.State targetState) {
@@ -81,11 +88,22 @@ public ChassisSpeeds calculateRobotRelativeSpeeds(
         targetState.headingAngularVelocityRps);
   }
 
+  /**
+   * Resets the controller based on the current state of the robot
+   *
+   * @param currentPose Current robot pose
+   * @param currentSpeeds Current robot relative chassis speeds
+   */
   @Override
   public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) {
     lastError = 0;
   }
 
+  /**
+   * Get the current positional error between the robot's actual and target positions
+   *
+   * @return Positional error, in meters
+   */
   @Override
   public double getPositionalError() {
     return lastError;

pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPRamseteController.java

@@ -28,6 +28,13 @@ public PPRamseteController() {
     super();
   }
 
+  /**
+   * Calculates the next output of the path following controller
+   *
+   * @param currentPose The current robot pose
+   * @param targetState The desired trajectory state
+   * @return The next robot relative output of the path following controller
+   */
   @Override
   public ChassisSpeeds calculateRobotRelativeSpeeds(
       Pose2d currentPose, PathPlannerTrajectory.State targetState) {
@@ -40,11 +47,22 @@ public ChassisSpeeds calculateRobotRelativeSpeeds(
         targetState.headingAngularVelocityRps);
   }
 
+  /**
+   * Resets the controller based on the current state of the robot
+   *
+   * @param currentPose Current robot pose
+   * @param currentSpeeds Current robot relative chassis speeds
+   */
   @Override
   public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) {
     lastError = 0;
   }
 
+  /**
+   * Get the current positional error between the robot's actual and target positions
+   *
+   * @return Positional error, in meters
+   */
   @Override
   public double getPositionalError() {
     return lastError;

pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PathFollowingController.java

@@ -23,5 +23,10 @@ ChassisSpeeds calculateRobotRelativeSpeeds(
    */
   void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds);
 
+  /**
+   * Get the current positional error between the robot's actual and target positions
+   *
+   * @return Positional error, in meters
+   */
   double getPositionalError();
 }