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

artf4127: Implemented velocity PID controller

Browse Source 
Change-Id: I8c0f84422f7ca0ac5c50fddb282fb3452ee1d491
This commit is contained in:
Tyler Veness
2015-07-14 20:37:52 -07:00
parent f0e3bb5164
commit d5922bb037
43 changed files with 532 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
wpilibc/wpilibC++Sim/src/PIDController.cpp
View File

@@ -148,23 +148,42 @@ void PIDController::Calculate()
}
}
if(m_I != 0)
{
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput)
{
if (potentialIGain > m_minimumOutput)
m_totalError += m_error;
else
m_totalError = m_minimumOutput / m_I;
}
else
{
m_totalError = m_maximumOutput / m_I;
}
}
if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
if (m_P != 0) {
double potentialPGain = (m_totalError + m_error) * m_P;
if (potentialPGain < m_maximumOutput) {
if (potentialPGain > m_minimumOutput) {
m_totalError += m_error;
}
else {
m_totalError = m_minimumOutput / m_P;
}
}
else {
m_totalError = m_maximumOutput / m_P;
}
}
m_result = m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F;
m_result = m_D * m_error + m_P * m_totalError + m_setpoint * m_F;
}
else {
if (m_I != 0) {
double potentialIGain = (m_totalError + m_error) * m_I;
if (potentialIGain < m_maximumOutput) {
if (potentialIGain > m_minimumOutput) {
m_totalError += m_error;
}
else {
m_totalError = m_minimumOutput / m_I;
}
}
else {
m_totalError = m_maximumOutput / m_I;
}
}
m_result = m_P * m_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F;
}
m_prevInput = input;
if (m_result > m_maximumOutput) m_result = m_maximumOutput;