Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

125
wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
View File

@@ -209,6 +209,131 @@ class SwerveControllerCommand
output,
std::span<Subsystem* const> 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,
* provided by the odometry class.
* @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 output The raw output module states from the
* position controllers.
* @param requirements The subsystems to require.
*/
SwerveControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::SwerveDriveKinematics<NumModules> kinematics,
frc::HolonomicDriveController controller,
std::function<frc::Rotation2d()> desiredRotation,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
output,
std::initializer_list<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.
*
* <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,
* provided by the odometry class.
* @param kinematics The kinematics for the robot drivetrain.
* @param controller The HolonomicDriveController for the drivetrain.
* @param output The raw output module states from the
* position controllers.
* @param requirements The subsystems to require.
*/
SwerveControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::SwerveDriveKinematics<NumModules> kinematics,
frc::HolonomicDriveController controller,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
output,
std::initializer_list<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.
*
* <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,
* provided by the odometry class.
* @param kinematics The kinematics for the robot drivetrain.
* @param controller The HolonomicDriveController for the robot.
* @param desiredRotation The angle that the drivetrain should be
* facing. This is sampled at each time step.
* @param output The raw output module states from the
* position controllers.
* @param requirements The subsystems to require.
*/
SwerveControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::SwerveDriveKinematics<NumModules> kinematics,
frc::HolonomicDriveController controller,
std::function<frc::Rotation2d()> desiredRotation,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
output,
std::span<Subsystem* const> 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,
* provided by the odometry class.
* @param kinematics The kinematics for the robot drivetrain.
* @param controller The HolonomicDriveController for the drivetrain.
* @param output The raw output module states from the
* position controllers.
* @param requirements The subsystems to require.
*/
SwerveControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::SwerveDriveKinematics<NumModules> kinematics,
frc::HolonomicDriveController controller,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
output,
std::span<Subsystem* const> requirements = {});
void Initialize() override;
void Execute() override;

64
wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
View File

@@ -79,6 +79,70 @@ SwerveControllerCommand<NumModules>::SwerveControllerCommand(
this->AddRequirements(requirements);
}
template <size_t NumModules>
SwerveControllerCommand<NumModules>::SwerveControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::SwerveDriveKinematics<NumModules> kinematics,
frc::HolonomicDriveController controller,
std::function<frc::Rotation2d()> desiredRotation,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
std::initializer_list<Subsystem*> requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_kinematics(kinematics),
m_controller(std::move(controller)),
m_desiredRotation(std::move(desiredRotation)),
m_outputStates(output) {
this->AddRequirements(requirements);
}
template <size_t NumModules>
SwerveControllerCommand<NumModules>::SwerveControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::SwerveDriveKinematics<NumModules> kinematics,
frc::HolonomicDriveController controller,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
std::initializer_list<Subsystem*> requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_kinematics(kinematics),
m_controller(std::move(controller)),
m_outputStates(output) {
this->AddRequirements(requirements);
}
template <size_t NumModules>
SwerveControllerCommand<NumModules>::SwerveControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::SwerveDriveKinematics<NumModules> kinematics,
frc::HolonomicDriveController controller,
std::function<frc::Rotation2d()> desiredRotation,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
std::span<Subsystem* const> requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_kinematics(kinematics),
m_controller(std::move(controller)),
m_desiredRotation(std::move(desiredRotation)),
m_outputStates(output) {
this->AddRequirements(requirements);
}
template <size_t NumModules>
SwerveControllerCommand<NumModules>::SwerveControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
frc::SwerveDriveKinematics<NumModules> kinematics,
frc::HolonomicDriveController controller,
std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
std::span<Subsystem* const> requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_kinematics(kinematics),
m_controller(std::move(controller)),
m_outputStates(output) {
this->AddRequirements(requirements);
}
template <size_t NumModules>
void SwerveControllerCommand<NumModules>::Initialize() {
if (m_desiredRotation == nullptr) {