Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11: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

67
wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
View File

@@ -1,67 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc2/command/ProfiledPIDCommand.h"
using namespace frc2;
using State = frc::TrapezoidProfile::State;
ProfiledPIDCommand::ProfiledPIDCommand(
frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource,
std::function<State()> goalSource,
std::function<void(double, State)> useOutput,
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);
}
ProfiledPIDCommand::ProfiledPIDCommand(
frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource,
std::function<units::meter_t()> goalSource,
std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements)
: ProfiledPIDCommand(controller, measurementSource,
[&goalSource]() {
return State{goalSource(), 0_mps};
},
useOutput, requirements) {}
ProfiledPIDCommand::ProfiledPIDCommand(
frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource, State goal,
std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements)
: ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
useOutput, requirements) {}
ProfiledPIDCommand::ProfiledPIDCommand(
frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource, units::meter_t goal,
std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements)
: ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
useOutput, requirements) {}
void ProfiledPIDCommand::Initialize() { m_controller.Reset(); }
void ProfiledPIDCommand::Execute() {
m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
m_controller.GetSetpoint());
}
void ProfiledPIDCommand::End(bool interrupted) {
m_useOutput(0, State{0_m, 0_mps});
}
frc::ProfiledPIDController& ProfiledPIDCommand::GetController() {
return m_controller;
}

36
wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp
View File

@@ -1,36 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc2/command/ProfiledPIDSubsystem.h"
using namespace frc2;
using State = frc::TrapezoidProfile::State;
ProfiledPIDSubsystem::ProfiledPIDSubsystem(
frc::ProfiledPIDController controller)
: m_controller{controller} {}
void ProfiledPIDSubsystem::Periodic() {
if (m_enabled) {
UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
m_controller.GetSetpoint());
}
}
void ProfiledPIDSubsystem::Enable() {
m_controller.Reset();
m_enabled = true;
}
void ProfiledPIDSubsystem::Disable() {
UseOutput(0, State{0_m, 0_mps});
m_enabled = false;
}
frc::ProfiledPIDController& ProfiledPIDSubsystem::GetController() {
return m_controller;
}

33
wpilibNewCommands/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
View File

@@ -1,33 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc2/command/TrapezoidProfileCommand.h"
using namespace frc2;
TrapezoidProfileCommand::TrapezoidProfileCommand(
frc::TrapezoidProfile profile,
std::function<void(frc::TrapezoidProfile::State)> output,
std::initializer_list<Subsystem*> requirements)
: m_profile(profile), m_output(output) {
AddRequirements(requirements);
}
void TrapezoidProfileCommand::Initialize() {
m_timer.Reset();
m_timer.Start();
}
void TrapezoidProfileCommand::Execute() {
m_output(m_profile.Calculate(m_timer.Get()));
}
void TrapezoidProfileCommand::End(bool interrupted) { m_timer.Stop(); }
bool TrapezoidProfileCommand::IsFinished() {
return m_timer.HasPeriodPassed(m_profile.TotalTime());
}

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);
}

133
wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
View File

@@ -1,133 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/controller/ProfiledPIDController.h"
#include <algorithm>
#include <cmath>
#include "frc/smartdashboard/SendableBuilder.h"
using namespace frc;
ProfiledPIDController::ProfiledPIDController(
double Kp, double Ki, double Kd,
frc::TrapezoidProfile::Constraints constraints, units::second_t period)
: m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
void ProfiledPIDController::SetP(double Kp) { m_controller.SetP(Kp); }
void ProfiledPIDController::SetI(double Ki) { m_controller.SetI(Ki); }
void ProfiledPIDController::SetD(double Kd) { m_controller.SetD(Kd); }
double ProfiledPIDController::GetP() const { return m_controller.GetP(); }
double ProfiledPIDController::GetI() const { return m_controller.GetI(); }
double ProfiledPIDController::GetD() const { return m_controller.GetD(); }
units::second_t ProfiledPIDController::GetPeriod() const {
return m_controller.GetPeriod();
}
void ProfiledPIDController::SetGoal(TrapezoidProfile::State goal) {
m_goal = goal;
}
void ProfiledPIDController::SetGoal(units::meter_t goal) {
m_goal = {goal, 0_mps};
}
TrapezoidProfile::State ProfiledPIDController::GetGoal() const {
return m_goal;
}
bool ProfiledPIDController::AtGoal() const {
return AtSetpoint() && m_goal == m_setpoint;
}
void ProfiledPIDController::SetConstraints(
frc::TrapezoidProfile::Constraints constraints) {
m_constraints = constraints;
}
TrapezoidProfile::State ProfiledPIDController::GetSetpoint() const {
return m_setpoint;
}
bool ProfiledPIDController::AtSetpoint() const {
return m_controller.AtSetpoint();
}
void ProfiledPIDController::EnableContinuousInput(double minimumInput,
double maximumInput) {
m_controller.EnableContinuousInput(minimumInput, maximumInput);
}
void ProfiledPIDController::DisableContinuousInput() {
m_controller.DisableContinuousInput();
}
void ProfiledPIDController::SetIntegratorRange(double minimumIntegral,
double maximumIntegral) {
m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
}
void ProfiledPIDController::SetTolerance(double positionTolerance,
double velocityTolerance) {
m_controller.SetTolerance(positionTolerance, velocityTolerance);
}
double ProfiledPIDController::GetPositionError() const {
return m_controller.GetPositionError();
}
double ProfiledPIDController::GetVelocityError() const {
return m_controller.GetVelocityError();
}
double ProfiledPIDController::Calculate(units::meter_t measurement) {
frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
m_setpoint = profile.Calculate(GetPeriod());
return m_controller.Calculate(measurement.to<double>(),
m_setpoint.position.to<double>());
}
double ProfiledPIDController::Calculate(units::meter_t measurement,
TrapezoidProfile::State goal) {
SetGoal(goal);
return Calculate(measurement);
}
double ProfiledPIDController::Calculate(units::meter_t measurement,
units::meter_t goal) {
SetGoal(goal);
return Calculate(measurement);
}
double ProfiledPIDController::Calculate(
units::meter_t measurement, units::meter_t goal,
frc::TrapezoidProfile::Constraints constraints) {
SetConstraints(constraints);
return Calculate(measurement, goal);
}
void ProfiledPIDController::Reset() { m_controller.Reset(); }
void ProfiledPIDController::InitSendable(frc::SendableBuilder& builder) {
builder.SetSmartDashboardType("ProfiledPIDController");
builder.AddDoubleProperty("p", [this] { return GetP(); },
[this](double value) { SetP(value); });
builder.AddDoubleProperty("i", [this] { return GetI(); },
[this](double value) { SetI(value); });
builder.AddDoubleProperty("d", [this] { return GetD(); },
[this](double value) { SetD(value); });
builder.AddDoubleProperty(
"goal", [this] { return GetGoal().position.to<double>(); },
[this](double value) { SetGoal(units::meter_t{value}); });
}

125
wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
View File

@@ -7,6 +7,8 @@
#pragma once
#include <algorithm>
#include <cmath>
#include <functional>
#include <limits>
@@ -14,6 +16,7 @@
#include "frc/controller/PIDController.h"
#include "frc/smartdashboard/Sendable.h"
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableHelper.h"
#include "frc/trajectory/TrapezoidProfile.h"
@@ -23,8 +26,20 @@ namespace frc {
* Implements a PID control loop whose setpoint is constrained by a trapezoid
* profile.
*/
class ProfiledPIDController : public Sendable,
public SendableHelper<ProfiledPIDController> {
template <class Distance>
class ProfiledPIDController
: public Sendable,
public SendableHelper<ProfiledPIDController<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 Acceleration =
units::compound_unit<Velocity, units::inverse<units::seconds>>;
using Acceleration_t = units::unit_t<Acceleration>;
using State = typename TrapezoidProfile<Distance>::State;
using Constraints = typename TrapezoidProfile<Distance>::Constraints;
public:
/**
* Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
@@ -38,8 +53,8 @@ class ProfiledPIDController : public Sendable,
* default is 20 milliseconds.
*/
ProfiledPIDController(double Kp, double Ki, double Kd,
frc::TrapezoidProfile::Constraints constraints,
units::second_t period = 20_ms);
Constraints constraints, units::second_t period = 20_ms)
: m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
~ProfiledPIDController() override = default;
@@ -57,96 +72,98 @@ class ProfiledPIDController : public Sendable,
* @param Ki Integral coefficient
* @param Kd Differential coefficient
*/
void SetPID(double Kp, double Ki, double Kd);
void SetPID(double Kp, double Ki, double Kd) {
m_controller.SetPID(Kp, Ki, Kd);
}
/**
* Sets the proportional coefficient of the PID controller gain.
*
* @param Kp proportional coefficient
*/
void SetP(double Kp);
void SetP(double Kp) { m_controller.SetP(Kp); }
/**
* Sets the integral coefficient of the PID controller gain.
*
* @param Ki integral coefficient
*/
void SetI(double Ki);
void SetI(double Ki) { m_controller.SetI(Ki); }
/**
* Sets the differential coefficient of the PID controller gain.
*
* @param Kd differential coefficient
*/
void SetD(double Kd);
void SetD(double Kd) { m_controller.SetD(Kd); }
/**
* Gets the proportional coefficient.
*
* @return proportional coefficient
*/
double GetP() const;
double GetP() const { return m_controller.GetP(); }
/**
* Gets the integral coefficient.
*
* @return integral coefficient
*/
double GetI() const;
double GetI() const { return m_controller.GetI(); }
/**
* Gets the differential coefficient.
*
* @return differential coefficient
*/
double GetD() const;
double GetD() const { return m_controller.GetD(); }
/**
* Gets the period of this controller.
*
* @return The period of the controller.
*/
units::second_t GetPeriod() const;
units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
/**
* Sets the goal for the ProfiledPIDController.
*
* @param goal The desired unprofiled setpoint.
*/
void SetGoal(TrapezoidProfile::State goal);
void SetGoal(State goal) { m_goal = goal; }
/**
* Sets the goal for the ProfiledPIDController.
*
* @param goal The desired unprofiled setpoint.
*/
void SetGoal(units::meter_t goal);
void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
/**
* Gets the goal for the ProfiledPIDController.
*/
TrapezoidProfile::State GetGoal() const;
State GetGoal() const { return m_goal; }
/**
* Returns true if the error is within the tolerance of the error.
*
* This will return false until at least one input value has been computed.
*/
bool AtGoal() const;
bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
/**
* Set velocity and acceleration constraints for goal.
*
* @param constraints Velocity and acceleration constraints for goal.
*/
void SetConstraints(frc::TrapezoidProfile::Constraints constraints);
void SetConstraints(Constraints constraints) { m_constraints = constraints; }
/**
* Returns the current setpoint of the ProfiledPIDController.
*
* @return The current setpoint.
*/
TrapezoidProfile::State GetSetpoint() const;
State GetSetpoint() const { return m_setpoint; }
/**
* Returns true if the error is within the tolerance of the error.
@@ -157,7 +174,7 @@ class ProfiledPIDController : public Sendable,
*
* This will return false until at least one input value has been computed.
*/
bool AtSetpoint() const;
bool AtSetpoint() const { return m_controller.AtSetpoint(); }
/**
* Enables continuous input.
@@ -169,12 +186,15 @@ class ProfiledPIDController : public Sendable,
* @param minimumInput The minimum value expected from the input.
* @param maximumInput The maximum value expected from the input.
*/
void EnableContinuousInput(double minimumInput, double maximumInput);
void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
m_controller.EnableContinuousInput(minimumInput.template to<double>(),
maximumInput.template to<double>());
}
/**
* Disables continuous input.
*/
void DisableContinuousInput();
void DisableContinuousInput() { m_controller.DisableContinuousInput(); }
/**
* Sets the minimum and maximum values for the integrator.
@@ -185,7 +205,9 @@ class ProfiledPIDController : public Sendable,
* @param minimumIntegral The minimum value of the integrator.
* @param maximumIntegral The maximum value of the integrator.
*/
void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
}
/**
* Sets the error which is considered tolerable for use with
@@ -196,26 +218,37 @@ class ProfiledPIDController : public Sendable,
*/
void SetTolerance(
double positionTolerance,
double velocityTolerance = std::numeric_limits<double>::infinity());
double velocityTolerance = std::numeric_limits<double>::infinity()) {
m_controller.SetTolerance(positionTolerance, velocityTolerance);
}
/**
* Returns the difference between the setpoint and the measurement.
*
* @return The error.
*/
double GetPositionError() const;
Distance_t GetPositionError() const {
return Distance_t(m_controller.GetPositionError());
}
/**
* Returns the change in error per second.
*/
double GetVelocityError() const;
Velocity_t GetVelocityError() const {
return Velocity_t(m_controller.GetVelocityError());
}
/**
* Returns the next output of the PID controller.
*
* @param measurement The current measurement of the process variable.
*/
double Calculate(units::meter_t measurement);
double Calculate(Distance_t measurement) {
frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
m_setpoint = profile.Calculate(GetPeriod());
return m_controller.Calculate(measurement.template to<double>(),
m_setpoint.position.template to<double>());
}
/**
* Returns the next output of the PID controller.
@@ -223,15 +256,20 @@ class ProfiledPIDController : public Sendable,
* @param measurement The current measurement of the process variable.
* @param goal The new goal of the controller.
*/
double Calculate(units::meter_t measurement, TrapezoidProfile::State goal);
double Calculate(Distance_t measurement, State goal) {
SetGoal(goal);
return Calculate(measurement);
}
/**
* Returns the next output of the PID controller.
*
* @param measurement The current measurement of the process variable.
* @param goal The new goal of the controller.
*/
double Calculate(units::meter_t measurement, units::meter_t goal);
double Calculate(Distance_t measurement, Distance_t goal) {
SetGoal(goal);
return Calculate(measurement);
}
/**
* Returns the next output of the PID controller.
@@ -240,21 +278,36 @@ class ProfiledPIDController : public Sendable,
* @param goal The new goal of the controller.
* @param constraints Velocity and acceleration constraints for goal.
*/
double Calculate(units::meter_t measurement, units::meter_t goal,
frc::TrapezoidProfile::Constraints constraints);
double Calculate(
Distance_t measurement, Distance_t goal,
typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
SetConstraints(constraints);
return Calculate(measurement, goal);
}
/**
* Reset the previous error, the integral term, and disable the controller.
*/
void Reset();
void Reset() { m_controller.Reset(); }
void InitSendable(frc::SendableBuilder& builder) override;
void InitSendable(frc::SendableBuilder& builder) override {
builder.SetSmartDashboardType("ProfiledPIDController");
builder.AddDoubleProperty("p", [this] { return GetP(); },
[this](double value) { SetP(value); });
builder.AddDoubleProperty("i", [this] { return GetI(); },
[this](double value) { SetI(value); });
builder.AddDoubleProperty("d", [this] { return GetD(); },
[this](double value) { SetD(value); });
builder.AddDoubleProperty(
"goal", [this] { return GetGoal().position.template to<double>(); },
[this](double value) { SetGoal(Distance_t{value}); });
}
private:
frc2::PIDController m_controller;
frc::TrapezoidProfile::State m_goal;
frc::TrapezoidProfile::State m_setpoint;
frc::TrapezoidProfile::Constraints m_constraints;
typename frc::TrapezoidProfile<Distance>::State m_goal;
typename frc::TrapezoidProfile<Distance>::State m_setpoint;
typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
};
} // namespace frc

24
wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
View File

@@ -40,18 +40,27 @@ namespace frc {
* `Calculate()` and to determine when the profile has completed via
* `IsFinished()`.
*/
template <class Distance>
class TrapezoidProfile {
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 Acceleration =
units::compound_unit<Velocity, units::inverse<units::seconds>>;
using Acceleration_t = units::unit_t<Acceleration>;
public:
class Constraints {
public:
units::meters_per_second_t maxVelocity = 0_mps;
units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
Velocity_t maxVelocity{0};
Acceleration_t maxAcceleration{0};
};
class State {
public:
units::meter_t position = 0_m;
units::meters_per_second_t velocity = 0_mps;
Distance_t position{0};
Velocity_t velocity{0};
bool operator==(const State& rhs) const {
return position == rhs.position && velocity == rhs.velocity;
}
@@ -66,7 +75,7 @@ class TrapezoidProfile {
* @param initial The initial state (usually the current state).
*/
TrapezoidProfile(Constraints constraints, State goal,
State initial = State{0_m, 0_mps});
State initial = State{Distance_t(0), Velocity_t(0)});
TrapezoidProfile(const TrapezoidProfile&) = default;
TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
@@ -86,7 +95,7 @@ class TrapezoidProfile {
*
* @param target The target distance.
*/
units::second_t TimeLeftUntil(units::meter_t target) const;
units::second_t TimeLeftUntil(Distance_t target) const;
/**
* Returns the total time the profile takes to reach the goal.
@@ -135,5 +144,6 @@ class TrapezoidProfile {
units::second_t m_endFullSpeed;
units::second_t m_endDeccel;
};
} // namespace frc
#include "TrapezoidProfile.inc"

61
wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp → wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
View File

@@ -5,12 +5,14 @@
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/trajectory/TrapezoidProfile.h"
#pragma once
using namespace frc;
#include <algorithm>
TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
State initial)
namespace frc {
template <class Distance>
TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
State goal, State initial)
: m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
m_constraints(constraints),
m_initial(Direct(initial)),
@@ -24,30 +26,30 @@ TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
// ended at zero velocity
units::second_t cutoffBegin =
m_initial.velocity / m_constraints.maxAcceleration;
units::meter_t cutoffDistBegin =
Distance_t cutoffDistBegin =
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
units::meter_t cutoffDistEnd =
Distance_t cutoffDistEnd =
cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
// Now we can calculate the parameters as if it was a full trapezoid instead
// of a truncated one
units::meter_t fullTrapezoidDist =
Distance_t fullTrapezoidDist =
cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
units::second_t accelerationTime =
m_constraints.maxVelocity / m_constraints.maxAcceleration;
units::meter_t fullSpeedDist =
Distance_t fullSpeedDist =
fullTrapezoidDist -
accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
if (fullSpeedDist < 0_m) {
if (fullSpeedDist < Distance_t(0)) {
accelerationTime =
units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
fullSpeedDist = 0_m;
fullSpeedDist = Distance_t(0);
}
m_endAccel = accelerationTime - cutoffBegin;
@@ -55,7 +57,9 @@ TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
}
TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const {
template <class Distance>
typename TrapezoidProfile<Distance>::State
TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
State result = m_initial;
if (t < m_endAccel) {
@@ -83,9 +87,11 @@ TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const {
return Direct(result);
}
units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
units::meter_t position = m_initial.position * m_direction;
units::meters_per_second_t velocity = m_initial.velocity * m_direction;
template <class Distance>
units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
Distance_t target) const {
Distance_t position = m_initial.position * m_direction;
Velocity_t velocity = m_initial.velocity * m_direction;
units::second_t endAccel = m_endAccel * m_direction;
units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
@@ -101,21 +107,19 @@ units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
endDeccel = units::math::max(endDeccel, 0_s);
const units::meters_per_second_squared_t acceleration =
m_constraints.maxAcceleration;
const units::meters_per_second_squared_t decceleration =
-m_constraints.maxAcceleration;
const Acceleration_t acceleration = m_constraints.maxAcceleration;
const Acceleration_t decceleration = -m_constraints.maxAcceleration;
units::meter_t distToTarget = units::math::abs(target - position);
Distance_t distToTarget = units::math::abs(target - position);
if (distToTarget < 1e-6_m) {
if (distToTarget < Distance_t(1e-6)) {
return 0_s;
}
units::meter_t accelDist =
Distance_t accelDist =
velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
units::meters_per_second_t deccelVelocity;
Velocity_t deccelVelocity;
if (endAccel > 0_s) {
deccelVelocity = units::math::sqrt(
units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
@@ -123,20 +127,20 @@ units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
deccelVelocity = velocity;
}
units::meter_t deccelDist =
Distance_t deccelDist =
deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
deccelDist = units::math::max(deccelDist, 0_m);
deccelDist = units::math::max(deccelDist, Distance_t(0));
units::meter_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
if (accelDist > distToTarget) {
accelDist = distToTarget;
fullSpeedDist = 0_m;
deccelDist = 0_m;
fullSpeedDist = Distance_t(0);
deccelDist = Distance_t(0);
} else if (accelDist + fullSpeedDist > distToTarget) {
fullSpeedDist = distToTarget - accelDist;
deccelDist = 0_m;
deccelDist = Distance_t(0);
} else {
deccelDist = distToTarget - fullSpeedDist - accelDist;
}
@@ -156,3 +160,4 @@ units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
return accelTime + fullSpeedTime + deccelTime;
}
} // namespace frc

94
wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
View File

@@ -25,12 +25,13 @@ static constexpr auto kDt = 10_ms;
}
TEST(TrapezoidProfileTest, ReachesGoal) {
frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State goal{3_m, 0_mps};
frc::TrapezoidProfile::State state;
frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 450; ++i) {
frc::TrapezoidProfile profile{constraints, goal, state};
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
@@ -39,10 +40,11 @@ TEST(TrapezoidProfileTest, ReachesGoal) {
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State goal{12_m, 0_mps};
frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
frc::TrapezoidProfile profile{constraints, goal};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto lastPos = state.position;
@@ -51,7 +53,7 @@ TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
constraints.maxVelocity = 0.75_mps;
}
profile = frc::TrapezoidProfile{constraints, goal, state};
profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
state = profile.Calculate(kDt);
auto estimatedVel = (state.position - lastPos) / kDt;
@@ -71,31 +73,33 @@ TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
// There is some somewhat tricky code for dealing with going backwards
TEST(TrapezoidProfileTest, Backwards) {
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State goal{-2_m, 0_mps};
frc::TrapezoidProfile::State state;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 400; ++i) {
frc::TrapezoidProfile profile{constraints, goal, state};
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
}
TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State goal{-2_m, 0_mps};
frc::TrapezoidProfile::State state;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 200; ++i) {
frc::TrapezoidProfile profile{constraints, goal, state};
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_NE(state, goal);
goal = {0.0_m, 0.0_mps};
for (int i = 0; i < 550; ++i) {
frc::TrapezoidProfile profile{constraints, goal, state};
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
@@ -103,30 +107,32 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
// Checks to make sure that it hits top speed
TEST(TrapezoidProfileTest, TopSpeed) {
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State goal{4_m, 0_mps};
frc::TrapezoidProfile::State state;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 200; ++i) {
frc::TrapezoidProfile profile{constraints, goal, state};
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
for (int i = 0; i < 2000; ++i) {
frc::TrapezoidProfile profile{constraints, goal, state};
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
}
EXPECT_EQ(state, goal);
}
TEST(TrapezoidProfileTest, TimingToCurrent) {
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State goal{2_m, 0_mps};
frc::TrapezoidProfile::State state;
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::State state;
for (int i = 0; i < 400; i++) {
frc::TrapezoidProfile profile{constraints, goal, state};
frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
state = profile.Calculate(kDt);
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
}
@@ -135,16 +141,17 @@ TEST(TrapezoidProfileTest, TimingToCurrent) {
TEST(TrapezoidProfileTest, TimingToGoal) {
using units::unit_cast;
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile profile{constraints, goal};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile(constraints, goal, state);
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal && state == goal) {
// Expected value using for loop index is just an approximation since the
@@ -158,16 +165,17 @@ TEST(TrapezoidProfileTest, TimingToGoal) {
TEST(TrapezoidProfileTest, TimingBeforeGoal) {
using units::unit_cast;
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State goal{2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
frc::TrapezoidProfile profile{constraints, goal};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile(constraints, goal, state);
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal &&
(units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
@@ -180,16 +188,17 @@ TEST(TrapezoidProfileTest, TimingBeforeGoal) {
TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
using units::unit_cast;
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile profile{constraints, goal};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile(constraints, goal, state);
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal && state == goal) {
// Expected value using for loop index is just an approximation since the
@@ -203,16 +212,17 @@ TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
using units::unit_cast;
frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State goal{-2_m, 0_mps};
frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
frc::TrapezoidProfile profile{constraints, goal};
frc::TrapezoidProfile<units::meter> profile{constraints, goal};
auto state = profile.Calculate(kDt);
auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
bool reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = frc::TrapezoidProfile(constraints, goal, state);
profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
state = profile.Calculate(kDt);
if (!reachedGoal &&
(units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {

6
wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
View File

@@ -41,8 +41,10 @@ class Robot : public frc::TimedRobot {
// Create a PID controller whose setpoint's change is subject to maximum
// velocity and acceleration constraints.
frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
frc::ProfiledPIDController m_controller{1.3, 0.0, 0.7, m_constraints, kDt};
frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
0.75_mps_sq};
frc::ProfiledPIDController<units::meters> m_controller{1.3, 0.0, 0.7,
m_constraints, kDt};
};
#ifndef RUNNING_FRC_TESTS

10
wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
View File

@@ -31,7 +31,8 @@ class Robot : public frc::TimedRobot {
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint, the desired goal, and the
// current setpoint.
frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
frc::TrapezoidProfile<units::meter> profile{m_constraints, m_goal,
m_setpoint};
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
@@ -49,9 +50,10 @@ class Robot : public frc::TimedRobot {
frc::PWMVictorSPX m_motor{1};
frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt};
frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq};
frc::TrapezoidProfile::State m_goal;
frc::TrapezoidProfile::State m_setpoint;
frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
0.75_mps_sq};
frc::TrapezoidProfile<units::meters>::State m_goal;
frc::TrapezoidProfile<units::meters>::State m_setpoint;
};
#ifndef RUNNING_FRC_TESTS

8
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
View File

@@ -26,7 +26,8 @@ SwerveModule::SwerveModule(const int driveMotorChannel,
// Limit the PID Controller's input range between -pi and pi and set the input
// to be continuous.
m_turningPIDController.EnableContinuousInput(-wpi::math::pi, wpi::math::pi);
m_turningPIDController.EnableContinuousInput(-units::radian_t(wpi::math::pi),
units::radian_t(wpi::math::pi));
}
frc::SwerveModuleState SwerveModule::GetState() const {
@@ -41,10 +42,7 @@ void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) {
// Calculate the turning motor output from the turning PID controller.
const auto turnOutput = m_turningPIDController.Calculate(
units::meter_t(m_turningEncoder.Get()),
// We have to convert to the meters type here because that's what
// ProfiledPIDController wants.
units::meter_t(state.angle.Radians().to<double>()));
units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
// Set the motor outputs.
m_driveMotor.Set(driveOutput);

15
wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
View File

@@ -24,15 +24,10 @@ class SwerveModule {
static constexpr double kWheelRadius = 0.0508;
static constexpr int kEncoderResolution = 4096;
// We have to use meters here instead of radians because of the fact that
// ProfiledPIDController's constraints only take in meters per second and
// meters per second squared.
static constexpr units::meters_per_second_t kModuleMaxAngularVelocity =
units::meters_per_second_t(wpi::math::pi); // radians per second
static constexpr units::meters_per_second_squared_t
kModuleMaxAngularAcceleration = units::meters_per_second_squared_t(
wpi::math::pi * 2.0); // radians per second squared
static constexpr auto kModuleMaxAngularVelocity =
wpi::math::pi * 1_rad_per_s; // radians per second
static constexpr auto kModuleMaxAngularAcceleration =
wpi::math::pi * 2_rad_per_s / 1_s; // radians per second^2
frc::PWMVictorSPX m_driveMotor;
frc::PWMVictorSPX m_turningMotor;
@@ -41,7 +36,7 @@ class SwerveModule {
frc::Encoder m_turningEncoder{2, 3};
frc2::PIDController m_drivePIDController{1.0, 0, 0};
frc::ProfiledPIDController m_turningPIDController{
frc::ProfiledPIDController<units::radians> m_turningPIDController{
1.0,
0.0,
0.0,