Speed PID class. More...

#include <DynaPidSpeed.h>

Inheritance diagram for DynaPidSpeed:

Public Member Functions
	DynaPidSpeed (double fKp=SpeedPidKpDft, double fKi=SpeedPidKiDft, double fKd=SpeedPidKdDft, double fWi=DynaPid::WiSumErrDft)
	Default constructor. More...

	DynaPidSpeed (const DynaPidSpeed &src)
	Copy constructor.

virtual	~DynaPidSpeed ()
	Default destructor.

virtual void	SetServoParams (int nMode, int nSpeedMin, int nSpeedMax, int nOdModulo)
	Set servo paramaters required for speed PID. More...

virtual void	SetStartingPos (uint_t uStartPos)
	Set speed PID starting positions. More...

virtual double	CalcDpDt (uint_t uCurPos, uint_t uPrevPos, double dt)
	Convert the current and previous positions measured over delta time to dp/dt. More...

virtual double	CalcDpDt (int nCurPos, int nPrevPos, double dt)
	Convert the current and previous positions measured over delta time to dp/dt. More...

virtual void	SpecifySetPoint (double fDpDt, bool bUnwind=false)
	Specify the setpoint. More...

virtual double	Control (uint_t uCurPos, double dt)
	Apply PID control. More...

DynaPidSpeed &	operator= (const DynaPidSpeed &rhs)
	Assignment operator. More...

Public Member Functions inherited from DynaPid
	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 double	Control (double fPV, double dt)
	Apply PID control. More...

DynaPid &	operator= (const DynaPid &rhs)
	Assignment operator. More...

Static Public Attributes
static const double	SpeedPidKpDft = 0.5
	default Kp constant

static const double	SpeedPidKiDft = 1.0
	default Ki constant

static const double	SpeedPidKdDft = 0.1
	default Kd constant

Static Public Attributes inherited from DynaPid
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	toPV (uint_t uCurPos, double dt)
	Save current position and convert to dp/dt process variable. More...

virtual double	toOutput ()
	Convert control variable to raw servo speed. More...

Protected Member Functions inherited from DynaPid
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
int	m_nPos [2]
	servo current (0) and previous (1) positions

int	m_nMode
	servo mode

int	m_nSpeedMin
	servo minimum raw value

int	m_nSpeedMax
	servo maximum raw value

int	m_nOdModulo
	servo odometer rollover modulo

Protected Attributes inherited from DynaPid
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

Speed PID class.

Definition at line 76 of file DynaPidSpeed.h.

Constructor & Destructor Documentation

DynaPidSpeed::DynaPidSpeed	(	double	fKp = `SpeedPidKpDft`,
		double	fKi = `SpeedPidKiDft`,
		double	fKd = `SpeedPidKdDft`,
		double	fWi = `DynaPid::WiSumErrDft`
	)

inline

Default constructor.

Parameters

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

Definition at line 91 of file DynaPidSpeed.h.

References DYNA_MODE_SERVO, DYNA_POS_MODULO, DYNA_SPEED_MAX_CTL, and DYNA_SPEED_MIN_CTL.

                                               :
       DynaPid(fKp, fKi, fKd, fWi)
   {
     SetServoParams(DYNA_MODE_SERVO, DYNA_SPEED_MIN_CTL, DYNA_SPEED_MAX_CTL,
                     DYNA_POS_MODULO);
     SetStartingPos(0);
   }

Member Function Documentation

virtual double DynaPidSpeed::CalcDpDt	(	uint_t	uCurPos,
		uint_t	uPrevPos,
		double	dt
	)

inlinevirtual

Convert the current and previous positions measured over delta time to dp/dt.

Since there are two solutions to delta position for servos in continuous mode, the smallest delta is taken. This assumes that the position sampling rate is fast enough not to cause ambiguity.

Parameters

nCurPos	Current servo position.
nPrevPos	Current servo position.
dt	Delta time between previous and current positions (seconds).

Returns: dp/dt.

Definition at line 162 of file DynaPidSpeed.h.

   {
     return CalcDpDt((int)uCurPos, (int)uPrevPos, dt);
   }

double DynaPidSpeed::CalcDpDt	(	int	nCurPos,
		int	nPrevPos,
		double	dt
	)

virtual

Convert the current and previous positions measured over delta time to dp/dt.

Since there are two solutions to delta position for servos in continuous mode, the smallest delta is taken. This assumes that the position sampling rate is fast enough not to cause ambiguity.

Parameters

nCurPos	Current servo position.
nPrevPos	Current servo position.
dt	Delta time between previous and current positions (seconds).

Returns: dp/dt.

Definition at line 101 of file DynaPidSpeed.cxx.

References DYNA_MODE_CONTINUOUS, and iabs().

 {
   int     nDp;      // delta position
   double  fDpDt;    // dp/dt where dp = delta raw position, dt = delta time
 
   nDp = nCurPos - nPrevPos;
 
   //
   // The two positions straddle the roll-over point.
   //
   if( (m_nMode == DYNA_MODE_CONTINUOUS) && (iabs(nDp) > m_nOdModulo/2) )
   {
     if( nDp < 0 )
     {
       nDp = m_nOdModulo + nDp;
     }
     else
     {
       nDp = nDp - m_nOdModulo;
     }
   }
 
   if( dt > 0.0 )
   {
     fDpDt = ((double)(nDp))/dt;
   }
   else
   {
     fDpDt = 0.0;
   }
 
   return fDpDt;
 }

virtual double DynaPidSpeed::Control	(	uint_t	uCurPos,
		double	dt
	)

inlinevirtual

Apply PID control.

Parameters

uCurPos	Current servo position.
dt	Delta time (seconds).

Returns: Output servo raw speed.

Definition at line 205 of file DynaPidSpeed.h.

References DynaPid::Control().

    {
      return DynaPid::Control(toPV(uCurPos, dt), dt);
    }

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

inline

Assignment operator.

Parameters

rhs	Right hand side object.

Returns: Returns *this.

Definition at line 217 of file DynaPidSpeed.h.

References DynaPid::m_fKd, DynaPid::m_fKi, DynaPid::m_fKp, DynaPid::m_fWi, m_nMode, m_nOdModulo, m_nPos, m_nSpeedMax, and m_nSpeedMin.

   {
     m_fWi = rhs.m_fWi;
     SetConstants(rhs.m_fKp, rhs.m_fKi, rhs.m_fKd);
     SetServoParams(rhs.m_nMode, rhs.m_nSpeedMin, rhs.m_nSpeedMax,
                       rhs.m_nOdModulo);
     SetStartingPos(rhs.m_nPos[0]);
     InitControl();
     return *this;
   }

virtual void DynaPidSpeed::SetServoParams	(	int	nMode,
		int	nSpeedMin,
		int	nSpeedMax,
		int	nOdModulo
	)

inlinevirtual

Set servo paramaters required for speed PID.

Parameters

nMode	Servo mode. (See Dynamixel Operational Modes).
nSpeedMin	Servo minimum raw speed.
nSpeedMax	Servo maximum raw speed.
nOdModulo	Odometer roll-over modulo.

Definition at line 126 of file DynaPidSpeed.h.

   {
     m_nMode     = nMode;
     m_nSpeedMin = nSpeedMin;
     m_nSpeedMax = nSpeedMax;
     m_nOdModulo = nOdModulo;
   }

virtual void DynaPidSpeed::SetStartingPos ( uint_t uStartPos )

inlinevirtual

Set speed PID starting positions.

Parameters

uStartPos Starting servo position.

Definition at line 142 of file DynaPidSpeed.h.

   {
     m_nPos[0] = (int)uStartPos;
     m_nPos[1] = (int)uStartPos;
   }

void DynaPidSpeed::SpecifySetPoint	(	double	fDpDt,
		bool	bUnwind = `false`
	)

virtual

Specify the setpoint.

The PID can also be reset. This PID technique is used when a new setpoint causes the PID to behave badly, normally through integral windup. The reset: o Zeros accumulated sum of errors.

Parameters

fDpDt	Speed dp/dt where dp = delta position, dt = delta time.
bUnwind	Do [not] reset controller integral sum of errors term

Reimplemented from DynaPid.

Definition at line 96 of file DynaPidSpeed.cxx.

References DynaPid::SpecifySetPoint().

 {
   DynaPid::SpecifySetPoint(fDpDt, bUnwind);
 }

double DynaPidSpeed::toOutput ( )

protectedvirtual

Convert control variable to raw servo speed.

Returns: Output.

Definition at line 135 of file DynaPidSpeed.cxx.

 {
   if( m_fCV < (double)m_nSpeedMin )
   {
     return (double)m_nSpeedMin;
   }
   else if( m_fCV > (double)m_nSpeedMax )
   {
     return (double)m_nSpeedMax;
   }
   else
   {
     return m_fCV;
   }
 }

virtual double DynaPidSpeed::toPV	(	uint_t	uCurPos,
		double	dt
	)

inlineprotectedvirtual

Save current position and convert to dp/dt process variable.

Parameters

uCurPos	Current servo position.
dt	Delta time between previous and current positions (seconds).

Returns: Process variable (PV).

Definition at line 242 of file DynaPidSpeed.h.

   {
     // age
     m_nPos[1] = m_nPos[0];
     m_nPos[0] = (int)uCurPos;
 
     return CalcDpDt(m_nPos[0], m_nPos[1], dt);
   }

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

include/Dynamixel/DynaPidSpeed.h
libDynamixel/DynaPidSpeed.cxx

Public Member Functions

Static Public Attributes

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation