mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-29 02:21:44 +00:00
Add ProfiledPIDController
This commit is contained in:
Tyler Veness
committed by
Peter Johnson
Peter Johnson
parent
fc98a79dbb
commit
3ebc5a6d3a
146
wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
Normal file
146
wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
Normal file
@@ -0,0 +1,146 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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(units::meter_t goal) {
|
||||
m_goal = {goal, 0_mps};
|
||||
}
|
||||
|
||||
units::meter_t ProfiledPIDController::GetGoal() const {
|
||||
return m_goal.position;
|
||||
}
|
||||
|
||||
bool ProfiledPIDController::AtGoal(
|
||||
double positionTolerance, double velocityTolerance,
|
||||
frc2::PIDController::Tolerance toleranceType) const {
|
||||
return AtSetpoint(positionTolerance, velocityTolerance, toleranceType) &&
|
||||
m_goal == m_setpoint;
|
||||
}
|
||||
|
||||
bool ProfiledPIDController::AtGoal() const {
|
||||
return AtSetpoint() && m_goal == m_setpoint;
|
||||
}
|
||||
|
||||
void ProfiledPIDController::SetConstraints(
|
||||
frc::TrapezoidProfile::Constraints constraints) {
|
||||
m_constraints = constraints;
|
||||
}
|
||||
|
||||
double ProfiledPIDController::GetSetpoint() const {
|
||||
return m_controller.GetSetpoint();
|
||||
}
|
||||
|
||||
bool ProfiledPIDController::AtSetpoint(
|
||||
double positionTolerance, double velocityTolerance,
|
||||
frc2::PIDController::Tolerance toleranceType) const {
|
||||
return m_controller.AtSetpoint(positionTolerance, velocityTolerance,
|
||||
toleranceType);
|
||||
}
|
||||
|
||||
bool ProfiledPIDController::AtSetpoint() const {
|
||||
return m_controller.AtSetpoint();
|
||||
}
|
||||
|
||||
void ProfiledPIDController::SetInputRange(double minimumInput,
|
||||
double maximumInput) {
|
||||
m_controller.SetInputRange(minimumInput, maximumInput);
|
||||
}
|
||||
|
||||
void ProfiledPIDController::EnableContinuousInput(double minimumInput,
|
||||
double maximumInput) {
|
||||
m_controller.EnableContinuousInput(minimumInput, maximumInput);
|
||||
}
|
||||
|
||||
void ProfiledPIDController::DisableContinuousInput() {
|
||||
m_controller.DisableContinuousInput();
|
||||
}
|
||||
|
||||
void ProfiledPIDController::SetOutputRange(double minimumOutput,
|
||||
double maximumOutput) {
|
||||
m_controller.SetOutputRange(minimumOutput, maximumOutput);
|
||||
}
|
||||
|
||||
void ProfiledPIDController::SetAbsoluteTolerance(double positionTolerance,
|
||||
double velocityTolerance) {
|
||||
m_controller.SetAbsoluteTolerance(positionTolerance, velocityTolerance);
|
||||
}
|
||||
|
||||
void ProfiledPIDController::SetPercentTolerance(double positionTolerance,
|
||||
double velocityTolerance) {
|
||||
m_controller.SetPercentTolerance(positionTolerance, velocityTolerance);
|
||||
}
|
||||
|
||||
double ProfiledPIDController::GetPositionError() const {
|
||||
return m_controller.GetPositionError();
|
||||
}
|
||||
|
||||
double ProfiledPIDController::GetVelocityError() const {
|
||||
return m_controller.GetVelocityError();
|
||||
}
|
||||
|
||||
double ProfiledPIDController::Calculate(double measurement) {
|
||||
frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
|
||||
m_setpoint = profile.Calculate(GetPeriod());
|
||||
return m_controller.Calculate(measurement, m_setpoint.position.to<double>());
|
||||
}
|
||||
|
||||
double ProfiledPIDController::Calculate(double measurement,
|
||||
units::meter_t goal) {
|
||||
SetGoal(goal);
|
||||
return Calculate(measurement);
|
||||
}
|
||||
|
||||
double ProfiledPIDController::Calculate(
|
||||
double 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().to<double>(); },
|
||||
[this](double value) { SetGoal(units::meter_t{value}); });
|
||||
}
|
||||
@@ -0,0 +1,289 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <limits>
|
||||
|
||||
#include <units/units.h>
|
||||
|
||||
#include "frc/controller/PIDController.h"
|
||||
#include "frc/smartdashboard/SendableBase.h"
|
||||
#include "frc/trajectory/TrapezoidProfile.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Implements a PID control loop whose setpoint is constrained by a trapezoid
|
||||
* profile.
|
||||
*/
|
||||
class ProfiledPIDController : public SendableBase {
|
||||
public:
|
||||
/**
|
||||
* Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
|
||||
* Kd.
|
||||
*
|
||||
* @param Kp The proportional coefficient.
|
||||
* @param Ki The integral coefficient.
|
||||
* @param Kd The derivative coefficient.
|
||||
* @param constraints Velocity and acceleration constraints for goal.
|
||||
* @param period The period between controller updates in seconds. The
|
||||
* default is 20 milliseconds.
|
||||
*/
|
||||
ProfiledPIDController(double Kp, double Ki, double Kd,
|
||||
frc::TrapezoidProfile::Constraints constraints,
|
||||
units::second_t period = 20_ms);
|
||||
|
||||
~ProfiledPIDController() override = default;
|
||||
|
||||
ProfiledPIDController(const ProfiledPIDController&) = default;
|
||||
ProfiledPIDController& operator=(const ProfiledPIDController&) = default;
|
||||
ProfiledPIDController(ProfiledPIDController&&) = default;
|
||||
ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
|
||||
|
||||
/**
|
||||
* Sets the PID Controller gain parameters.
|
||||
*
|
||||
* Sets the proportional, integral, and differential coefficients.
|
||||
*
|
||||
* @param Kp Proportional coefficient
|
||||
* @param Ki Integral coefficient
|
||||
* @param Kd Differential coefficient
|
||||
*/
|
||||
void SetPID(double Kp, double Ki, double Kd);
|
||||
|
||||
/**
|
||||
* Sets the proportional coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Kp proportional coefficient
|
||||
*/
|
||||
void SetP(double Kp);
|
||||
|
||||
/**
|
||||
* Sets the integral coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Ki integral coefficient
|
||||
*/
|
||||
void SetI(double Ki);
|
||||
|
||||
/**
|
||||
* Sets the differential coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Kd differential coefficient
|
||||
*/
|
||||
void SetD(double Kd);
|
||||
|
||||
/**
|
||||
* Gets the proportional coefficient.
|
||||
*
|
||||
* @return proportional coefficient
|
||||
*/
|
||||
double GetP() const;
|
||||
|
||||
/**
|
||||
* Gets the integral coefficient.
|
||||
*
|
||||
* @return integral coefficient
|
||||
*/
|
||||
double GetI() const;
|
||||
|
||||
/**
|
||||
* Gets the differential coefficient.
|
||||
*
|
||||
* @return differential coefficient
|
||||
*/
|
||||
double GetD() const;
|
||||
|
||||
/**
|
||||
* Gets the period of this controller.
|
||||
*
|
||||
* @return The period of the controller.
|
||||
*/
|
||||
units::second_t GetPeriod() const;
|
||||
|
||||
/**
|
||||
* Sets the goal for the ProfiledPIDController.
|
||||
*
|
||||
* @param goal The desired unprofiled setpoint.
|
||||
*/
|
||||
void SetGoal(units::meter_t goal);
|
||||
|
||||
/**
|
||||
* Gets the goal for the ProfiledPIDController.
|
||||
*/
|
||||
units::meter_t GetGoal() const;
|
||||
|
||||
/**
|
||||
* Returns true if the error is within tolerance of the setpoint.
|
||||
*
|
||||
* This will return false until at least one input value has been computed.
|
||||
*
|
||||
* @param positionTolerance The maximum allowable position error.
|
||||
* @param velocityTolerance The maximum allowable velocity error.
|
||||
* @param toleranceType The type of tolerance specified.
|
||||
*/
|
||||
bool AtGoal(
|
||||
double positionTolerance,
|
||||
double velocityTolerance = std::numeric_limits<double>::infinity(),
|
||||
frc2::PIDController::Tolerance toleranceType =
|
||||
frc2::PIDController::Tolerance::kAbsolute) const;
|
||||
|
||||
/**
|
||||
* 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;
|
||||
|
||||
/**
|
||||
* Set velocity and acceleration constraints for goal.
|
||||
*
|
||||
* @param constraints Velocity and acceleration constraints for goal.
|
||||
*/
|
||||
void SetConstraints(frc::TrapezoidProfile::Constraints constraints);
|
||||
|
||||
/**
|
||||
* Returns the current setpoint of the ProfiledPIDController.
|
||||
*
|
||||
* @return The current setpoint.
|
||||
*/
|
||||
double GetSetpoint() const;
|
||||
|
||||
/**
|
||||
* Returns true if the error is within tolerance of the setpoint.
|
||||
*
|
||||
* This will return false until at least one input value has been computed.
|
||||
*
|
||||
* @param positionTolerance The maximum allowable position error.
|
||||
* @param velocityTolerance The maximum allowable velocity error.
|
||||
* @param toleranceType The type of tolerance specified.
|
||||
*/
|
||||
bool AtSetpoint(
|
||||
double positionTolerance,
|
||||
double velocityTolerance = std::numeric_limits<double>::infinity(),
|
||||
frc2::PIDController::Tolerance toleranceType =
|
||||
frc2::PIDController::Tolerance::kAbsolute) const;
|
||||
|
||||
/**
|
||||
* Returns true if the error is within the tolerance of the error.
|
||||
*
|
||||
* Currently this just reports on target as the actual value passes through
|
||||
* the setpoint. Ideally it should be based on being within the tolerance for
|
||||
* some period of time.
|
||||
*
|
||||
* This will return false until at least one input value has been computed.
|
||||
*/
|
||||
bool AtSetpoint() const;
|
||||
|
||||
/**
|
||||
* Sets the minimum and maximum values expected from the input.
|
||||
*
|
||||
* @param minimumInput The minimum value expected from the input.
|
||||
* @param maximumInput The maximum value expected from the input.
|
||||
*/
|
||||
void SetInputRange(double minimumInput, double maximumInput);
|
||||
|
||||
/**
|
||||
* Enables continuous input.
|
||||
*
|
||||
* Rather then using the max and min input range as constraints, it considers
|
||||
* them to be the same point and automatically calculates the shortest route
|
||||
* to the setpoint.
|
||||
*
|
||||
* @param minimumInput The minimum value expected from the input.
|
||||
* @param maximumInput The maximum value expected from the input.
|
||||
*/
|
||||
void EnableContinuousInput(double minimumInput, double maximumInput);
|
||||
|
||||
/**
|
||||
* Disables continuous input.
|
||||
*/
|
||||
void DisableContinuousInput();
|
||||
|
||||
/**
|
||||
* Sets the minimum and maximum values to write.
|
||||
*
|
||||
* @param minimumOutput the minimum value to write to the output
|
||||
* @param maximumOutput the maximum value to write to the output
|
||||
*/
|
||||
void SetOutputRange(double minimumOutput, double maximumOutput);
|
||||
|
||||
/**
|
||||
* Sets the absolute error which is considered tolerable for use with
|
||||
* AtSetpoint().
|
||||
*
|
||||
* @param positionTolerance Position error which is tolerable.
|
||||
* @param velocityTolerance Velocity error which is tolerable.
|
||||
*/
|
||||
void SetAbsoluteTolerance(
|
||||
double positionTolerance,
|
||||
double velocityTolerance = std::numeric_limits<double>::infinity());
|
||||
|
||||
/**
|
||||
* Sets the percent error which is considered tolerable for use with
|
||||
* AtSetpoint().
|
||||
*
|
||||
* @param positionTolerance Position error which is tolerable.
|
||||
* @param velocityTolerance Velocity error which is tolerable.
|
||||
*/
|
||||
void SetPercentTolerance(
|
||||
double positionTolerance,
|
||||
double velocityTolerance = std::numeric_limits<double>::infinity());
|
||||
|
||||
/**
|
||||
* Returns the difference between the setpoint and the measurement.
|
||||
*
|
||||
* @return The error.
|
||||
*/
|
||||
double GetPositionError() const;
|
||||
|
||||
/**
|
||||
* Returns the change in error per second.
|
||||
*/
|
||||
double GetVelocityError() const;
|
||||
|
||||
/**
|
||||
* Returns the next output of the PID controller.
|
||||
*
|
||||
* @param measurement The current measurement of the process variable.
|
||||
*/
|
||||
double Calculate(double 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(double measurement, units::meter_t goal);
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* @param constraints Velocity and acceleration constraints for goal.
|
||||
*/
|
||||
double Calculate(double measurement, units::meter_t goal,
|
||||
frc::TrapezoidProfile::Constraints constraints);
|
||||
|
||||
/**
|
||||
* Reset the previous error, the integral term, and disable the controller.
|
||||
*/
|
||||
void Reset();
|
||||
|
||||
void InitSendable(frc::SendableBuilder& builder) override;
|
||||
|
||||
private:
|
||||
frc2::PIDController m_controller;
|
||||
frc::TrapezoidProfile::State m_goal;
|
||||
frc::TrapezoidProfile::State m_setpoint;
|
||||
frc::TrapezoidProfile::Constraints m_constraints;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
Reference in New Issue
Block a user