WPILibC++  2020.3.2
MecanumControllerCommand.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #include <cmath>
9 #include <functional>
10 #include <initializer_list>
11 #include <memory>
12 
13 #include <frc/controller/PIDController.h>
14 #include <frc/controller/ProfiledPIDController.h>
15 #include <frc/controller/SimpleMotorFeedforward.h>
16 #include <frc/geometry/Pose2d.h>
17 #include <frc/kinematics/ChassisSpeeds.h>
18 #include <frc/kinematics/MecanumDriveKinematics.h>
19 #include <frc/kinematics/MecanumDriveWheelSpeeds.h>
20 #include <frc/trajectory/Trajectory.h>
21 #include <units/units.h>
22 #include <wpi/ArrayRef.h>
23 
24 #include "CommandBase.h"
25 #include "CommandHelper.h"
26 #include "frc2/Timer.h"
27 
28 #pragma once
29 
30 namespace frc2 {
51  : public CommandHelper<CommandBase, MecanumControllerCommand> {
52  public:
88  frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
90  frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
91  frc2::PIDController yController,
93  units::meters_per_second_t maxWheelVelocity,
94  std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
95  frc2::PIDController frontLeftController,
96  frc2::PIDController rearLeftController,
97  frc2::PIDController frontRightController,
98  frc2::PIDController rearRightController,
99  std::function<void(units::volt_t, units::volt_t, units::volt_t,
100  units::volt_t)>
101  output,
102  std::initializer_list<Subsystem*> requirements);
103 
139  frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
141  frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
142  frc2::PIDController yController,
144  units::meters_per_second_t maxWheelVelocity,
145  std::function<frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds,
146  frc2::PIDController frontLeftController,
147  frc2::PIDController rearLeftController,
148  frc2::PIDController frontRightController,
149  frc2::PIDController rearRightController,
150  std::function<void(units::volt_t, units::volt_t, units::volt_t,
151  units::volt_t)>
152  output,
153  wpi::ArrayRef<Subsystem*> requirements = {});
154 
182  frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
183  frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
184  frc2::PIDController yController,
186  units::meters_per_second_t maxWheelVelocity,
187  std::function<void(units::meters_per_second_t, units::meters_per_second_t,
188  units::meters_per_second_t,
189  units::meters_per_second_t)>
190  output,
191  std::initializer_list<Subsystem*> requirements);
192 
220  frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
221  frc::MecanumDriveKinematics kinematics, frc2::PIDController xController,
222  frc2::PIDController yController,
224  units::meters_per_second_t maxWheelVelocity,
225  std::function<void(units::meters_per_second_t, units::meters_per_second_t,
226  units::meters_per_second_t,
227  units::meters_per_second_t)>
228  output,
229  wpi::ArrayRef<Subsystem*> requirements = {});
230 
231  void Initialize() override;
232 
233  void Execute() override;
234 
235  void End(bool interrupted) override;
236 
237  bool IsFinished() override;
238 
239  private:
240  frc::Trajectory m_trajectory;
241  std::function<frc::Pose2d()> m_pose;
243  frc::MecanumDriveKinematics m_kinematics;
244  std::unique_ptr<frc2::PIDController> m_xController;
245  std::unique_ptr<frc2::PIDController> m_yController;
246  std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
247  const units::meters_per_second_t m_maxWheelVelocity;
248  std::unique_ptr<frc2::PIDController> m_frontLeftController;
249  std::unique_ptr<frc2::PIDController> m_rearLeftController;
250  std::unique_ptr<frc2::PIDController> m_frontRightController;
251  std::unique_ptr<frc2::PIDController> m_rearRightController;
252  std::function<frc::MecanumDriveWheelSpeeds()> m_currentWheelSpeeds;
253  std::function<void(units::meters_per_second_t, units::meters_per_second_t,
254  units::meters_per_second_t, units::meters_per_second_t)>
255  m_outputVel;
256  std::function<void(units::volt_t, units::volt_t, units::volt_t,
257  units::volt_t)>
258  m_outputVolts;
259 
260  bool m_usePID;
261  frc2::Timer m_timer;
262  frc::MecanumDriveWheelSpeeds m_prevSpeeds;
263  units::second_t m_prevTime;
264  frc::Pose2d m_finalPose;
265 };
266 } // namespace frc2
frc2::MecanumControllerCommand::Execute
void Execute() override
The main body of a command.
frc::SimpleMotorFeedforward< units::meters >
frc2::MecanumControllerCommand
A command that uses two PID controllers (PIDController) and a ProfiledPIDController (ProfiledPIDContr...
Definition: MecanumControllerCommand.h:50
wpi::ArrayRef
ArrayRef - Represent a constant reference to an array (0 or more elements consecutively in memory),...
Definition: ArrayRef.h:42
frc2::Timer
A wrapper for the frc::Timer class that returns unit-typed values.
Definition: Timer.h:40
frc2::MecanumControllerCommand::IsFinished
bool IsFinished() override
Whether the command has finished.
frc::MecanumDriveWheelSpeeds
Represents the wheel speeds for a mecanum drive drivetrain.
Definition: MecanumDriveWheelSpeeds.h:16
frc2::MecanumControllerCommand::Initialize
void Initialize() override
The initial subroutine of a command.
frc2::MecanumControllerCommand::MecanumControllerCommand
MecanumControllerCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SimpleMotorFeedforward< units::meters > feedforward, frc::MecanumDriveKinematics kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, units::meters_per_second_t maxWheelVelocity, std::function< frc::MecanumDriveWheelSpeeds()> currentWheelSpeeds, frc2::PIDController frontLeftController, frc2::PIDController rearLeftController, frc2::PIDController frontRightController, frc2::PIDController rearRightController, std::function< void(units::volt_t, units::volt_t, units::volt_t, units::volt_t)> output, std::initializer_list< Subsystem * > requirements)
Constructs a new MecanumControllerCommand that when executed will follow the provided trajectory.
frc::Pose2d
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
frc::ProfiledPIDController
Implements a PID control loop whose setpoint is constrained by a trapezoid profile.
Definition: ProfiledPIDController.h:33
frc2::MecanumControllerCommand::End
void End(bool interrupted) override
The action to take when the command ends.
frc2::CommandHelper
CRTP implementation to allow polymorphic decorator functions in Command.
Definition: CommandHelper.h:26
frc::Trajectory
Represents a time-parameterized trajectory.
Definition: Trajectory.h:34
frc2::PIDController
Implements a PID control loop.
Definition: PIDController.h:23
frc::MecanumDriveKinematics
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition: MecanumDriveKinematics.h:42