WPILibC++  2020.3.2
SwerveControllerCommand.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/geometry/Pose2d.h>
16 #include <frc/kinematics/ChassisSpeeds.h>
17 #include <frc/kinematics/SwerveDriveKinematics.h>
18 #include <frc/kinematics/SwerveModuleState.h>
19 #include <frc/trajectory/Trajectory.h>
20 #include <units/units.h>
21 #include <wpi/ArrayRef.h>
22 
23 #include "CommandBase.h"
24 #include "CommandHelper.h"
25 #include "frc2/Timer.h"
26 
27 #pragma once
28 
29 namespace frc2 {
30 
50 template <size_t NumModules>
52  : public CommandHelper<CommandBase, SwerveControllerCommand<NumModules>> {
53  using voltsecondspermeter =
54  units::compound_unit<units::voltage::volt, units::second,
55  units::inverse<units::meter>>;
56  using voltsecondssquaredpermeter =
57  units::compound_unit<units::voltage::volt, units::squared<units::second>,
58  units::inverse<units::meter>>;
59 
60  public:
89  frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
91  frc2::PIDController xController, frc2::PIDController yController,
93  std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
94  output,
95  std::initializer_list<Subsystem*> requirements);
96 
125  frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
127  frc2::PIDController xController, frc2::PIDController yController,
129  std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
130  output,
131  wpi::ArrayRef<Subsystem*> requirements = {});
132 
133  void Initialize() override;
134 
135  void Execute() override;
136 
137  void End(bool interrupted) override;
138 
139  bool IsFinished() override;
140 
141  private:
142  frc::Trajectory m_trajectory;
143  std::function<frc::Pose2d()> m_pose;
145  std::unique_ptr<frc2::PIDController> m_xController;
146  std::unique_ptr<frc2::PIDController> m_yController;
147  std::unique_ptr<frc::ProfiledPIDController<units::radians>> m_thetaController;
148  std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
149  m_outputStates;
150 
151  frc2::Timer m_timer;
152  units::second_t m_prevTime;
153  frc::Pose2d m_finalPose;
154 };
155 } // namespace frc2
156 
157 #include "SwerveControllerCommand.inc"
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::SwerveControllerCommand::SwerveControllerCommand
SwerveControllerCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::SwerveDriveKinematics< NumModules > kinematics, frc2::PIDController xController, frc2::PIDController yController, frc::ProfiledPIDController< units::radians > thetaController, 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.
Definition: SwerveControllerCommand.inc:15
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::SwerveControllerCommand::Execute
void Execute() override
The main body of a command.
Definition: SwerveControllerCommand.inc:63
frc::SwerveDriveKinematics
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module ...
Definition: SwerveDriveKinematics.h:47
frc2::SwerveControllerCommand::End
void End(bool interrupted) override
The action to take when the command ends.
Definition: SwerveControllerCommand.inc:100
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::SwerveControllerCommand::Initialize
void Initialize() override
The initial subroutine of a command.
Definition: SwerveControllerCommand.inc:55
frc2::PIDController
Implements a PID control loop.
Definition: PIDController.h:23
frc2::SwerveControllerCommand::IsFinished
bool IsFinished() override
Whether the command has finished.
Definition: SwerveControllerCommand.inc:105
frc2::SwerveControllerCommand
A command that uses two PID controllers (PIDController) and a ProfiledPIDController (ProfiledPIDContr...
Definition: SwerveControllerCommand.h:51