Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +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

57
wpilibc/wpilibC++Devices/src/PIDController.cpp
View File

@@ -127,20 +127,37 @@ 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)
@@ -325,7 +342,7 @@ double PIDController::GetSetpoint() const {
}
/**
* Retruns the current difference of the input from the setpoint
* Returns the current difference of the input from the setpoint
* @return the current error
*/
float PIDController::GetError() const {
@@ -337,6 +354,20 @@ float PIDController::GetError() const {
return GetSetpoint() - pidInput;
}
/**
* Sets what type of input the PID controller will use
*/
void PIDController::SetPIDSourceType(PIDSourceType pidSource) {
m_pidInput->SetPIDSourceType(pidSource);
}
/**
* Returns the type of input the PID controller is using
* @return the PID controller input type
*/
PIDSourceType PIDController::GetPIDSourceType() const {
return m_pidInput->GetPIDSourceType();
}
/*
* Set the percentage error which is considered tolerable for use with
* OnTarget.