DynaPid Class Reference

Proportional–Integral–Derivative Controller for Dynamixel servos. More...

#include <DynaPid.h>

Inheritance diagram for DynaPid:

Public Member Functions
	DynaPid (double fKp=PidKpDft, double fKi=PidKiDft, double fKd=PidKdDft, double fWi=WiSumErrDft)
	Default constructor. More...
	DynaPid (const DynaPid &src)
	Copy constructor.
virtual	~DynaPid ()
	Default destructor.
virtual void	SetConstants (double fKp, double fKi, double fKd)
	Set PID constants. More...
double	GetKp () const
	Get PID proportional constant. More...
double	GetKi () const
	Get PID integral constant. More...
double	GetKd () const
	Get PID derivative constant. More...
double	GetSP () const
	Get PID setpoint. More...
double	GetCV () const
	Get PID control variable. More...
double	GetOutput () const
	Get PID output. More...
double	GetError () const
	Get current error. More...
virtual void	InitControl ()
	Initialize the PID control variables.
virtual void	SpecifySetPoint (double fSP, bool bUnwind=false)
	Specify the setpoint. More...
virtual double	Control (double fPV, double dt)
	Apply PID control. More...
DynaPid &	operator= (const DynaPid &rhs)
	Assignment operator. More...

Static Public Attributes
static const double	PidKpDft = 0.5
	default Kp constant
static const double	PidKiDft = 1.0
	default Ki constant
static const double	PidKdDft = 0.1
	default Kd constant
static const double	WiSumErrDft = 0.1
	default sum error weight

Protected Member Functions
virtual double	error (double fPV)
	Calculate error from setpoint and current process variable value. More...
virtual double	toOutput (double fCV)
	Convert control variable to application-specific output value. More...

Protected Attributes
double	m_fSP
	setpoint
double	m_fPV
	process variable
double	m_fKp
	proportional constant
double	m_fKi
	integral constant
double	m_fKd
	derivative constant
double	m_fWi
	weighted sum of errors moving average constant
bool	m_bHasPrev
	has previous error
double	m_fErrPrev
	previous error
double	m_fErr
	error
double	m_fErrSum
	sum of errors
double	m_fCV
	control variable value
double	m_fOutput
	control variable output (could be same as CV)

Detailed Description

Proportional–Integral–Derivative Controller for Dynamixel servos.

The Servo PID:

setpoint (SP)	=	application specific
process variable (PV)	=	application specific
control variable (CV)	=	application specific

The PID Control Loop:

CV	=	Kp * errj + Ki * Σjerrk * dtk + Kd * (errj - errj-1) / dtj
output	=	converted control variable

where:

j	this time step
j-1	previous time step
dtk	delta time between time steps k-1 and k
errk	error = SP - PVk
PVk	process variable at time step k
Kp	proportional constant
Ki	integral constant
Kd	derivative constant

Definition at line 104 of file DynaPid.h.

Constructor & Destructor Documentation

DynaPid::DynaPid ( double  fKp = PidKpDft, double  fKi = PidKiDft, double  fKd = PidKdDft, double  fWi = WiSumErrDft  )

inline

Default constructor.

Parameters

fKp	PID proportional constant.
fKi	PID integral constant.
fKd	PID derivative constant.
fWi	PID integral sum of errors weight constant.

Definition at line 120 of file DynaPid.h.

  {
    m_fWi = fWi;
    SetConstants(fKp, fKi, fKd);
    InitControl();
  }

Member Function Documentation

double DynaPid::Control ( double  fPV, double  dt  )

virtual

Apply PID control.

Parameters

fPV	Process variable.
dt	Delta time step (seconds).

Returns

Output.

Definition at line 83 of file DynaPid.cxx.

Referenced by DynaPidSpeed::Control(), and DynaBgThread::execPosCtl().

{
  double  fErrDt;       // change in error w.r.t delta time

  // new process variable value
  m_fPV = fPV;
  
  // error
  m_fErr = error(m_fPV);
    
  if( !m_bHasPrev )
  {
    m_fErrPrev = m_fErr;
    m_bHasPrev = true;
  }

  // weigthed sum of errors moving average
  m_fErrSum = (1.0 - m_fWi) * m_fErrSum + m_fWi * (m_fErr * dt);

  // delta error
  if( dt > 0.0 )
  {
    fErrDt = (m_fErr - m_fErrPrev)/dt;
  }
  else
  {
    fErrDt = 0.0;
  }
  
  m_fErrPrev = m_fErr;

  // new control variable value
  m_fCV = (m_fKp * m_fErr) + (m_fKi * m_fErrSum) + (m_fKd * fErrDt);
  
  m_fOutput = toOutput(m_fCV);

  return m_fOutput;
}

virtual double DynaPid::error ( double  fPV )

inlineprotectedvirtual

Calculate error from setpoint and current process variable value.

Parameters

fPV	Process variable value.

Returns

Error.

Reimplemented in DynaPidPos.

Definition at line 274 of file DynaPid.h.

  {
    return m_fSP - fPV;
  }

double DynaPid::GetCV ( ) const

inline

Get PID control variable.

Returns

CV.

Definition at line 187 of file DynaPid.h.

{ return m_fCV; }

double DynaPid::GetError ( ) const

inline

Get current error.

Returns

Error.

Definition at line 199 of file DynaPid.h.

Referenced by DynaBgThread::execPosCtl().

{ return m_fErr; }

double DynaPid::GetKd ( ) const

inline

Get PID derivative constant.

Returns

K_d

Definition at line 175 of file DynaPid.h.

{ return m_fKd; }

double DynaPid::GetKi ( ) const

inline

Get PID integral constant.

Returns

K_i

Definition at line 169 of file DynaPid.h.

{ return m_fKi; }

double DynaPid::GetKp ( ) const

inline

Get PID proportional constant.

Returns

K_p

Definition at line 163 of file DynaPid.h.

{ return m_fKp; }

double DynaPid::GetOutput ( ) const

inline

Get PID output.

Returns

Output.

Definition at line 193 of file DynaPid.h.

{ return m_fOutput; }

double DynaPid::GetSP ( ) const

inline

Get PID setpoint.

Returns

SP.

Definition at line 181 of file DynaPid.h.

{ return m_fSP; }

DynaPid& DynaPid::operator= ( const DynaPid &  rhs )

inline

Assignment operator.

Parameters

rhs	Right hand side object.

Returns

Returns *this.

Definition at line 245 of file DynaPid.h.

References m_fKd, m_fKi, m_fKp, and m_fWi.

  {
    m_fWi = rhs.m_fWi;
    SetConstants(rhs.m_fKp, rhs.m_fKi, rhs.m_fKd);
    InitControl();
    return *this;
  }

virtual void DynaPid::SetConstants ( double  fKp, double  fKi, double  fKd  )

inlinevirtual

Set PID constants.

Parameters

fKp	PID proportional constant.
fKi	PID integral constant.
fKd	PID derivative constant.

Definition at line 152 of file DynaPid.h.

  {
    m_fKp = fKp;
    m_fKi = fKi;
    m_fKd = fKd;
  }

void DynaPid::SpecifySetPoint ( double  fSP, bool  bUnwind = false  )

virtual

Specify the setpoint.

The PID can also be unwound. This PID technique is used when a new setpoint causes the PID to behave badly, normally through integral windup. Unwinding resets the accumulated sum of errors integral term to zero.

Parameters

fSP	New setpoint.
bUnwind	Do [not] reset controller integral sum of errors term

Reimplemented in DynaPidSpeed.

Definition at line 71 of file DynaPid.cxx.

Referenced by DynaPidPos::SpecifySetPoint(), and DynaPidSpeed::SpecifySetPoint().

{
  m_fSP = fSP;

  m_bHasPrev = false;

  if( bUnwind )
  {
    m_fErrSum = 0.0;
  }
}

virtual double DynaPid::toOutput ( double  fCV )

inlineprotectedvirtual

Convert control variable to application-specific output value.

Parameters

fCV	Control variable value.

Returns

Output.

Reimplemented in DynaPidPos.

Definition at line 286 of file DynaPid.h.

  {
    return fCV;
  }

---

The documentation for this class was generated from the following files:

• include/Dynamixel/DynaPid.h
• libDynamixel/DynaPid.cxx