SwerveControllerCommand

class commands2.SwerveControllerCommand(*args, **kwargs)

Bases: Command

A command that uses two PID controllers (wpimath.controller.PIDController) and a HolonomicDriveController (wpimath.controller.HolonomicDriveController) to follow a trajectory (wpimath.trajectory.Trajectory) with a swerve drive.

This command outputs the raw desired Swerve Module States (wpimath.kinematics.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.

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. Can be kinematics for 2/3/4/6 SwerveKinematics.

• controller – The HolonomicDriveController for the drivetrain. If you have x, y, and theta controllers, pass them into HolonomicPIDController.

• outputModuleStates – The raw output module states from the position controllers.

• requirements – The subsystems to require.

• desiredRotation – (optional) The angle that the drivetrain should be facing. This is sampled at each time step. If not specified, that rotation of the final pose in the trajectory is used.

end(interrupted)

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 andThen() instead.

Parameters:

interrupted – whether the command was interrupted/canceled

execute()

The main body of a command. Called repeatedly while the command is scheduled.

initialize()

The initial subroutine of a command. Called once when the command is initially scheduled.

isFinished()

Whether the command has finished. Once a command finishes, the scheduler will call its commands2.Command.end() method and un-schedule it.

Returns:

whether the command has finished.