Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[commands] Add constructor for SwerveControllerCommand that takes a HolonomicDriveController (#4785)

Browse Source 
Also adds copy and move constructors to HolonomicDriveController.
This commit is contained in:
Ryan Blue
2022-12-25 21:48:27 -05:00
committed by GitHub
parent 1e7fcd5637
commit 6efb9ee405
4 changed files with 282 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
View File

@@ -70,23 +70,17 @@ public class SwerveControllerCommand extends CommandBase {
Supplier<Rotation2d> desiredRotation,
Consumer<SwerveModuleState[]> outputModuleStates,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
m_controller =
this(
trajectory,
pose,
kinematics,
new HolonomicDriveController(
requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
requireNonNullParam(yController, "yController", "SwerveControllerCommand"),
requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand"));
m_outputModuleStates =
requireNonNullParam(outputModuleStates, "outputModuleStates", "SwerveControllerCommand");
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "SwerveControllerCommand");
addRequirements(requirements);
requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand")),
desiredRotation,
outputModuleStates,
requirements);
}
/**
@@ -134,6 +128,85 @@ public class SwerveControllerCommand extends CommandBase {
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.
*
* <p>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.
*
* <p>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.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of the odometry classes to
* provide this.
* @param kinematics The kinematics for the robot drivetrain.
* @param controller The HolonomicDriveController for the drivetrain.
* @param outputModuleStates The raw output module states from the position controllers.
* @param requirements The subsystems to require.
*/
public SwerveControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
SwerveDriveKinematics kinematics,
HolonomicDriveController controller,
Consumer<SwerveModuleState[]> outputModuleStates,
Subsystem... requirements) {
this(
trajectory,
pose,
kinematics,
controller,
() ->
trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
outputModuleStates,
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.
*
* <p>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.
*
* @param trajectory The trajectory to follow.
* @param pose A function that supplies the robot pose - use one of the odometry classes to
* provide this.
* @param kinematics The kinematics for the robot drivetrain.
* @param controller The HolonomicDriveController for the drivetrain.
* @param desiredRotation The angle that the drivetrain should be facing. This is sampled at each
* time step.
* @param outputModuleStates The raw output module states from the position controllers.
* @param requirements The subsystems to require.
*/
public SwerveControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
SwerveDriveKinematics kinematics,
HolonomicDriveController controller,
Supplier<Rotation2d> desiredRotation,
Consumer<SwerveModuleState[]> outputModuleStates,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
m_controller = requireNonNullParam(controller, "controller", "SwerveControllerCommand");
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "SwerveControllerCommand");
m_outputModuleStates =
requireNonNullParam(outputModuleStates, "outputModuleStates", "SwerveControllerCommand");
addRequirements(requirements);
}
@Override
public void initialize() {
m_timer.reset();