-
Notifications
You must be signed in to change notification settings - Fork 2
/
controller.cpp
103 lines (79 loc) · 1.92 KB
/
controller.cpp
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
88
89
90
91
92
93
94
95
96
97
98
99
100
#include "controller.h"
Controller::Controller()
{
erroAnt = 0;
integrador = 0;
tensaoAnt = 0;
vps = 0;
taw = 75;
}
double Controller::ganhoKp(double Kp, double erro)
{
P = erro * Kp;
return P;
}
double Controller::ganhoKi(double Ki, double Kd, double Kp, double erro)
{
this->integrador += erro * Ki * TEMPO_AMOSTRAGEM;
I = this->integrador;
taw = 75/sqrt(0.005/0.05) * sqrt(Kd/Ki);
if(isWindUp()){
I += (Kp/taw)*TEMPO_AMOSTRAGEM*(vps - tensaoAnt);
}
this->integrador = I;
return I;
}
double Controller::ganhoKd(double Kd, double erro)
{
double derivador = Kd * (erro-this->erroAnt) / TEMPO_AMOSTRAGEM;
this->erroAnt = erro;
D = derivador;
return D;
}
double Controller::controlerP(double Kp, double erro)
{
return ganhoKp(Kp, erro);
}
double Controller::controlerPI(double Kp, double Ki, double erro)
{
return ganhoKp(Kp, erro) + ganhoKi(Ki, 0, Kp, erro);
}
double Controller::controlerPD(double Kp, double Kd, double erro)
{
return ganhoKp(Kp, erro) + ganhoKd(Kd, erro);
}
double Controller::controlerPID(double Kp, double Ki, double Kd, double erro)
{
return ganhoKp(Kp, erro) + ganhoKi(Ki, Kd, Kp, erro) + ganhoKd(Kd, erro);
}
double Controller::controlerPI_D(double Kp, double Ki, double Kd, double erro, double amplitude)
{
return ganhoKp(Kp, erro) + ganhoKi(Ki, Kd, Kp, erro) + ganhoKd(Kd, amplitude);
}
double Controller::getP() const {
return P;
}
double Controller::getI() const {
return I;
}
double Controller::getD() const {
return D;
}
void Controller::setTensaoAnt(double tensao){
this->tensaoAnt = tensao;
}
double Controller::getTensaoAnt(){
return this->tensaoAnt;
}
void Controller::setWindUp(bool windup){
this->windup = windup;
}
bool Controller::isWindUp(){
return this->windup;
}
void Controller::setVPS(double vps){
this->vps = vps;
}
double Controller::getVPS(){
return this->vps;
}