From e3ce991f18477153fdac8e062cbb12a0153eada9 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 26 Aug 2015 03:19:35 -0700 Subject: [PATCH] PIDController feed forward term can now be calculated by the end user 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 --- wpilibc/Athena/src/PIDController.cpp | 94 ++++++++++++------ wpilibc/shared/include/PIDController.h | 9 +- wpilibc/simulation/src/PIDController.cpp | 98 +++++++++++++------ .../edu/wpi/first/wpilibj/PIDController.java | 55 +++++++++-- 4 files changed, 190 insertions(+), 66 deletions(-) diff --git a/wpilibc/Athena/src/PIDController.cpp b/wpilibc/Athena/src/PIDController.cpp index eaeec8f010..81e97fc83a 100644 --- a/wpilibc/Athena/src/PIDController.cpp +++ b/wpilibc/Athena/src/PIDController.cpp @@ -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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard sync(m_mutex); return m_P; } @@ -228,7 +256,7 @@ double PIDController::GetP() const { * @return integral coefficient */ double PIDController::GetI() const { - std::lock_guard sync(m_mutex); + std::lock_guard sync(m_mutex); return m_I; } @@ -237,7 +265,7 @@ double PIDController::GetI() const { * @return differential coefficient */ double PIDController::GetD() const { - std::lock_guard sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard sync(m_mutex); m_enabled = true; } @@ -486,7 +524,7 @@ void PIDController::Enable() { */ void PIDController::Disable() { { - std::lock_guard sync(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard sync(m_mutex); return m_enabled; } @@ -510,8 +548,8 @@ bool PIDController::IsEnabled() const { void PIDController::Reset() { Disable(); - std::lock_guard sync(m_mutex); - m_prevInput = 0; + std::lock_guard sync(m_mutex); + m_prevError = 0; m_totalError = 0; m_result = 0; } diff --git a/wpilibc/shared/include/PIDController.h b/wpilibc/shared/include/PIDController.h index e0a9521fd6..5e743894e2 100644 --- a/wpilibc/shared/include/PIDController.h +++ b/wpilibc/shared/include/PIDController.h @@ -14,6 +14,7 @@ #include "PIDSource.h" #include "Notifier.h" #include "HAL/cpp/priority_mutex.h" +#include "Timer.h" #include @@ -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 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 m_buf; double m_bufTotal = 0; - mutable priority_mutex m_mutex; + mutable priority_recursive_mutex m_mutex; std::unique_ptr m_controlLoop; + Timer m_setpointTimer; void Initialize(float p, float i, float d, float f, PIDSource *source, PIDOutput *output, float period = 0.05); diff --git a/wpilibc/simulation/src/PIDController.cpp b/wpilibc/simulation/src/PIDController.cpp index fa42439191..f337d45b30 100644 --- a/wpilibc/simulation/src/PIDController.cpp +++ b/wpilibc/simulation/src/PIDController.cpp @@ -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 lock(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 lock(m_mutex); + std::lock_guard 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 lock(m_mutex); + std::lock_guard 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 lock(m_mutex); + std::lock_guard lock(m_mutex); return m_P; } @@ -253,7 +282,7 @@ double PIDController::GetP() const */ double PIDController::GetI() const { - std::lock_guard lock(m_mutex); + std::lock_guard lock(m_mutex); return m_I; } @@ -263,7 +292,7 @@ double PIDController::GetI() const */ double PIDController::GetD() const { - std::lock_guard lock(m_mutex); + std::lock_guard lock(m_mutex); return m_D; } @@ -273,7 +302,7 @@ double PIDController::GetD() const */ double PIDController::GetF() const { - std::lock_guard lock(m_mutex); + std::lock_guard lock(m_mutex); return m_F; } @@ -284,7 +313,7 @@ double PIDController::GetF() const */ float PIDController::Get() const { - std::lock_guard lock(m_mutex); + std::lock_guard lock(m_mutex); return m_result; } @@ -297,7 +326,7 @@ float PIDController::Get() const */ void PIDController::SetContinuous(bool continuous) { - std::lock_guard lock(m_mutex); + std::lock_guard 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 lock(m_mutex); + std::lock_guard 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 lock(m_mutex); + std::lock_guard 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 lock(m_mutex); + std::lock_guard 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 lock(m_mutex); + std::lock_guard 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 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 lock(m_mutex); + std::lock_guard 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 sync(m_mutex); + std::lock_guard 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 lock(m_mutex); + std::lock_guard 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 lock(m_mutex); + std::lock_guard 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 lock(m_mutex); + std::lock_guard lock(m_mutex); m_toleranceType = kAbsoluteTolerance; m_tolerance = absTolerance; } @@ -480,7 +520,7 @@ bool PIDController::OnTarget() const { double error = GetError(); - std::lock_guard sync(m_mutex); + std::lock_guard 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 lock(m_mutex); + std::lock_guard lock(m_mutex); m_enabled = true; } @@ -515,7 +555,7 @@ void PIDController::Enable() void PIDController::Disable() { { - std::lock_guard lock(m_mutex); + std::lock_guard lock(m_mutex); m_pidOutput->PIDWrite(0); m_enabled = false; } @@ -530,7 +570,7 @@ void PIDController::Disable() */ bool PIDController::IsEnabled() const { - std::lock_guard lock(m_mutex); + std::lock_guard lock(m_mutex); return m_enabled; } @@ -541,8 +581,8 @@ void PIDController::Reset() { Disable(); - std::lock_guard lock(m_mutex); - m_prevInput = 0; + std::lock_guard lock(m_mutex); + m_prevError = 0; m_totalError = 0; m_result = 0; } diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java index d6f74a34dc..da0989bcca 100644 --- a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java +++ b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java @@ -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 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; }