Hello!
We are new to the Command-Based framework, and are trying to figure out how to build a simple autonomous program that simply follows an AprilTag around using the TrajectoryGenerator and SwerveControllerCommand. Our goal is that it will drive to an AprilTag someone is holding, and follow it around if they move it (not really trying to accomplish any particular game task, just get a basic implementation running).
We were able to get our auto command to run once, but run into issues when it reaches the end of its trajectory and tries calculating a new one. Because the TrajectoryGenerator takes a Pose2D object for the “start” argument and not a function, we can’t get the trajectory to update after each loop. As a result, on the first iteration of the command, the robot drives as we expect, but for each iteration afterwards, the “start” parameter is still the original start position of the robot - not its current position. As a result, the trajectory it follows is wrong for every subsequent run.
Is there a way for us to:
- Dynamically create and run the SwerveControllerCommand at the start of each loop so that we can re-create the Trajectory object?
- Dynamically update the Trajectory we pass to the SwerveControllerCommand so that we can modify its start position at the beginning of each loop?
- Repeatedly re-create our entire autonomous command and run it in auto so that our getPose() function can be called each time to update the trajectory?
Again, we’re very new to this whole method of programming, so please let me know if we’re going about this all wrong. Thanks!
Autos.java (just the function for getting our auto command):
public static Command visionAuto(DriveSubsystem driveSubsystem, VisionSubsystem vision) {
TrajectoryConfig config = new TrajectoryConfig(
AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
.setKinematics(DriveConstants.kDriveKinematics
);
var thetaController = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
thetaController.enableContinuousInput(-Math.PI, Math.PI);
Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
vision.getPose(), // This is only eval'ed once!! (presumably our problem)
List.of(),
new Pose2d(2, 0, new Rotation2d(0)),
config
);
return new SwerveControllerCommand(
trajectory,
vision::getPose,
DriveConstants.kDriveKinematics,
new PIDController(AutoConstants.kPXController, 0, 0),
new PIDController(AutoConstants.kPYController, 0, 0),
thetaController,
driveSubsystem::setModuleStates,
driveSubsystem,
vision
);
}
RobotContainer.java (just the auto function)
public Command getAutonomousCommand() {
return Autos.visionAuto(m_robotDrive, visionSubsystem).repeatedly();
}
