232 lines
5.9 KiB
C++
232 lines
5.9 KiB
C++
///////////////////////////////////////////////////////////////////////
|
|
// PIDContoller.h
|
|
//
|
|
// (c) 2002 IDV, Inc.
|
|
//
|
|
// This file contains a simple discrete PID contoller class.
|
|
//
|
|
//
|
|
// *** INTERACTIVE DATA VISUALIZATION (IDV) PROPRIETARY INFORMATION ***
|
|
//
|
|
// This software is supplied under the terms of a license agreement or
|
|
// nondisclosure agreement with Interactive Data Visualization and may
|
|
// not be copied or disclosed except in accordance with the terms of
|
|
// that agreement.
|
|
//
|
|
// Copyright (c) 2001-2002 IDV, Inc.
|
|
// All Rights Reserved.
|
|
//
|
|
// IDV, Inc.
|
|
// 1233 Washington St. Suite 610
|
|
// Columbia, SC 29201
|
|
// Voice: (803) 799-1699
|
|
// Fax: (803) 931-0320
|
|
// Web: http://www.idvinc.com
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// Preprocessor
|
|
|
|
#ifndef _3DRENDERER_PIDCONTROLLER_H_
|
|
#define _3DRENDERER_PIDCONTROLLER_H_
|
|
|
|
#include <float.h>
|
|
#include <math.h>
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// Constants
|
|
|
|
const double c_dMaxAllowableDeltaTime = 0.03; // if more time (in seconds) than this has passed, no PID adjustments will be made
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// CPIDController
|
|
|
|
class CPIDController
|
|
{
|
|
public:
|
|
CPIDController( ) :
|
|
m_dValue(0.0),
|
|
m_dWantedValue(0.0),
|
|
m_dPConst(1.0),
|
|
m_dIConst(0.0),
|
|
m_dDConst(0.0),
|
|
m_dRunningError(0.0),
|
|
m_dLastDelta(0.0),
|
|
m_dLastError(0.0),
|
|
m_bValidError(false),
|
|
m_dMaxAcceleration(0.0),
|
|
m_dLastDeltaTime(0.0)
|
|
{
|
|
}
|
|
|
|
void SetValue(double dValue);
|
|
void SetWantedValue(double dWantedValue);
|
|
void SetConstants(double dPConst, double dIConst, double dDConst, double dMaxAcceleration = 0.0);
|
|
|
|
double GetValue( ) const;
|
|
double GetWantedValue( ) const;
|
|
double GetLastError( ) const;
|
|
double GetLastDelta( ) const;
|
|
double GetValue(double dDeltaTime);
|
|
|
|
void ResetError(void);
|
|
|
|
void SetMaxAcceleration(double dValue) { m_dMaxAcceleration = dValue; }
|
|
|
|
private:
|
|
double m_dValue; // current value of the controller
|
|
double m_dWantedValue; // the value the controller is trying to achieve
|
|
|
|
double m_dPConst; // proportional constant (Kp)
|
|
double m_dIConst; // integral constant (Ki)
|
|
double m_dDConst; // derivative constant (Kd)
|
|
double m_dMaxAcceleration; // limits how fast the control can accelerate the value
|
|
|
|
double m_dLastError; // previous error
|
|
double m_dLastDelta; // amout of change during last adjustment
|
|
double m_dRunningError; // summed errors (using as the integral value)
|
|
bool m_bValidError; // prevents numerical problems on the first adjustment
|
|
|
|
double m_dLastDeltaTime;
|
|
};
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// CPIDController::SetValue
|
|
|
|
inline void CPIDController::SetValue(double dValue)
|
|
{
|
|
m_dValue = dValue;
|
|
m_dLastError = 0.0;
|
|
m_dLastDelta = 0.0;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// CPIDController::SetWantedValue
|
|
|
|
inline void CPIDController::SetWantedValue(double dWantedValue)
|
|
{
|
|
m_dWantedValue = dWantedValue;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// CPIDController::SetConstants
|
|
|
|
inline void CPIDController::SetConstants(double dPConst, double dIConst, double dDConst, double dAcceleration)
|
|
{
|
|
m_dPConst = dPConst;
|
|
m_dIConst = dIConst;
|
|
m_dDConst = dDConst;
|
|
m_dMaxAcceleration = dAcceleration;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// CPIDController::GetValue
|
|
|
|
inline double CPIDController::GetValue( ) const
|
|
{
|
|
return m_dValue;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// CPIDController::GetLastError
|
|
|
|
inline double CPIDController::GetLastError( ) const
|
|
{
|
|
return m_dLastError;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// CPIDController::GetWantedValue
|
|
|
|
inline double CPIDController::GetWantedValue( ) const
|
|
{
|
|
return m_dWantedValue;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// CPIDController::GetLastDelta
|
|
|
|
inline double CPIDController::GetLastDelta( ) const
|
|
{
|
|
return m_dLastDelta;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// CPIDController::GetValue
|
|
|
|
inline double CPIDController::GetValue(double dDeltaTime)
|
|
{
|
|
// if too much time has passed, do nothing
|
|
if (dDeltaTime == 0.0f)
|
|
return m_dValue;
|
|
else if (dDeltaTime > c_dMaxAllowableDeltaTime)
|
|
dDeltaTime = c_dMaxAllowableDeltaTime;
|
|
|
|
// compute the error and sum of the errors for the integral
|
|
double dError = (m_dWantedValue - m_dValue) * dDeltaTime;
|
|
m_dRunningError += dError;
|
|
|
|
// proportional
|
|
double dP = m_dPConst * dError;
|
|
|
|
// integral
|
|
double dI = m_dIConst * m_dRunningError * dDeltaTime;
|
|
|
|
// derivative
|
|
double dD(0.0f);
|
|
if (m_bValidError)
|
|
dD = m_dDConst * (m_dLastError - dError) * dDeltaTime;
|
|
else
|
|
m_bValidError = true;
|
|
|
|
// remember the error for derivative
|
|
m_dLastError = dError;
|
|
|
|
// compute the adjustment
|
|
double dThisDelta = dP + dI + dD;
|
|
|
|
// clamp the acceleration
|
|
if (m_dMaxAcceleration != 0.0f || false)
|
|
{
|
|
double dTimeRatio(1.0);
|
|
if (m_dLastDeltaTime != 0.0)
|
|
dTimeRatio = dDeltaTime / m_dLastDeltaTime;
|
|
m_dLastDeltaTime = dDeltaTime;
|
|
|
|
m_dLastDelta *= dTimeRatio;
|
|
double dDifference = (dThisDelta - m_dLastDelta);
|
|
double dAccel = m_dMaxAcceleration * dDeltaTime * dDeltaTime;
|
|
|
|
if (dDifference < -dAccel)
|
|
dThisDelta = m_dLastDelta - dAccel;
|
|
else if (dDifference > dAccel)
|
|
dThisDelta = m_dLastDelta + dAccel;
|
|
}
|
|
|
|
// modify the value
|
|
m_dValue += dThisDelta;
|
|
m_dLastDelta = dThisDelta;
|
|
|
|
return m_dValue;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////////////////////////
|
|
// CPIDController::ResetError
|
|
|
|
inline void CPIDController::ResetError(void)
|
|
{
|
|
m_dRunningError = 0.0f;
|
|
m_bValidError = false;
|
|
}
|
|
|
|
#endif // _3DRENDERER_PIDCONTROLLER_H_
|