e3ca058c96
- Re-located repairprocessrow information in dedicated object - added error conditions to repair row and added condition handling to repair process git-svn-id: https://svn.vbchaos.nl/svn/hsb/trunk@260 05563f52-14a8-4384-a975-3d1654cca0fa
137 lines
3.4 KiB
C
137 lines
3.4 KiB
C
// -----------------------------------------------------------------------------
|
|
/// @file PID.c
|
|
/// @brief Description
|
|
// -----------------------------------------------------------------------------
|
|
// Micro-Key bv
|
|
// Industrieweg 28, 9804 TG Noordhorn
|
|
// Postbus 92, 9800 AB Zuidhorn
|
|
// The Netherlands
|
|
// Tel: +31 594 503020
|
|
// Fax: +31 594 505825
|
|
// Email: support@microkey.nl
|
|
// Web: www.microkey.nl
|
|
// -----------------------------------------------------------------------------
|
|
/// $Revision$
|
|
/// $Author$
|
|
/// $Date$
|
|
// (c) 2017 Micro-Key bv
|
|
// -----------------------------------------------------------------------------
|
|
|
|
/// @file PID.c
|
|
/// @ingroup {group_name}
|
|
|
|
|
|
// -----------------------------------------------------------------------------
|
|
// Include files
|
|
// -----------------------------------------------------------------------------
|
|
|
|
#include "PID.h"
|
|
|
|
#include "Logger.h"
|
|
|
|
// -----------------------------------------------------------------------------
|
|
// Constant and macro definitions
|
|
// -----------------------------------------------------------------------------
|
|
|
|
|
|
|
|
// -----------------------------------------------------------------------------
|
|
// Type definitions
|
|
// -----------------------------------------------------------------------------
|
|
|
|
|
|
// -----------------------------------------------------------------------------
|
|
// File-scope variables
|
|
// -----------------------------------------------------------------------------
|
|
|
|
|
|
|
|
// -----------------------------------------------------------------------------
|
|
// Function declarations
|
|
// -----------------------------------------------------------------------------
|
|
|
|
|
|
|
|
// -----------------------------------------------------------------------------
|
|
// Function definitions
|
|
// -----------------------------------------------------------------------------
|
|
|
|
|
|
ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd, int iMin, int iMax)
|
|
{
|
|
ErrorStatus returnValue = SUCCESS;
|
|
|
|
if (!self->initialized)
|
|
{
|
|
|
|
if ((Kp >= 0) && (Ki >= 0) && (Kd >= 0))
|
|
{
|
|
self->iTerm = 0;
|
|
self->Kd = Kd;
|
|
self->Ki = Ki;
|
|
self->Kp = Kp;
|
|
self->input_d1 = 0;
|
|
self->iMin = iMin;
|
|
self->iMax = iMax;
|
|
|
|
self->initialized = true;
|
|
}
|
|
else
|
|
{
|
|
returnValue = ERROR;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
returnValue = ERROR;
|
|
}
|
|
return returnValue;
|
|
}
|
|
|
|
|
|
void PID_destruct(struct Pid* self)
|
|
{
|
|
self->initialized = false;
|
|
}
|
|
|
|
|
|
int PID_calculate(struct Pid* self, int error)
|
|
{
|
|
int returnValue = 0;
|
|
|
|
int dTerm;
|
|
int pTerm;
|
|
|
|
if (self->initialized)
|
|
{
|
|
// Calculate integral
|
|
self->iTerm += (self->Ki * error);
|
|
|
|
// Control integrator
|
|
if (self->iTerm > self->iMax)
|
|
{
|
|
self->iTerm = self->iMax;
|
|
}
|
|
else if(self->iTerm < self->iMin)
|
|
{
|
|
self->iTerm = self->iMin;
|
|
}
|
|
|
|
// Calculate differential
|
|
dTerm = ((error - self->input_d1) * self->Kd);
|
|
|
|
// Calculate proportional
|
|
pTerm = (self->Kp * error);
|
|
|
|
returnValue = (self->iTerm + dTerm + pTerm) / PID_FIXED_POINT_FACTOR;
|
|
self->input_d1 = error;
|
|
}
|
|
else
|
|
{
|
|
returnValue = 0;
|
|
}
|
|
|
|
return returnValue;
|
|
|
|
}
|