Running commands during autonomous path following

Technical Programming
1

Our robots odometry system is working and is capable of following paths during autonomous, however I want to be able to have the robot do other tasks while moving along its path, for example: lowering an intake to pick up notes and raising said intake.
I reasoned I could do this by creating a path to the first note, moving intake, creating another path… etc. but this is very inefficient and I know there is another way to do this, but don’t know what it is.

Here is how I am running the autonomous command currently, in RobotContainer:

Trajectory ExampleTrajectory =
TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
m_robotDrive.getPose(),
// Pass through these two interior waypoints, making an ‘s’ curve path
List.of(new Translation2d(Units.inchesToMeters(114), Units.inchesToMeters(161.64))),
// End 3 meters straight ahead of where we started, facing forward
new Pose2d(Units.inchesToMeters(150), Units.inchesToMeters(293.64), new Rotation2d(0)),
config);

SwerveControllerCommand swerveControllerCommand =
new SwerveControllerCommand(
exampleTrajectory,
m_robotDrive::getPose, // Functional interface to feed supplier
DriveConstants.kDriveKinematics,
// Position controllers
new PIDController(AutoConstants.kPXController, 0, 0),
new PIDController(AutoConstants.kPYController, 0, 0),
thetaController,
m_robotDrive::setModuleStates,
m_robotDrive);

return swerveControllerCommand.andThen(() → m_robotDrive.drive(0.0, 0.0, 0.0, false, true));

Thanks for reading

2

Hi!

I’d recommend using Path Planner which will allow you create autos using a graphic interface and also select when to run commands. All you’d have to do is change your SwerveControllerCommand to use AutoBuilder instead. Then you can create autos with the Path Planner app and deploy them to your robot code folder. With these autos, you can really easily load them with this class.

To add commands to run during your paths, you need to declare these NamedCommands in your code. In the graphic interface, you can then make Event Markers that will run your commands. Then you’re all set!

Here’s some example code.

I hope this was helpful! Let me know if you have any questions!

3 Likes