95 units::meters_per_second_t maxWheelVelocity,
101 std::function<
void(units::volt_t, units::volt_t, units::volt_t,
148 units::meters_per_second_t maxWheelVelocity,
154 std::function<
void(units::volt_t, units::volt_t, units::volt_t,
190 units::meters_per_second_t maxWheelVelocity,
191 std::function<
void(units::meters_per_second_t, units::meters_per_second_t,
192 units::meters_per_second_t,
193 units::meters_per_second_t)>
230 units::meters_per_second_t maxWheelVelocity,
231 std::function<
void(units::meters_per_second_t, units::meters_per_second_t,
232 units::meters_per_second_t,
233 units::meters_per_second_t)>
241 void End(
bool interrupted)
override;
252 const units::meters_per_second_t m_maxWheelVelocity;
253 std::unique_ptr<frc::PIDController> m_frontLeftController;
254 std::unique_ptr<frc::PIDController> m_rearLeftController;
255 std::unique_ptr<frc::PIDController> m_frontRightController;
256 std::unique_ptr<frc::PIDController> m_rearRightController;
258 std::function<void(units::meters_per_second_t, units::meters_per_second_t,
259 units::meters_per_second_t, units::meters_per_second_t)>
261 std::function<void(units::volt_t, units::volt_t, units::volt_t,
268 units::second_t m_prevTime;
CRTP implementation to allow polymorphic decorator functions in Command.
Definition: CommandHelper.h:25
A command that uses two PID controllers (PIDController) and a profiled PID controller (ProfiledPIDCon...
Definition: MecanumControllerCommand.h:53
MecanumControllerCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SimpleMotorFeedforward< units::meters > feedforward, frc::MecanumDriveKinematics kinematics, frc::PIDController xController, frc::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, units::meters_per_second_t maxWheelVelocity, std::function< frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds, frc::PIDController frontLeftController, frc::PIDController rearLeftController, frc::PIDController frontRightController, frc::PIDController rearRightController, std::function< void(units::volt_t, units::volt_t, units::volt_t, units::volt_t)> output, Requirements requirements={})
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
MecanumControllerCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::MecanumDriveKinematics kinematics, frc::PIDController xController, frc::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, std::function< frc::Rotation2d()> desiredRotation, units::meters_per_second_t maxWheelVelocity, std::function< void(units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t)> output, Requirements requirements)
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
bool IsFinished() override
MecanumControllerCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SimpleMotorFeedforward< units::meters > feedforward, frc::MecanumDriveKinematics kinematics, frc::PIDController xController, frc::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, std::function< frc::Rotation2d()> desiredRotation, units::meters_per_second_t maxWheelVelocity, std::function< frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds, frc::PIDController frontLeftController, frc::PIDController rearLeftController, frc::PIDController frontRightController, frc::PIDController rearRightController, std::function< void(units::volt_t, units::volt_t, units::volt_t, units::volt_t)> output, Requirements requirements={})
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
void Initialize() override
void End(bool interrupted) override
MecanumControllerCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::MecanumDriveKinematics kinematics, frc::PIDController xController, frc::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, units::meters_per_second_t maxWheelVelocity, std::function< void(units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t, units::meters_per_second_t)> output, Requirements requirements={})
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
Represents requirements for a command, which is a set of (pointers to) subsystems.
Definition: Requirements.h:20
This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain (i....
Definition: HolonomicDriveController.h:32
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition: MecanumDriveKinematics.h:44
Implements a PID control loop.
Definition: PIDController.h:23
Represents a 2D pose containing translational and rotational elements.
Definition: Pose2d.h:23
Implements a PID control loop whose setpoint is constrained by a trapezoid profile.
Definition: ProfiledPIDController.h:35
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:23
A timer class.
Definition: Timer.h:36
Represents a time-parameterized trajectory.
Definition: Trajectory.h:25
Definition: TrapezoidProfileSubsystem.h:12
class WPILIB_DLLEXPORT Pose2d
Definition: Transform2d.h:13
Represents the wheel speeds for a mecanum drive drivetrain.
Definition: MecanumDriveWheelSpeeds.h:15