WPILibC++  2020.3.2-60-g3011ebe
RamseteCommand.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 #pragma once
9 
10 #include <functional>
11 #include <initializer_list>
12 #include <memory>
13 
14 #include <frc/controller/PIDController.h>
15 #include <frc/controller/RamseteController.h>
16 #include <frc/controller/SimpleMotorFeedforward.h>
17 #include <frc/geometry/Pose2d.h>
18 #include <frc/kinematics/DifferentialDriveKinematics.h>
19 #include <frc/trajectory/Trajectory.h>
20 #include <units/units.h>
21 #include <wpi/ArrayRef.h>
22 
23 #include "frc2/Timer.h"
24 #include "frc2/command/CommandBase.h"
25 #include "frc2/command/CommandHelper.h"
26 
27 namespace frc2 {
45 class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
46  public:
74  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
75  frc::RamseteController controller,
78  std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
79  frc2::PIDController leftController,
80  frc2::PIDController rightController,
81  std::function<void(units::volt_t, units::volt_t)> output,
82  std::initializer_list<Subsystem*> requirements);
83 
111  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
112  frc::RamseteController controller,
115  std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
116  frc2::PIDController leftController,
117  frc2::PIDController rightController,
118  std::function<void(units::volt_t, units::volt_t)> output,
119  wpi::ArrayRef<Subsystem*> requirements = {});
120 
137  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
138  frc::RamseteController controller,
140  std::function<void(units::meters_per_second_t,
141  units::meters_per_second_t)>
142  output,
143  std::initializer_list<Subsystem*> requirements);
144 
161  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
162  frc::RamseteController controller,
164  std::function<void(units::meters_per_second_t,
165  units::meters_per_second_t)>
166  output,
167  wpi::ArrayRef<Subsystem*> requirements = {});
168 
169  void Initialize() override;
170 
171  void Execute() override;
172 
173  void End(bool interrupted) override;
174 
175  bool IsFinished() override;
176 
177  private:
178  frc::Trajectory m_trajectory;
179  std::function<frc::Pose2d()> m_pose;
180  frc::RamseteController m_controller;
183  std::function<frc::DifferentialDriveWheelSpeeds()> m_speeds;
184  std::unique_ptr<frc2::PIDController> m_leftController;
185  std::unique_ptr<frc2::PIDController> m_rightController;
186  std::function<void(units::volt_t, units::volt_t)> m_outputVolts;
187  std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
188  m_outputVel;
189 
190  Timer m_timer;
191  units::second_t m_prevTime;
193  bool m_usePID;
194 };
195 } // namespace frc2
frc::SimpleMotorFeedforward< units::meters >
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::RamseteCommand::End
void End(bool interrupted) override
The action to take when the command ends.
frc::Pose2d
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
frc::DifferentialDriveWheelSpeeds
Represents the wheel speeds for a differential drive drivetrain.
Definition: DifferentialDriveWheelSpeeds.h:16
frc2::RamseteCommand::IsFinished
bool IsFinished() override
Whether the command has finished.
frc::DifferentialDriveKinematics
Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel velo...
Definition: DifferentialDriveKinematics.h:25
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:27
frc2::RamseteCommand::Initialize
void Initialize() override
The initial subroutine of a command.
frc2::RamseteCommand::RamseteCommand
RamseteCommand(frc::Trajectory trajectory, std::function< frc::Pose2d()> pose, frc::RamseteController controller, frc::SimpleMotorFeedforward< units::meters > feedforward, frc::DifferentialDriveKinematics kinematics, std::function< frc::DifferentialDriveWheelSpeeds()> wheelSpeeds, frc2::PIDController leftController, frc2::PIDController rightController, std::function< void(units::volt_t, units::volt_t)> output, std::initializer_list< Subsystem * > requirements)
Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
frc2::RamseteCommand
A command that uses a RAMSETE controller to follow a trajectory with a differential drive.
Definition: RamseteCommand.h:45
frc2::PIDController
Implements a PID control loop.
Definition: PIDController.h:23
frc2::RamseteCommand::Execute
void Execute() override
The main body of a command.
frc::RamseteController
Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model to ...
Definition: RamseteController.h:45