PPSwerveControllerCommand loop overruns

Technical Java
,
1

Hi,

When trying to follow paths I have made in pathplanner, it is causing pretty substantial loop overruns:

4:07:21.616 AM
autonomousPeriodic(): 0.000107s Shuffleboard.update(): 0.000015s LiveWindow.updateValues(): 0.000012s robotPeriodic(): 0.087307s Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): SmartDashboard.updateValues(): 0.000379s Warning 1 SmartDashboard.updateValues(): 0.000379s
robotPeriodic(): 0.087307s
LiveWindow.updateValues(): 0.000012s
Shuffleboard.update(): 0.000015s
autonomousPeriodic(): 0.000107s
edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62) PPSwerveControllerCommand.initialize(): 0.070168s ThreePieceBalanceAuto.execute(): 0.002295s VisionSubsystemImpl.periodic(): 0.000019s DriveSubsystemImpl.periodic(): 0.001202s buttons.run(): 0.000014s ArmSubsystemImpl.periodic(): 0.000011s Warning at edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62): ClawSubsystemImpl.periodic(): 0.000031s Warning 1 ClawSubsystemImpl.periodic(): 0.000031s
ArmSubsystemImpl.periodic(): 0.000011s
buttons.run(): 0.000014s
DriveSubsystemImpl.periodic(): 0.001202s
VisionSubsystemImpl.periodic(): 0.000019s
ThreePieceBalanceAuto.execute(): 0.002295s
PPSwerveControllerCommand.initialize(): 0.070168s
edu.wpi.first.wpilibj.Tracer.lambda$printEpochs$0(Tracer.java:62) CommandScheduler loop overrun

How should I fix this? This is my current code for following a pathplanner trajectory:

package frc.robot.commands.autonomous;

import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.commands.PPSwerveControllerCommand;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.TrajectoryConstants;
import frc.robot.commands.DriveCommandBase;
import frc.robot.subsystems.drive.DriveSubsystem;
import frc.robot.subsystems.vision.VisionSubsystem;

public class FollowPathPlannerTrajectory extends DriveCommandBase {

  private final DriveSubsystem driveSubsystem;
  private final boolean resetOdometryToTrajectoryStart;
  
  private PPSwerveControllerCommand followTrajectoryCommand;
  private Pose2d trajectoryInitialPose;
  
  /* EDIT CODE BELOW HERE */
  // You should have constants for everything in here

  private final SwerveDriveKinematics driveKinematics = DriveConstants.DRIVE_KINEMATICS;
  
  private final double autoMaxVelocity = TrajectoryConstants.MAX_SPEED;
  private final double autoMaxAcceleration = TrajectoryConstants.MAX_ACCELERATION - .5;

  // Your probably only want to edit the P values
  private final PIDController xController = new PIDController(TrajectoryConstants.X_CONTROLLER_P - 2.5, 0, 0);
  private final PIDController yController = new PIDController(TrajectoryConstants.Y_CONTROLLER_P - 2.5, 0, 0);
  private final PIDController thetaController = new PIDController(TrajectoryConstants.THETA_CONTROLLER_P - 1.3, 0, 0);
  
  // IMPORTANT: Make sure your driveSubsystem has the methods resetOdometry, getPose, and setModuleStates
  
  /* EDIT CODE ABOVE HERE (ONLY TOUCH THE REST OF THE CODE IF YOU KNOW WHAT YOU'RE DOING) */

  /**
   * Follows the specified PathPlanner trajectory.
   * @param driveSubsystem The subsystem for the swerve drive.
   * @param visionSubsystem The subsystem for vision measurements
   * @param trajectoryName The name of the PathPlanner path file. It should not include the filepath or 
   * .path extension.
   * @param resetOdometryToTrajectoryStart Set as true if you want the odometry of the robot to be set to the
   * start of the trajectory.
   */
  public FollowPathPlannerTrajectory(DriveSubsystem driveSubsystem, VisionSubsystem visionSubsystem, String trajectoryName, boolean resetOdometryToTrajectoryStart) {
    super(driveSubsystem, visionSubsystem);
    this.driveSubsystem = driveSubsystem;    
    addRequirements(visionSubsystem);
    this.resetOdometryToTrajectoryStart = resetOdometryToTrajectoryStart;

    // Makes a trajectory                                                     
    PathPlannerTrajectory trajectoryToFollow = PathPlanner.loadPath(trajectoryName, autoMaxVelocity, autoMaxAcceleration);

    trajectoryInitialPose = trajectoryToFollow.getInitialHolonomicPose();

    // Makes it so wheels don't have to turn more than 90 degrees
    thetaController.enableContinuousInput(-Math.PI, Math.PI);

    // Create a PPSwerveControllerCommand. This is almost identical to WPILib's SwerveControllerCommand, but it uses the holonomic rotation from the PathPlannerTrajectory to control the robot's rotation.
    followTrajectoryCommand = new PPSwerveControllerCommand(
      trajectoryToFollow,
      driveSubsystem::getPose, // Functional interface to feed supplier
      driveKinematics,
      xController,
      yController,
      thetaController,
      driveSubsystem::setModuleStates,
      false,
      driveSubsystem
    );
  }

  /**
   * Follows the specified PathPlanner trajectory.
   * @param driveSubsystem The subsystem for the swerve drive.
   * @param visionSubsystem The subsystem for vision measurements
   * @param trajectoryName The name of the PathPlanner path file. It should not include the filepath or 
   * .path extension.
   */
  public FollowPathPlannerTrajectory(DriveSubsystem driveSubsystem, VisionSubsystem visionSubsystem, String trajectoryName) {
    this(driveSubsystem, visionSubsystem, trajectoryName, false);
  }

  @Override
  public void initialize() {
    if (resetOdometryToTrajectoryStart) {
      driveSubsystem.resetOdometryAndRotation(trajectoryInitialPose, trajectoryInitialPose.getRotation().getDegrees());
    }

    followTrajectoryCommand.schedule();
  }

  @Override
  public void execute() {
    super.execute();
  }

  @Override
  public void end(boolean interrupted) {}

  @Override
  public boolean isFinished() {
    return followTrajectoryCommand.isFinished();
  }
}
2

Based on this one file, I don’t see anything easy. I will propose some potential ideas although none them are great.

You could change your time step from 0.02 to 0.03. I haven’t done that but I have read about it. It may impact other areas as taking derivates and integrals will have a different time step.

You could rewrite PIDController and drop the I and D terms as you aren’t using them. That would likely be a lot of work but it would save a lot of math operations.

In other areas of your code, look for opportunities to replace floating point division with multiplication as division takes around 8 times more operations than multiplication. Instead of dividing by two, multiply by 0.5. This does not apply for pure integer math. Unless you have a lot of floating point math somewhere, this likely won’t do much.

Integer logic can be more efficient.

2 Likes