Position PID class. More...

#include <DynaPidPos.h>

Inheritance diagram for DynaPidPos:

Public Member Functions
	DynaPidPos (double fKp=0.5, double fKi=0.0, double fKd=0.005, double fWi=0.25)
	Default constructor. More...

	DynaPidPos (const DynaPid &src)
	Copy constructor.

virtual	~DynaPidPos ()
	Default destructor.

DynaPidPos &	operator= (const DynaPidPos &rhs)
	Assignment operator.

virtual void	SpecifySetPoint (int nOdPosStart, int nOdPosGoal, int nSpeedLim, int nCurSpeed, bool bIsReversed, bool bUnwind=false)
	Specify position setpoint. 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 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	TuneMaxSpeedDelta = 1000.0
	max speed change deltas More...

static const double	TuneAbsMinSpeed = 0.0
	absolute minimum speed

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
void	initParams ()
	Initialize parameters.

virtual double	error (double fPVodPosCur)
	Calculate error from goal postion setpoint and current position variable value. More...

virtual double	toOutput (double fCVSpeed)
	Convert control variable to application-specific output value. More...

Protected Attributes
int	m_nOdPosStart
	starting odometer position

int	m_nOdPosGoal
	goal ing odometer position (set point)

int	m_nSpeedLim
	speed limit (max neg/pos speed)

int	m_nOdDir
	odometer direction

int	m_nOdPosCur
	current odometer position

double	m_fSpeedAbsMax
	absolute speed limit >= 0

double	m_fPrevSpeed
	previous speed

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

Position PID class.

Definition at line 74 of file DynaPidPos.h.

Constructor & Destructor Documentation

DynaPidPos::DynaPidPos	(	double	fKp = `0.5`,
		double	fKi = `0.0`,
		double	fKd = `0.005`,
		double	fWi = `0.25`
	)

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 88 of file DynaPidPos.h.

                               :
     DynaPid(fKp, fKi, fKd, fWi)
   {
     initParams();
   }

Member Function Documentation

double DynaPidPos::error ( double fPVodPosCur )

protectedvirtual

Calculate error from goal postion setpoint and current position variable value.

Parameters

fPVOdPosCur Current odometer position process variable value.

Returns: Error.

Reimplemented from DynaPid.

Definition at line 91 of file DynaPidPos.cxx.

 {
   m_nOdPosCur = (int)fPVOdPosCur;
 
   return (double)(m_nOdPosGoal - m_nOdPosCur);
 }

void DynaPidPos::SpecifySetPoint	(	int	nOdPosStart,
		int	nOdPosGoal,
		int	nSpeedLim,
		int	nCurSpeed,
		bool	bIsReversed,
		bool	bUnwind = `false`
	)

virtual

Specify position 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

nOdPosStart	Starting position (odometer ticks).
nOdPosGoal	New goal position setpoint (odometer ticks).
nSpeedLim	Speed limit (raw).
nCurSpeed	Current servo speed (raw).
bIsReversed	Odometer is [not] reversed from encoder.
bUnwind	Do [not] reset controller integral sum of errors term

Definition at line 71 of file DynaPidPos.cxx.

References DynaPid::SpecifySetPoint().

Referenced by DynaBgThread::WriteGoalPos().

 { 
   DynaPid::SpecifySetPoint((double)nOdPosGoal, bUnwind);
 
   m_nOdPosStart   = nOdPosStart;
   m_nOdPosGoal    = nOdPosGoal;
   m_nSpeedLim     = nSpeedLim;
   m_nOdDir        = bIsReversed? -1: 1;
 
   m_nOdPosCur     = nOdPosStart;
 
   m_fSpeedAbsMax  = m_nSpeedLim >= 0? (double)m_nSpeedLim: -(double)m_nSpeedLim;
   m_fPrevSpeed    = (double)nCurSpeed;
 }

double DynaPidPos::toOutput ( double fCVSpeed )

protectedvirtual

Convert control variable to application-specific output value.

Parameters

fCVSpeed Speed control variable value.

Returns: Output.

Reimplemented from DynaPid.

Definition at line 98 of file DynaPidPos.cxx.

 {
   double  fSpeed;
 
   fSpeed = fCVSpeed * (double)m_nOdDir;
 
   //
   // Cap speed to the absolute maximum which is the target goal speed.
   //
   if( fSpeed > m_fSpeedAbsMax )
   {
     fSpeed = m_fSpeedAbsMax;
   }
   else if( fSpeed < -m_fSpeedAbsMax )
   {
     fSpeed = -m_fSpeedAbsMax;
   }
 
   //
   // If there is an absolute lower speed, then cap as necessary.
   //
   if( TuneAbsMinSpeed > 0.0 )
   {
     if( (fSpeed > 0.0) && (fSpeed < TuneAbsMinSpeed) )
     {
       fSpeed = TuneAbsMinSpeed;
     }
     else if( (fSpeed < 0.0) && (fSpeed > -TuneAbsMinSpeed) )
     {
       fSpeed = -TuneAbsMinSpeed;
     }
   }
 
   //
   // Cap the acceleration so that jerkiness and oscillations are minimized.
   //
   if( fabs(fSpeed-m_fPrevSpeed) > TuneMaxSpeedDelta )
   {
     if( m_fPrevSpeed > fSpeed )
     {
       fSpeed = m_fPrevSpeed - TuneMaxSpeedDelta;
     }
     else 
     {
       fSpeed = m_fPrevSpeed + TuneMaxSpeedDelta;
     }
   }
 
   m_fPrevSpeed = fSpeed;
 
   return fSpeed;
 }

Member Data Documentation

const double DynaPidPos::TuneMaxSpeedDelta = 1000.0

static

max speed change deltas

max speed chg deltas

Definition at line 77 of file DynaPidPos.h.

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

include/Dynamixel/DynaPidPos.h
libDynamixel/DynaPidPos.cxx

Public Member Functions

Static Public Attributes

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation