diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java index 5c45cf1b78..1db7dd4383 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java @@ -70,23 +70,17 @@ public class SwerveControllerCommand extends CommandBase { Supplier desiredRotation, Consumer 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. + * + *

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. + * + * @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 pose, + SwerveDriveKinematics kinematics, + HolonomicDriveController controller, + Consumer 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. + * + *

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 pose, + SwerveDriveKinematics kinematics, + HolonomicDriveController controller, + Supplier desiredRotation, + Consumer 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(); diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h index fefb793e65..d51ae1e47e 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h @@ -209,6 +209,131 @@ class SwerveControllerCommand output, std::span 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. + * + * @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 pose, + frc::SwerveDriveKinematics kinematics, + frc::HolonomicDriveController controller, + std::function desiredRotation, + std::function)> + output, + std::initializer_list 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. + * + * @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 pose, + frc::SwerveDriveKinematics kinematics, + frc::HolonomicDriveController controller, + std::function)> + output, + std::initializer_list 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. + * + * + * @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 pose, + frc::SwerveDriveKinematics kinematics, + frc::HolonomicDriveController controller, + std::function desiredRotation, + std::function)> + output, + std::span 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. + * + * @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 pose, + frc::SwerveDriveKinematics kinematics, + frc::HolonomicDriveController controller, + std::function)> + output, + std::span requirements = {}); + void Initialize() override; void Execute() override; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc index 4b6e76c467..a644057dd3 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc @@ -79,6 +79,70 @@ SwerveControllerCommand::SwerveControllerCommand( this->AddRequirements(requirements); } +template +SwerveControllerCommand::SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc::HolonomicDriveController controller, + std::function desiredRotation, + std::function)> output, + std::initializer_list 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 +SwerveControllerCommand::SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc::HolonomicDriveController controller, + std::function)> output, + std::initializer_list 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 +SwerveControllerCommand::SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc::HolonomicDriveController controller, + std::function desiredRotation, + std::function)> output, + std::span 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 +SwerveControllerCommand::SwerveControllerCommand( + frc::Trajectory trajectory, std::function pose, + frc::SwerveDriveKinematics kinematics, + frc::HolonomicDriveController controller, + std::function)> output, + std::span 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 void SwerveControllerCommand::Initialize() { if (m_desiredRotation == nullptr) { diff --git a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h index 93aa58e704..9a749c6f0d 100644 --- a/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h +++ b/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h @@ -45,6 +45,12 @@ class WPILIB_DLLEXPORT HolonomicDriveController { frc2::PIDController xController, frc2::PIDController yController, ProfiledPIDController thetaController); + HolonomicDriveController(const HolonomicDriveController&) = default; + HolonomicDriveController& operator=(const HolonomicDriveController&) = + default; + HolonomicDriveController(HolonomicDriveController&&) = default; + HolonomicDriveController& operator=(HolonomicDriveController&&) = default; + /** * Returns true if the pose error is within tolerance of the reference. */