blob: eac1f605a2e09874db5188873784a2a13fbdd853 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
|
#include "PIDAlgo.h"
float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
{
float error;
float derivative;
float output = 0;
//Calculate P,I,D
error = _setPoint - _mesuredParam;
//In case of error too small then stop integration
if(fabs(error) > params->epsilon)
{
*_integral = *_integral + error*params->dt;
}
derivative = (error - *_pre_error)/params->dt;
output = params->Kp*error/params->ProportionalErrorMultiplier + params->Ki**_integral/params->IntegralErrorMultiplier + params->Kd*derivative;
//}
//Saturation Filter
if(output > params->MAX)
{
output = params->MAX;
}
else if(output < params->MIN)
{
output = params->MIN;
}
//Update error
*_pre_error = error;
return output;
}
/*
float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
{
float error;
float derivative;
float output;
//double error = *mySetpoint - input;
error = _setPoint - _mesuredParam;
//ITerm+= (ki * error);
*_integral = *_integral + (error*params->Ki);
/`* if(ITerm > outMax)
{
ITerm= outMax;
}
else if(ITerm < outMin)
{
ITerm= outMin;
} *`/
if(*_integral > params->MAX)
{
*_integral = params->MAX;
}
else if(*_integral < params->MIN)
{
*_integral = params->MIN;
}
// double dInput = (input - lastInput);
derivative = error - *_pre_error;
/`*Compute PID Output*`/
// double output = kp * error + ITerm- kd * dInput;
output = params->Kp*error/params->ProportionalErrorMultiplier + *_integral/params->IntegralErrorMultiplier + params->Kd*derivative;
//Saturation Filter
if(output > params->MAX)
{
output = params->MAX;
}
else if(output < params->MIN)
{
output = params->MIN;
}
//Update error
*_pre_error = error;
return output;
}
*/
|
