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

PIDController feed forward term can now be calculated by the end user

Browse Source 
The current feed forward calculation is only useful for velocity PID controllers where F, the feed forward constant, is 1 over the maximum setpoint for the output. For motion profiles which use position PID controllers, the appropriate calculation for velocity and acceleration feed forwards is different. This change allows the user to provide their own feed forward implementation without having to rewrite the entire Calculate() function.

Both default feed forward calculations are velocity feed forwards. Suggestions for sensible feed forward constants are included in the inline comments.

Change-Id: Id175786f26bd342de52a1fae89595cbeba5dfc93
This commit is contained in:
Tyler Veness
2015-08-26 03:19:35 -07:00
parent 3cd1253977
commit e3ce991f18
4 changed files with 190 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
wpilibc/Athena/src/PIDController.cpp
View File

@@ -67,6 +67,7 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
m_period = period;
m_controlLoop->StartPeriodic(m_period);
m_setpointTimer.Start();
static int32_t instances = 0;
instances++;
@@ -87,7 +88,7 @@ void PIDController::Calculate() {
PIDOutput *pidOutput;
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
pidInput = m_pidInput;
pidOutput = m_pidOutput;
enabled = m_enabled;
@@ -97,7 +98,7 @@ void PIDController::Calculate() {
if (pidOutput == nullptr) return;
if (enabled) {
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
float input = pidInput->PIDGet();
float result;
PIDOutput *pidOutput;
@@ -126,7 +127,7 @@ void PIDController::Calculate() {
}
}
m_result = m_D * m_error + m_P * m_totalError + m_setpoint * m_F;
m_result = m_D * m_error + m_P * m_totalError + CalculateFeedForward();
}
else {
if (m_I != 0) {
@@ -142,9 +143,9 @@ void PIDController::Calculate() {
}
m_result = m_P * m_error + m_I * m_totalError +
m_D * (m_prevInput - input) + m_setpoint * m_F;
m_D * (m_error - m_prevError) + CalculateFeedForward();
}
m_prevInput = input;
m_prevError = m_error;
if (m_result > m_maximumOutput)
m_result = m_maximumOutput;
@@ -167,6 +168,33 @@ void PIDController::Calculate() {
}
}
/**
* Calculate the feed forward term
*
* Both of the provided feed forward calculations are velocity feed forwards.
* If a different feed forward calculation is desired, the user can override
* this function and provide his or her own. This function does no
* synchronization because the PIDController class only calls it in synchronized
* code, so be careful if calling it oneself.
*
* If a velocity PID controller is being used, the F term should be set to 1
* over the maximum setpoint for the output. If a position PID controller is
* being used, the F term should be set to 1 over the maximum speed for the
* output measured in setpoint units per this controller's update period (see
* the default period in this class's constructor).
*/
double PIDController::CalculateFeedForward() {
if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
return m_F * GetSetpoint();
}
else {
double temp = m_F * GetDeltaSetpoint();
m_prevSetpoint = m_setpoint;
m_setpointTimer.Reset();
return temp;
}
}
/**
* Set the PID Controller gain parameters.
* Set the proportional, integral, and differential coefficients.
@@ -176,7 +204,7 @@ void PIDController::Calculate() {
*/
void PIDController::SetPID(double p, double i, double d) {
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_P = p;
m_I = i;
m_D = d;
@@ -199,7 +227,7 @@ void PIDController::SetPID(double p, double i, double d) {
*/
void PIDController::SetPID(double p, double i, double d, double f) {
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_P = p;
m_I = i;
m_D = d;
@@ -219,7 +247,7 @@ void PIDController::SetPID(double p, double i, double d, double f) {
* @return proportional coefficient
*/
double PIDController::GetP() const {
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_P;
}
@@ -228,7 +256,7 @@ double PIDController::GetP() const {
* @return integral coefficient
*/
double PIDController::GetI() const {
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_I;
}
@@ -237,7 +265,7 @@ double PIDController::GetI() const {
* @return differential coefficient
*/
double PIDController::GetD() const {
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_D;
}
@@ -246,7 +274,7 @@ double PIDController::GetD() const {
* @return Feed forward coefficient
*/
double PIDController::GetF() const {
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_F;
}
@@ -256,7 +284,7 @@ double PIDController::GetF() const {
* @return the latest calculated output
*/
float PIDController::Get() const {
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_result;
}
@@ -268,7 +296,7 @@ float PIDController::Get() const {
* @param continuous Set to true turns on continuous, false turns off continuous
*/
void PIDController::SetContinuous(bool continuous) {
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_continuous = continuous;
}
@@ -280,7 +308,7 @@ void PIDController::SetContinuous(bool continuous) {
*/
void PIDController::SetInputRange(float minimumInput, float maximumInput) {
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
}
@@ -296,7 +324,7 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput) {
*/
void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
@@ -309,7 +337,8 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
*/
void PIDController::SetSetpoint(float setpoint) {
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
if (m_maximumInput > m_minimumInput) {
if (setpoint > m_maximumInput)
m_setpoint = m_maximumInput;
@@ -335,10 +364,19 @@ void PIDController::SetSetpoint(float setpoint) {
* @return the current setpoint
*/
double PIDController::GetSetpoint() const {
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_setpoint;
}
/**
* Returns the change in setpoint over time of the PIDController
* @return the change in setpoint over time
*/
double PIDController::GetDeltaSetpoint() const {
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
}
/**
* Returns the current difference of the input from the setpoint
* @return the current error
@@ -346,7 +384,7 @@ double PIDController::GetSetpoint() const {
float PIDController::GetError() const {
double pidInput;
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
pidInput = m_pidInput->PIDGet();
}
return GetSetpoint() - pidInput;
@@ -375,7 +413,7 @@ PIDSourceType PIDController::GetPIDSourceType() const {
float PIDController::GetAvgError() const {
float avgError = 0;
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
// Don't divide by zero.
if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
}
@@ -389,7 +427,7 @@ float PIDController::GetAvgError() const {
*/
void PIDController::SetTolerance(float percent) {
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
@@ -402,7 +440,7 @@ void PIDController::SetTolerance(float percent) {
*/
void PIDController::SetPercentTolerance(float percent) {
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
@@ -415,7 +453,7 @@ void PIDController::SetPercentTolerance(float percent) {
*/
void PIDController::SetAbsoluteTolerance(float absTolerance) {
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_toleranceType = kAbsoluteTolerance;
m_tolerance = absTolerance;
}
@@ -452,7 +490,7 @@ void PIDController::SetToleranceBuffer(unsigned bufLength) {
bool PIDController::OnTarget() const {
double error = GetAvgError();
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
switch (m_toleranceType) {
case kPercentTolerance:
return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
@@ -472,7 +510,7 @@ bool PIDController::OnTarget() const {
*/
void PIDController::Enable() {
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_enabled = true;
}
@@ -486,7 +524,7 @@ void PIDController::Enable() {
*/
void PIDController::Disable() {
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_pidOutput->PIDWrite(0);
m_enabled = false;
}
@@ -500,7 +538,7 @@ void PIDController::Disable() {
* Return true if PIDController is enabled.
*/
bool PIDController::IsEnabled() const {
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return m_enabled;
}
@@ -510,8 +548,8 @@ bool PIDController::IsEnabled() const {
void PIDController::Reset() {
Disable();
std::lock_guard<priority_mutex> sync(m_mutex);
m_prevInput = 0;
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_prevError = 0;
m_totalError = 0;
m_result = 0;
}

9
wpilibc/shared/include/PIDController.h
View File

@@ -14,6 +14,7 @@
#include "PIDSource.h"
#include "Notifier.h"
#include "HAL/cpp/priority_mutex.h"
#include "Timer.h"
#include <memory>
@@ -55,6 +56,7 @@ class PIDController : public LiveWindowSendable,
virtual void SetSetpoint(float setpoint) override;
virtual double GetSetpoint() const override;
double GetDeltaSetpoint() const;
virtual float GetError() const;
virtual float GetAvgError() const;
@@ -82,6 +84,7 @@ class PIDController : public LiveWindowSendable,
std::shared_ptr<ITable> m_table;
virtual void Calculate();
virtual double CalculateFeedForward();
private:
float m_P; // factor for "proportional" control
@@ -94,7 +97,7 @@ class PIDController : public LiveWindowSendable,
float m_minimumInput = 0; // minimum input - limit setpoint to this
bool m_continuous = false; // do the endpoints wrap around? eg. Absolute encoder
bool m_enabled = false; // is the pid controller enabled
float m_prevInput = 0; // the prior sensor input (used to compute velocity)
float m_prevError = 0; // the prior error (used to compute velocity)
double m_totalError = 0; // the sum of the errors for use in the integral calc
enum {
kAbsoluteTolerance,
@@ -105,6 +108,7 @@ class PIDController : public LiveWindowSendable,
// the percetage or absolute error that is considered on target.
float m_tolerance = 0.05;
float m_setpoint = 0;
float m_prevSetpoint = 0;
float m_error = 0;
float m_result = 0;
float m_period;
@@ -114,9 +118,10 @@ class PIDController : public LiveWindowSendable,
std::queue<double> m_buf;
double m_bufTotal = 0;
mutable priority_mutex m_mutex;
mutable priority_recursive_mutex m_mutex;
std::unique_ptr<Notifier> m_controlLoop;
Timer m_setpointTimer;
void Initialize(float p, float i, float d, float f, PIDSource *source,
PIDOutput *output, float period = 0.05);

98
wpilibc/simulation/src/PIDController.cpp
View File

@@ -75,7 +75,7 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
m_enabled = false;
m_setpoint = 0;
m_prevInput = 0;
m_prevError = 0;
m_totalError = 0;
m_tolerance = .05;
@@ -108,7 +108,7 @@ void PIDController::Calculate()
PIDSource *pidInput;
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
if (m_pidInput == 0) return;
if (m_pidOutput == 0) return;
enabled = m_enabled;
@@ -122,7 +122,7 @@ void PIDController::Calculate()
PIDOutput *pidOutput;
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
m_error = m_setpoint - input;
if (m_continuous)
{
@@ -155,7 +155,8 @@ void PIDController::Calculate()
}
}
m_result = m_D * m_error + m_P * m_totalError + m_setpoint * m_F;
m_result = m_D * m_error + m_P * m_totalError +
CalculateFeedForward();
}
else {
if (m_I != 0) {
@@ -173,9 +174,10 @@ void PIDController::Calculate()
}
}
m_result = m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F;
m_result = m_P * m_error + m_I * m_totalError +
m_D * (m_error - m_prevError) + CalculateFeedForward();
}
m_prevInput = input;
m_prevError = m_error;
if (m_result > m_maximumOutput) m_result = m_maximumOutput;
else if (m_result < m_minimumOutput) m_result = m_minimumOutput;
@@ -188,6 +190,33 @@ void PIDController::Calculate()
}
}
/**
* Calculate the feed forward term
*
* Both of the provided feed forward calculations are velocity feed forwards.
* If a different feed forward calculation is desired, the user can override
* this function and provide his or her own. This function does no
* synchronization because the PIDController class only calls it in synchronized
* code, so be careful if calling it oneself.
*
* If a velocity PID controller is being used, the F term should be set to 1
* over the maximum setpoint for the output. If a position PID controller is
* being used, the F term should be set to 1 over the maximum speed for the
* output measured in setpoint units per this controller's update period (see
* the default period in this class's constructor).
*/
double PIDController::CalculateFeedForward() {
if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
return m_F * GetSetpoint();
}
else {
double temp = m_F * GetDeltaSetpoint();
m_prevSetpoint = m_setpoint;
m_setpointTimer.Reset();
return temp;
}
}
/**
* Set the PID Controller gain parameters.
* Set the proportional, integral, and differential coefficients.
@@ -198,7 +227,7 @@ void PIDController::Calculate()
void PIDController::SetPID(double p, double i, double d)
{
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_P = p;
m_I = i;
m_D = d;
@@ -222,7 +251,7 @@ void PIDController::SetPID(double p, double i, double d)
void PIDController::SetPID(double p, double i, double d, double f)
{
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_P = p;
m_I = i;
m_D = d;
@@ -243,7 +272,7 @@ void PIDController::SetPID(double p, double i, double d, double f)
*/
double PIDController::GetP() const
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_P;
}
@@ -253,7 +282,7 @@ double PIDController::GetP() const
*/
double PIDController::GetI() const
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_I;
}
@@ -263,7 +292,7 @@ double PIDController::GetI() const
*/
double PIDController::GetD() const
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_D;
}
@@ -273,7 +302,7 @@ double PIDController::GetD() const
*/
double PIDController::GetF() const
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_F;
}
@@ -284,7 +313,7 @@ double PIDController::GetF() const
*/
float PIDController::Get() const
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_result;
}
@@ -297,7 +326,7 @@ float PIDController::Get() const
*/
void PIDController::SetContinuous(bool continuous)
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_continuous = continuous;
}
@@ -310,7 +339,7 @@ void PIDController::SetContinuous(bool continuous)
void PIDController::SetInputRange(float minimumInput, float maximumInput)
{
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
}
@@ -326,7 +355,7 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput)
*/
void PIDController::SetOutputRange(float minimumOutput, float maximumOutput)
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
@@ -338,7 +367,8 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput)
void PIDController::SetSetpoint(float setpoint)
{
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
if (m_maximumInput > m_minimumInput)
{
if (setpoint > m_maximumInput)
@@ -365,10 +395,20 @@ void PIDController::SetSetpoint(float setpoint)
*/
double PIDController::GetSetpoint() const
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_setpoint;
}
/**
* Returns the change in setpoint over time of the PIDController
* @return the change in setpoint over time
*/
double PIDController::GetDeltaSetpoint() const
{
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
}
/**
* Retruns the current difference of the input from the setpoint
* @return the current error
@@ -377,7 +417,7 @@ float PIDController::GetError() const
{
double pidInput;
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
pidInput = m_pidInput->PIDGet();
}
return GetSetpoint() - pidInput;
@@ -407,7 +447,7 @@ PIDSourceType PIDController::GetPIDSourceType() const {
float PIDController::GetAvgError() const {
float avgError = 0;
{
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
// Don't divide by zero.
if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
}
@@ -421,7 +461,7 @@ float PIDController::GetAvgError() const {
*/
void PIDController::SetTolerance(float percent)
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
@@ -433,7 +473,7 @@ void PIDController::SetTolerance(float percent)
*/
void PIDController::SetPercentTolerance(float percent)
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
@@ -445,7 +485,7 @@ void PIDController::SetPercentTolerance(float percent)
*/
void PIDController::SetAbsoluteTolerance(float absTolerance)
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_toleranceType = kAbsoluteTolerance;
m_tolerance = absTolerance;
}
@@ -480,7 +520,7 @@ bool PIDController::OnTarget() const
{
double error = GetError();
std::lock_guard<priority_mutex> sync(m_mutex);
std::lock_guard<priority_recursive_mutex> sync(m_mutex);
switch (m_toleranceType) {
case kPercentTolerance:
return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
@@ -500,7 +540,7 @@ bool PIDController::OnTarget() const
void PIDController::Enable()
{
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_enabled = true;
}
@@ -515,7 +555,7 @@ void PIDController::Enable()
void PIDController::Disable()
{
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_pidOutput->PIDWrite(0);
m_enabled = false;
}
@@ -530,7 +570,7 @@ void PIDController::Disable()
*/
bool PIDController::IsEnabled() const
{
std::lock_guard<priority_mutex> lock(m_mutex);
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
return m_enabled;
}
@@ -541,8 +581,8 @@ void PIDController::Reset()
{
Disable();
std::lock_guard<priority_mutex> lock(m_mutex);
m_prevInput = 0;
std::lock_guard<priority_recursive_mutex> lock(m_mutex);
m_prevError = 0;
m_totalError = 0;
m_result = 0;
}

55
wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java
View File

@@ -8,6 +8,7 @@ package edu.wpi.first.wpilibj;
import java.util.TimerTask;
import java.util.LinkedList;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
import edu.wpi.first.wpilibj.tables.ITable;
import edu.wpi.first.wpilibj.tables.ITableListener;
@@ -34,7 +35,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
private boolean m_continuous = false; // do the endpoints wrap around? eg.
// Absolute encoder
private boolean m_enabled = false; // is the pid controller enabled
private double m_prevInput = 0.0; // the prior sensor input (used to compute
private double m_prevError = 0.0; // the prior error (used to compute
// velocity)
private double m_totalError = 0.0; // the sum of the errors for use in the
// integral calc
@@ -44,12 +45,14 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
private LinkedList<Double> m_buf;
private double m_bufTotal = 0.0;
private double m_setpoint = 0.0;
private double m_prevSetpoint = 0.0;
private double m_error = 0.0;
private double m_result = 0.0;
private double m_period = kDefaultPeriod;
protected PIDSource m_pidInput;
protected PIDOutput m_pidOutput;
java.util.Timer m_controlLoop;
Timer m_setpointTimer;
private boolean m_freed = false;
private boolean m_usingPercentTolerance;
@@ -141,7 +144,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
}
m_controlLoop = new java.util.Timer();
m_setpointTimer = new Timer();
m_setpointTimer.start();
m_P = Kp;
m_I = Ki;
@@ -275,7 +279,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
m_totalError = m_maximumOutput / m_P;
}
m_result = m_P * m_totalError + m_D * m_error + m_setpoint * m_F;
m_result = m_P * m_totalError + m_D * m_error +
calculateFeedForward();
}
}
else {
@@ -292,10 +297,10 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
}
}
m_result =
m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F;
m_result = m_P * m_error + m_I * m_totalError +
m_D * (m_error - m_prevError) + calculateFeedForward();
}
m_prevInput = input;
m_prevError = m_error;
if (m_result > m_maximumOutput) {
m_result = m_maximumOutput;
@@ -318,6 +323,33 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
}
}
/**
* Calculate the feed forward term
*
* Both of the provided feed forward calculations are velocity feed forwards.
* If a different feed forward calculation is desired, the user can override
* this function and provide his or her own. This function does no
* synchronization because the PIDController class only calls it in
* synchronized code, so be careful if calling it oneself.
*
* If a velocity PID controller is being used, the F term should be set to 1
* over the maximum setpoint for the output. If a position PID controller is
* being used, the F term should be set to 1 over the maximum speed for the
* output measured in setpoint units per this controller's update period (see
* the default period in this class's constructor).
*/
protected double calculateFeedForward() {
if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
return m_F * getSetpoint();
}
else {
double temp = m_F * getDeltaSetpoint();
m_prevSetpoint = m_setpoint;
m_setpointTimer.reset();
return temp;
}
}
/**
* Set the PID Controller gain parameters. Set the proportional, integral, and
* differential coefficients.
@@ -491,6 +523,15 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
return m_setpoint;
}
/**
* Returns the change in setpoint over time of the PIDController
*$
* @return the change in setpoint over time
*/
public synchronized double getDeltaSetpoint() {
return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
}
/**
* Returns the current difference of the input from the setpoint
*$
@@ -658,7 +699,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
*/
public synchronized void reset() {
disable();
m_prevInput = 0;
m_prevError = 0;
m_totalError = 0;
m_result = 0;
}