Dynamic Command Creation

[Christopher_Burie]

I am trying to dynamically create multiple PathPlanner PPSwerveController commands for an alignment routine. However, I cannot figure out how to dynamically create and run a command in a sequential command group. I need to update the Path Points based on more accurate Limelight data after I move and then run a second command. Does anyone know how this can be accomplished in the command based framework or how I could restructure my code to effectively accomplish the same as dynamically generating commands. The code of interest is put in InstantCommands for now but is clearly wrong.

Here is my current code:

package frc.robot.commands.Alignment;

import com.pathplanner.lib.PathConstraints;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.PathPoint;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.RobotContainer;
import frc.robot.commands.ArmMotions.Place;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Limelight;
import frc.robot.subsystems.Swerve;

public class LimelightPlace extends SequentialCommandGroup {
/** Creates a new Place. */

Swerve swerve;
Limelight limelight;
Arm arm;
Elevator elevator;
RobotContainer container;

PathPlannerTrajectory targetTraj;

public LimelightPlace(Swerve swerve, Limelight limelight, RobotContainer container, Arm arm, Elevator elevator) {

this.swerve = swerve;
this.limelight = limelight;
this.container = container;

this.arm = arm;
this.elevator = elevator;

// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
addCommands(
  new AlignToAngle(swerve, 0),

  new InstantCommand(() -> targetTraj = PathPlanner.generatePath(
    new PathConstraints(1, 1),
    
    new PathPoint(
      swerve.getPose().getTranslation(), 
      Rotation2d.fromDegrees(0), 
      swerve.getYaw()
    ), //Sets starting point of path to current position.

    new PathPoint(
      swerve.getPose().getTranslation().plus(new Translation2d(limelight.getTz() + 1, -limelight.getTx())),
      Rotation2d.fromDegrees(0),
      swerve.getYaw()
    )
    ),
    limelight
),

new InstantCommand(() -> swerve.followTrajectoryCommand(targetTraj, false), swerve),

new InstantCommand(() -> targetTraj = PathPlanner.generatePath(
    new PathConstraints(1, 1),
    
    new PathPoint(
      swerve.getPose().getTranslation(), 
      Rotation2d.fromDegrees(0), 
      swerve.getYaw()
    ), //Sets starting point of path to current position.

    new PathPoint(
      swerve.getPose().getTranslation().plus(new Translation2d(limelight.getTz() + 1, -limelight.getTx())),
      Rotation2d.fromDegrees(0),
      swerve.getYaw()
    )
    ),
    limelight
  ),
  new InstantCommand(() -> swerve.followTrajectoryCommand(targetTraj, false)),

  new Place(container, arm, elevator).getCommand()
  //Add command to drive forward to place and then release. Add code to align with tx offset when close. Add code for cones offset.
);

}
}

[wanderingcarrot]

The simple answer is you don’t move the PathPoint, you move the robot. Take a look at the PoseEstimator classes, they replace odometry and allow you to correct your position mid-path. The PID controllers within PathPlanner will move you to the right spot if your position is correct.

[Christopher_Burie]

I am using the SwerveDrivePoseEstimator class in my Swerve subsystem. I have not experimented with updating my pose based on vision data, but I do not know if this would help. I am not using the Limelight for absolute field odometry but am instead using the Limelight position relative to the AprilTag target space to drive to a position offset from the AprilTag. I am only trying to figure out how much to translate the robot from its current pose. The limelight data is more accurate as I get closer, so I would like to be able to run the command again for simplicity.

Even if updating odometry could be used as a solution if I create my PathPoints based on field relative odometry, I still want to know the general question of how to use dynamic command generation. Maybe I am using the Command-Based paradigm incorrectly, but there have been other use cases where I originally was trying to create commands with dynamic parameters, such as setting arm angles to different values depending on my target level, but I had to switch to ConditionalCommands because I could not get it to work by passing values during command creation within a SequentialCommandGroup.