private/hsb
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Updated HW test items for SWo

Browse Source 
git-svn-id: https://svn.vbchaos.nl/svn/hsb/trunk@248 05563f52-14a8-4384-a975-3d1654cca0fa
This commit is contained in:
mmi
2017-10-11 07:19:14 +00:00
parent b32a1c107f
commit 129f76f19b
10 changed files with 179 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/src/PID.c
View File

@@ -27,6 +27,8 @@
#include "PID.h"
#include "Logger.h"
// -----------------------------------------------------------------------------
// Constant and macro definitions
// -----------------------------------------------------------------------------
@@ -55,17 +57,29 @@
// -----------------------------------------------------------------------------
ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd)
ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd, int iMin, int iMax)
{
ErrorStatus returnValue = SUCCESS;
if (!self->initialized)
{
self->Kd = Kd;
self->Ki = Ki;
self->Kp = Kp;
self->input_d1 = 0;
self->initialized = true;
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
{
@@ -75,18 +89,45 @@ ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd)
}
int PID_calculate(struct Pid* self, int error)
int PID_calculate(struct Pid* self, int input, int error)
{
int returnValue = 0;
int dTerm;
int pTerm;
input *= PID_FIXED_POINT_FACTOR;
error *= PID_FIXED_POINT_FACTOR;
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 = (input - self->input_d1) * self->Kd;
// Calculate proportional
pTerm = self->Kp * error;
returnValue = (self->iTerm + dTerm + pTerm) / PID_FIXED_POINT_FACTOR;
self->input_d1 = input;
}
else
{
returnValue = 0;
}
return returnValue;
}