Class SwerveControllerCommand
- java.lang.Object
-
- edu.wpi.first.wpilibj2.command.CommandBase
-
- edu.wpi.first.wpilibj2.command.SwerveControllerCommand
-
public class SwerveControllerCommand extends CommandBase
A command that uses two PID controllers (PIDController
) and a ProfiledPIDController (ProfiledPIDController
) to follow a trajectoryTrajectory
with a swerve drive.This command outputs the raw desired Swerve Module States (
SwerveModuleState
) in an array. The desired wheel and module rotation velocities should be taken from those and used in velocity PIDs.The robot angle controller does not follow the angle given by the trajectory but rather goes to the angle given in the final state of the trajectory.
-
-
Field Summary
-
Fields inherited from class edu.wpi.first.wpilibj2.command.CommandBase
m_requirements
-
-
Constructor Summary
Constructors Constructor Description SwerveControllerCommand(Trajectory trajectory, Supplier<Pose2d> pose, SwerveDriveKinematics kinematics, PIDController xController, PIDController yController, ProfiledPIDController thetaController, Consumer<SwerveModuleState[]> outputModuleStates, Subsystem... requirements)
Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory.SwerveControllerCommand(Trajectory trajectory, Supplier<Pose2d> pose, SwerveDriveKinematics kinematics, PIDController xController, PIDController yController, ProfiledPIDController thetaController, Supplier<Rotation2d> desiredRotation, Consumer<SwerveModuleState[]> outputModuleStates, Subsystem... requirements)
Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory.
-
Method Summary
All Methods Instance Methods Concrete Methods Modifier and Type Method Description void
end(boolean interrupted)
The action to take when the command ends.void
execute()
The main body of a command.void
initialize()
The initial subroutine of a command.boolean
isFinished()
Whether the command has finished.-
Methods inherited from class edu.wpi.first.wpilibj2.command.CommandBase
addRequirements, getName, getRequirements, getSubsystem, initSendable, setName, setSubsystem, withName
-
Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
-
Methods inherited from interface edu.wpi.first.wpilibj2.command.Command
alongWith, andThen, andThen, asProxy, beforeStarting, beforeStarting, cancel, deadlineWith, hasRequirement, isScheduled, perpetually, raceWith, runsWhenDisabled, schedule, schedule, withInterrupt, withTimeout
-
-
-
-
Constructor Detail
-
SwerveControllerCommand
public SwerveControllerCommand(Trajectory trajectory, Supplier<Pose2d> pose, SwerveDriveKinematics kinematics, PIDController xController, PIDController yController, ProfiledPIDController thetaController, Supplier<Rotation2d> desiredRotation, Consumer<SwerveModuleState[]> outputModuleStates, Subsystem... requirements)
Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will not return output voltages but rather raw module states from the position controllers which need to be put into a velocity PID.Note: The controllers will *not* set the outputVolts to zero upon completion of the path- this is left to the user, since it is not appropriate for paths with nonstationary endstates.
- Parameters:
trajectory
- The trajectory to follow.pose
- A function that supplies the robot pose - use one of the odometry classes to provide this.kinematics
- The kinematics for the robot drivetrain.xController
- The Trajectory Tracker PID controller for the robot's x position.yController
- The Trajectory Tracker PID controller for the robot's y position.thetaController
- The Trajectory Tracker PID controller for angle for the robot.desiredRotation
- The angle that the drivetrain should be facing. This is sampled at each time step.outputModuleStates
- The raw output module states from the position controllers.requirements
- The subsystems to require.
-
SwerveControllerCommand
public SwerveControllerCommand(Trajectory trajectory, Supplier<Pose2d> pose, SwerveDriveKinematics kinematics, PIDController xController, PIDController yController, ProfiledPIDController thetaController, Consumer<SwerveModuleState[]> outputModuleStates, Subsystem... requirements)
Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will not return output voltages but rather raw module states from the position controllers which need to be put into a velocity PID.Note: The controllers will *not* set the outputVolts to zero upon completion of the path- this is left to the user, since it is not appropriate for paths with nonstationary endstates.
Note 2: The final rotation of the robot will be set to the rotation of the final pose in the trajectory. The robot will not follow the rotations from the poses at each timestep. If alternate rotation behavior is desired, the other constructor with a supplier for rotation should be used.
- Parameters:
trajectory
- The trajectory to follow.pose
- A function that supplies the robot pose - use one of the odometry classes to provide this.kinematics
- The kinematics for the robot drivetrain.xController
- The Trajectory Tracker PID controller for the robot's x position.yController
- The Trajectory Tracker PID controller for the robot's y position.thetaController
- The Trajectory Tracker PID controller for angle for the robot.outputModuleStates
- The raw output module states from the position controllers.requirements
- The subsystems to require.
-
-
Method Detail
-
initialize
public void initialize()
Description copied from interface:Command
The initial subroutine of a command. Called once when the command is initially scheduled.
-
execute
public void execute()
Description copied from interface:Command
The main body of a command. Called repeatedly while the command is scheduled.
-
end
public void end(boolean interrupted)
Description copied from interface:Command
The action to take when the command ends. Called when either the command finishes normally, or when it interrupted/canceled.Do not schedule commands here that share requirements with this command. Use
Command.andThen(Command...)
instead.- Parameters:
interrupted
- whether the command was interrupted/canceled
-
isFinished
public boolean isFinished()
Description copied from interface:Command
Whether the command has finished. Once a command finishes, the scheduler will call its end() method and un-schedule it.- Returns:
- whether the command has finished.
-
-