WPILibC++  2020.3.2-60-g3011ebe
ProfiledPIDSubsystem.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 <frc/controller/ProfiledPIDController.h>
11 #include <units/units.h>
12 
13 #include "frc2/command/SubsystemBase.h"
14 
15 namespace frc2 {
22 template <class Distance>
24  using Distance_t = units::unit_t<Distance>;
25  using Velocity =
26  units::compound_unit<Distance, units::inverse<units::seconds>>;
27  using Velocity_t = units::unit_t<Velocity>;
28  using State = typename frc::TrapezoidProfile<Distance>::State;
29 
30  public:
38  Distance_t initialPosition = Distance_t{0})
39  : m_controller{controller} {
40  SetGoal(initialPosition);
41  }
42 
43  void Periodic() override {
44  if (m_enabled) {
45  UseOutput(m_controller.Calculate(GetMeasurement(), m_goal),
46  m_controller.GetSetpoint());
47  }
48  }
49 
55  void SetGoal(State goal) { m_goal = goal; }
56 
62  void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
63 
67  virtual void Enable() {
68  m_controller.Reset(GetMeasurement());
69  m_enabled = true;
70  }
71 
75  virtual void Disable() {
76  UseOutput(0, State{Distance_t(0), Velocity_t(0)});
77  m_enabled = false;
78  }
79 
85  bool IsEnabled() { return m_enabled; }
86 
93 
94  protected:
96  bool m_enabled{false};
97 
104  virtual Distance_t GetMeasurement() = 0;
105 
113  virtual void UseOutput(double output, State setpoint) = 0;
114 
115  private:
116  State m_goal;
117 };
118 } // namespace frc2
frc2::ProfiledPIDSubsystem::Disable
virtual void Disable()
Disables the PID control.
Definition: ProfiledPIDSubsystem.h:75
frc2::ProfiledPIDSubsystem
A subsystem that uses a ProfiledPIDController to control an output.
Definition: ProfiledPIDSubsystem.h:23
frc2::ProfiledPIDSubsystem::Periodic
void Periodic() override
This method is called periodically by the CommandScheduler.
Definition: ProfiledPIDSubsystem.h:43
frc2::ProfiledPIDSubsystem::GetMeasurement
virtual Distance_t GetMeasurement()=0
Returns the measurement of the process variable used by the ProfiledPIDController.
frc2::ProfiledPIDSubsystem::ProfiledPIDSubsystem
ProfiledPIDSubsystem(frc::ProfiledPIDController< Distance > controller, Distance_t initialPosition=Distance_t{0})
Creates a new ProfiledPIDSubsystem.
Definition: ProfiledPIDSubsystem.h:37
frc2::ProfiledPIDSubsystem::Enable
virtual void Enable()
Enables the PID control.
Definition: ProfiledPIDSubsystem.h:67
frc2::ProfiledPIDSubsystem::UseOutput
virtual void UseOutput(double output, State setpoint)=0
Uses the output from the ProfiledPIDController.
frc::ProfiledPIDController
Implements a PID control loop whose setpoint is constrained by a trapezoid profile.
Definition: ProfiledPIDController.h:34
frc2::ProfiledPIDSubsystem::IsEnabled
bool IsEnabled()
Returns whether the controller is enabled.
Definition: ProfiledPIDSubsystem.h:85
frc2::ProfiledPIDSubsystem::SetGoal
void SetGoal(Distance_t goal)
Sets the goal state for the subsystem.
Definition: ProfiledPIDSubsystem.h:62
frc::TrapezoidProfile::State
Definition: TrapezoidProfile.h:68
frc2::ProfiledPIDSubsystem::GetController
frc::ProfiledPIDController< Distance > & GetController()
Returns the ProfiledPIDController.
Definition: ProfiledPIDSubsystem.h:92
frc2::SubsystemBase
A base for subsystems that handles registration in the constructor, and provides a more intuitive met...
Definition: SubsystemBase.h:22
frc2::ProfiledPIDSubsystem::SetGoal
void SetGoal(State goal)
Sets the goal state for the subsystem.
Definition: ProfiledPIDSubsystem.h:55