39 lines
654 B
C++
39 lines
654 B
C++
/*
|
|
* control.c
|
|
*
|
|
* Created on: 24.09.2012
|
|
* Author: wn
|
|
*/
|
|
|
|
|
|
#include "control.h"
|
|
#include "utils.h"
|
|
|
|
|
|
|
|
|
|
Control::Control(float p_rMin, float p_rMax, float p_kP, float p_kI, float p_kD) :
|
|
m_rOld(0), m_eOld(0), m_eOld2(0),
|
|
m_rMin(p_rMin), m_rMax(p_rMax), m_kP(p_kP), m_kI(p_kI), m_kD(p_kD)
|
|
{
|
|
}
|
|
|
|
|
|
float Control::cycle(float vDes, float vCur) volatile {
|
|
float e = vDes - vCur;
|
|
|
|
float rMot = m_rOld +
|
|
m_kP * (e - m_eOld) +
|
|
m_kI * (e + m_eOld) / 2 +
|
|
m_kD * (e - 2 * m_eOld + m_eOld2);
|
|
|
|
float r = minmax(m_rMin, rMot, m_rMax);
|
|
|
|
m_rOld = r;
|
|
m_eOld2 = m_eOld;
|
|
m_eOld = e;
|
|
return r;
|
|
}
|
|
|
|
|