Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Brad Miller (WPI)
2016-01-06 14:38:59 -08:00
committed by Gerrit Code Review
parent 0e9f07d1c2 e3ce991f18
commit 3350926d26
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;
}