Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)

Browse Source
This commit is contained in:
Oblarg
2019-11-20 23:11:46 -05:00
committed by Peter Johnson
parent f62e23f1af
commit fa85fbfc1c
16 changed files with 320 additions and 448 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
View File

@@ -23,11 +23,16 @@ namespace frc2 {
* class. The controller calculation and output are performed synchronously in
* the command's execute() method.
*
* @see ProfiledPIDController
* @see ProfiledPIDController<Distance>
*/
template <class Distance>
class ProfiledPIDCommand
: public CommandHelper<CommandBase, ProfiledPIDCommand> {
using State = frc::TrapezoidProfile::State;
: public CommandHelper<CommandBase, ProfiledPIDCommand<Distance>> {
using Distance_t = units::unit_t<Distance>;
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
using Velocity_t = units::unit_t<Velocity>;
using State = frc::TrapezoidProfile<Distance>::State;
public:
/**
@@ -40,11 +45,17 @@ class ProfiledPIDCommand
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
ProfiledPIDCommand(frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource,
ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
std::function<units::unit_t<Distance>> measurementSource,
std::function<State()> goalSource,
std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements = {});
std::initializer_list<Subsystem*> requirements = {})
: m_controller{controller},
m_measurement{std::move(measurementSource)},
m_goal{std::move(goalSource)},
m_useOutput{std::move(useOutput)} {
AddRequirements(requirements);
}
/**
* Creates a new PIDCommand, which controls the given output with a
@@ -56,11 +67,16 @@ class ProfiledPIDCommand
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
ProfiledPIDCommand(frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource,
std::function<units::meter_t()> goalSource,
ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
std::function<units::unit_t<Distance>> measurementSource,
std::function<units::unit_t<Distance>> goalSource,
std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements);
std::initializer_list<Subsystem*> requirements)
: ProfiledPIDCommand(controller, measurementSource,
[&goalSource]() {
return State{goalSource(), 0_mps};
},
useOutput, requirements) {}
/**
* Creates a new PIDCommand, which controls the given output with a
@@ -72,10 +88,12 @@ class ProfiledPIDCommand
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
ProfiledPIDCommand(frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource,
ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
std::function<units::unit_t<Distance>> measurementSource,
State goal, std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements);
std::initializer_list<Subsystem*> requirements)
: ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
useOutput, requirements) {}
/**
* Creates a new PIDCommand, which controls the given output with a
@@ -87,32 +105,41 @@ class ProfiledPIDCommand
* @param useOutput the controller's output
* @param requirements the subsystems required by this command
*/
ProfiledPIDCommand(frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource,
ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
std::function<units::unit_t<Distance>> measurementSource,
units::meter_t goal,
std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements);
std::initializer_list<Subsystem*> requirements)
: ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
useOutput, requirements) {}
ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
void Initialize() override;
void Initialize() override { m_controller.Reset(); }
void Execute() override;
void Execute() override {
m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
m_controller.GetSetpoint());
}
void End(bool interrupted) override;
void End(bool interrupted) override {
m_useOutput(0, State{Distance_t(0), Velocity_t(0)});
}
/**
* Returns the ProfiledPIDController used by the command.
*
* @return The ProfiledPIDController
*/
frc::ProfiledPIDController& GetController();
frc::ProfiledPIDController<Distance>& GetController() {
return m_controller;
}
protected:
frc::ProfiledPIDController m_controller;
std::function<units::meter_t()> m_measurement;
frc::ProfiledPIDController<Distance> m_controller;
std::function<units::unit_t<Distance>> m_measurement;
std::function<State()> m_goal;
std::function<void(double, State)> m_useOutput;
};

33
wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
View File

@@ -19,8 +19,13 @@ namespace frc2 {
*
* @see ProfiledPIDController
*/
template <class Distance>
class ProfiledPIDSubsystem : public SubsystemBase {
using State = frc::TrapezoidProfile::State;
using Distance_t = units::unit_t<Distance>;
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
using Velocity_t = units::unit_t<Velocity>;
using State = frc::TrapezoidProfile<Distance>::State;
public:
/**
@@ -28,9 +33,15 @@ class ProfiledPIDSubsystem : public SubsystemBase {
*
* @param controller the ProfiledPIDController to use
*/
explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller);
explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller)
: m_controller{controller} {}
void Periodic() override;
void Periodic() override {
if (m_enabled) {
UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
m_controller.GetSetpoint());
}
}
/**
* Uses the output from the ProfiledPIDController.
@@ -59,22 +70,30 @@ class ProfiledPIDSubsystem : public SubsystemBase {
/**
* Enables the PID control. Resets the controller.
*/
virtual void Enable();
virtual void Enable() {
m_controller.Reset();
m_enabled = true;
}
/**
* Disables the PID control. Sets output to zero.
*/
virtual void Disable();
virtual void Disable() {
UseOutput(0, State{Distance_t(0), Velocity_t(0)});
m_enabled = false;
}
/**
* Returns the ProfiledPIDController.
*
* @return The controller.
*/
frc::ProfiledPIDController& GetController();
frc::ProfiledPIDController<Distance>& GetController() {
return m_controller;
}
protected:
frc::ProfiledPIDController m_controller;
frc::ProfiledPIDController<Distance> m_controller;
bool m_enabled;
};
} // namespace frc2

36
wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
View File

@@ -23,8 +23,14 @@ namespace frc2 {
*
* @see TrapezoidProfile
*/
template <class Distance>
class TrapezoidProfileCommand
: public CommandHelper<CommandBase, TrapezoidProfileCommand> {
: public CommandHelper<CommandBase, TrapezoidProfileCommand<Distance>> {
using Distance_t = units::unit_t<Distance>;
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
using Velocity_t = units::unit_t<Velocity>;
using State = frc::TrapezoidProfile<Distance>::State;
public:
/**
* Creates a new TrapezoidProfileCommand that will execute the given
@@ -34,21 +40,31 @@ class TrapezoidProfileCommand
* @param output The consumer for the profile output.
*/
TrapezoidProfileCommand(
frc::TrapezoidProfile profile,
std::function<void(frc::TrapezoidProfile::State)> output,
std::initializer_list<Subsystem*> requirements);
frc::TrapezoidProfile<Distance> profile,
std::function<void(State)> output,
std::initializer_list<Subsystem*> requirements)
: m_profile(profile), m_output(output) {
AddRequirements(requirements);
}
void Initialize() override;
void Initialize() override {
m_timer.Reset();
m_timer.Start();
}
void Execute() override;
void Execute() override {
m_output(m_profile.Calculate(m_timer.Get()));
}
void End(bool interrupted) override;
void End(bool interrupted) override { m_timer.Stop(); }
bool IsFinished() override;
bool IsFinished() override {
return m_timer.HasPeriodPassed(m_profile.TotalTime());
}
private:
frc::TrapezoidProfile m_profile;
std::function<void(frc::TrapezoidProfile::State)> m_output;
frc::TrapezoidProfile<Distance> m_profile;
std::function<void(State)> m_output;
Timer m_timer;
};

16
wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
View File

@@ -17,10 +17,14 @@ namespace frc2 {
* A subsystem that generates and runs trapezoidal motion profiles automatically. The user
* specifies how to use the current state of the motion profile by overriding the `UseState` method.
*/
template <class Unit>
template <class Distance>
class TrapezoidProfileSubsystem : public SubsystemBase {
using State = frc::TrapezoidProfile::State;
using Constraints = frc::TrapezoidProfile::Constraints;
using Distance_t = units::unit_t<Distance>;
using Velocity =
units::compound_unit<Distance, units::inverse<units::seconds>>;
using Velocity_t = units::unit_t<Velocity>;
using State = frc::TrapezoidProfile<Distance>::State;
using Constraints = frc::TrapezoidProfile<Distance>::Constraints;
public:
/**
* Creates a new TrapezoidProfileSubsystem.
@@ -31,14 +35,14 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
* @param period The period of the main robot loop, in seconds.
*/
TrapezoidProfileSubsystem(Constraints constraints,
units::unit_t<Unit> position,
Distance_t position,
units::second_t period = 20_ms)
: m_constraints(constraints),
m_state{units::meter_t(position.template to<double>()), 0_mps},
m_state{position, Velocity_t(0)},
m_period(period) {}
void Periodic() override {
auto profile = frc::TrapezoidProfile(m_constraints, GetGoal(), m_state);
auto profile = frc::TrapezoidProfile<Distance>(m_constraints, GetGoal(), m_state);
m_state = profile.Calculate(m_period);
UseState(m_state);
}