pathfindThenFollowPath PathPlanner help needed

Technical Programming
1

I’m in need of a little help. We have implemented a pathfindThenFollowPath which works (almost). We have a on the fly (OTF) path to amp tied to a short “Amp path”. It is bound to a button on shuffleboard. When the button is pressed it will run the OTF part, but we have to press the button again for it to run the predefined path. What am I doing wrong?

Binding in RobotContainer:

 private void configureBindings() {

    SmartDashboard.putData("Drive to Amp",new DriveToAmpPath() );

DriveToAmpPath command:

public class DriveToAmpPath extends Command {
  /** Creates a new DriveToAmpPath. */
  public DriveToAmpPath() {
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    // Load the path we want to pathfind to and follow
  }

  @Override
  public void execute() {
    PathPlannerPath path = PathPlannerPath.fromPathFile("Amp");
    PathConstraints constraints = new PathConstraints(
        3.0, 4.0,
        Units.degreesToRadians(540), Units.degreesToRadians(720));
    AutoBuilder.pathfindThenFollowPath(
        path,
        constraints,
        1.0
    // P.Ry was 3.0 Rotation delay distance in meters. This is how far the robot
    // should travel
    // before attempting to rotate.
    ).schedule();
    ;

  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {

    return false;
  }
}

Video of run

2

I think the problem is here, calling Command.schedule() to refer to another command inside your own command is NOT the right thing to do.

What you probably want to do is make it a factory, like this:

public class DriveToAmpFactory {
    public Command create() {
        PathPlannerPath path = PathPlannerPath.fromPathFile("Amp");
        PathConstraints constraints = new PathConstraints(
                3.0, 4.0,
                Units.degreesToRadians(540), Units.degreesToRadians(720));
        return AutoBuilder.pathfindThenFollowPath(
                path,
                constraints,
                1.0
                // P.Ry was 3.0 Rotation delay distance in meters. This is how far the robot
                // should travel
                // before attempting to rotate.
        );
    }
}

Alternatively, you can wrap it around

public class DriveToAmp extends Command {
    private final Command wrapped;
    public DriveToAmp() {
        PathPlannerPath path = PathPlannerPath.fromPathFile("Amp");
        PathConstraints constraints = new PathConstraints(
                3.0, 4.0,
                Units.degreesToRadians(540), Units.degreesToRadians(720));
        wrapped =  AutoBuilder.pathfindThenFollowPath(
                path,
                constraints,
                1.0
                // P.Ry was 3.0 Rotation delay distance in meters. This is how far the robot
                // should travel
                // before attempting to rotate.
        );
    }

    @Override
    public void initialize() {
        wrapped.initialize();
    }

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

    @Override
    public void end(boolean interrupted) {
        wrapped.end(interrupted);
    }

    @Override
    public boolean isFinished() {
        return wrapped.isFinished();
    }
}
3

I agree with @catrix, I was recently working on very similar code and .schedule() gave me some issues. I ended up creating a sequential command group and it worked much better. Additionally, if you are running auto cycles from the source to the amp then you would always be returning from the same direction but if you want to be able to hold a button and align with the amp from anywhere on the field It seems like you could run into issues when trying to align with the amp from directions you don’t have a predefined alignment for. The code I worked on below has two parts, the first is just like yours, pathfinds to the amp from anywhere, but instead of using a predefined path for the fine tune alignment, I use PID controllers for the X, Y, Theta of the bot and pass those back into the drivetrain. This gets the error to nearly zero. Pathfinding to a predefined path will probably be slightly faster and smoother but PID controller may be more versatile. Below is the code structure:

public class PathfindCmd extends SequentialCommandGroup {
public PathfindCmd(Pose2d targetPose, DrivetrainSubsystem drivetrainSubsystem) {
addCommands(generatePath(drivetrainSubsystem, targetPose), new FineTunePoseCmd(drivetrainSubsystem, targetPose));
}

//generates a pathplanner path that pathfinds to target pose while avoiding field elements. Only gets close, not exact which is why we have a fine tune cmd.
public Command generatePath(DrivetrainSubsystem drivetrainSubsystem, Pose2d targetPose) {
PathConstraints constraints = new PathConstraints(
2.0,
2.0,
1.5,
1.5);

Command pathfindingCommand = new PathfindHolonomic(
      targetPose,
      constraints,
      0.0, // Goal end velocity in m/s. Optional
      PoseEstimatorSubsystem::getPose,
      drivetrainSubsystem::getRobotRelativeSpeeds,
      drivetrainSubsystem::driveRobotRelative,
      new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class
                new PIDConstants(.5, 0.1, 0.0),
                new PIDConstants(.5, 0.1, 0.0),
                //new PIDConstants(1.1, 0.0, 0.0),
                //new PIDConstants(6.0, 0.0, 0.0),
                4.5, // Placeholder
                // AutoConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s
                Math.hypot(DriveConstants.TRACK_WIDTH / 2, DriveConstants.WHEEL_BASE / 2), // Drive base radius in meters. Distance from robot center to furthest module.
                new ReplanningConfig()), // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. Optional
      drivetrainSubsystem // Reference to drive subsystem to set requirements
);
return pathfindingCommand;

}

class FineTunePoseCmd extends Command {

private DrivetrainSubsystem drivetrainSubsystem;
private double targetX;
private double targetY;
private Rotation2d targetRotation;
private final PIDController xController = new PIDController(2, 0, 0);
private final PIDController yController = new PIDController(2, 0, 0);
private final PIDController thetaController = new PIDController(0.1, 0, 0);

public FineTunePoseCmd(DrivetrainSubsystem drivetrainSubsystem_para, Pose2d targetPose) {
  this.drivetrainSubsystem = drivetrainSubsystem_para;
  xController.setTolerance(0.05);
  yController.setTolerance(0.05);
  thetaController.setTolerance(0.5);

  targetX = targetPose.getX();
  targetY = targetPose.getY();
  targetRotation = targetPose.getRotation();

  addRequirements(drivetrainSubsystem);
}
@Override
public void initialize() {
}

@Override
public void execute() {

  double xVel = this.xController.calculate(PoseEstimatorSubsystem.getPose().getTranslation().getX(), targetX);
  double yVel = this.yController.calculate(PoseEstimatorSubsystem.getPose().getTranslation().getY(), targetY);
  double angVel = this.thetaController.calculate(DrivetrainSubsystem.getHeading(), targetRotation.getDegrees());  

  var chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xVel, yVel, angVel, DrivetrainSubsystem.getRotation2d());
  SwerveModuleState[] moduleStates = DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds);
  drivetrainSubsystem.setModuleStates(moduleStates);
}

@Override
public void end(boolean interrupted) {
  drivetrainSubsystem.stopModules();
}

@Override
public boolean isFinished() {
  return false;
}

}
}

1 Like
4

Would this be a better/proper implementation of the code:

Command driveIt = AutoBuilder.pathfindThenFollowPath(
        path,
        constraints,
        1.0 // Rotation delay distance in meters. This is how far the robot should travel
            // before attempting to rotate.
    );//.schedule();
    driveIt.execute();