mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
Add ProfiledPIDController
This commit is contained in:
Tyler Veness
committed by
Peter Johnson
Peter Johnson
parent
fc98a79dbb
commit
3ebc5a6d3a
@@ -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