philip/balancing-robot
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

dT as zero crash prevention

Browse Source 
Should not happen, but just in case.
This commit is contained in:
philsson 2018-09-09 22:38:58 +02:00
parent e1de1ac99f
commit d9ce797cae
2 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/control/PID.cpp
View File

@ -26,7 +26,7 @@ controllerPD::controllerPD(float kP, float kD, float saturation)
, m_kD(kD) , m_kD(kD)
, m_saturation(saturation) , m_saturation(saturation)
, m_lastError(0) , m_lastError(0)
, m_pt1FilterApply4(10.0f) , m_pt1FilterApply4(100.0f)
{ {
} }
@ -36,7 +36,7 @@ float controllerPD::run(float dT, float input, float setPoint)
float pTerm(m_kP*error); float pTerm(m_kP*error);
float dTerm(m_kD*((error-m_lastError)/dT)); float dTerm((dT != 0.0f) ? m_kD*((error-m_lastError)/dT) : 0.0f);
// Store error for next iteration // Store error for next iteration
m_lastError = error; m_lastError = error;

7
src/control/lpf.cpp
View File

@ -25,7 +25,12 @@ pt1FilterApply4::pt1FilterApply4(float freqCut)
float pt1FilterApply4::filter(float input, float dT) float pt1FilterApply4::filter(float input, float dT)
{ {
m_filtered = m_filtered + dT / (m_RC + dT) * (input - m_filtered); if (dT != 0.0f && m_RC + dT != 0.0f)
{
float gain(dT / (m_RC + dT));
m_filtered += gain * (input - m_filtered);
}
return m_filtered; return m_filtered;
} }