-
Notifications
You must be signed in to change notification settings - Fork 1
/
regulador.cpp
93 lines (84 loc) · 3.27 KB
/
regulador.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
#include "regulador.h"
Regulador::Regulador(double periodo, double ganancia, FDT *funcion, Conversor *conv, VariablesCompartidas *var)
{
// Guardar datos
T=periodo;
Kp=ganancia;
fdt=funcion;
conversor=conv;
varcomp=var;
pthread_mutex_init(&mutKp, NULL);
}
double Regulador::Calc_uk(double ek)
{
double salida_fdt = fdt->Calc(ek);
pthread_mutex_lock(&mutKp);
double uk = salida_fdt * Kp; // Kp se modifica por pantalla, proteger
pthread_mutex_unlock(&mutKp);
return uk;
}
double Regulador::Get_Kp()
{
double ganancia;
pthread_mutex_lock(&mutKp);
ganancia=Kp;
pthread_mutex_unlock(&mutKp);
return ganancia;
}
void Regulador::Set_Kp(double ganancia)
{
pthread_mutex_lock(&mutKp);
Kp=ganancia;
pthread_mutex_unlock(&mutKp);
}
double Regulador::Get_T()
{
return T;
}
void Regulador::Suma_timespec(struct timespec *t1, struct timespec *t2)
{
t1->tv_nsec += t2->tv_nsec;
t1->tv_sec += t2->tv_sec;
while(t1->tv_nsec >= 1e9) // Si desbordan los nanosegundos
{
t1->tv_nsec -= 1e9; // Reseteo nanosegundos
t1->tv_sec++; // Incrementar segundos
}
}
void Regulador::HiloRegulador()
{
struct timespec t, tperiodo; // Estructuras para guardar tiempo
int DR; // Registro de datos que se obtiene del conversor
double yk; // Valor de la salida real
tperiodo.tv_nsec=T*1e9; // Guardar el tiempo de periodo que siempre es fijo
tperiodo.tv_sec=0;
while(tperiodo.tv_nsec >= 1e9) // Si desbordan los nanosegundos
{
tperiodo.tv_nsec -= 1e9; // Reseteo nanosegundos
tperiodo.tv_sec++; // Incrementar segundos
}
while(1) // Bucle de ejecución continua.
{ // Tarea periodica pura:
clock_gettime(CLOCK_REALTIME,&t); // 1) Captura de tiempo actual
Suma_timespec(&t,&tperiodo); // 2) Suma al tiempo actual un periodo fijo
if(T==PERIODO_REGULADOR_MOTOR) // Comprobar si se trata del Reg. motor o Reg. crucero
{
conversor->Read(CH_MOTOR, &DR); // 3) Leer del conversor canal motor
// Cambiar la decodificación para complemento a 2
yk = (DR*(conversor->Get_Vmax()-conversor->Get_Vmin())/pow(2,conversor->Get_Resolucion()))
/(conversor->s_ch0->Get_Ganancia()*conversor->s_ch0->Get_Ratio());
double ek = varcomp->Get_r() - yk; // Obtener referencia y calcular ek
varcomp->Set_uk(Calc_uk(ek)); // Calcular uk a partir de ek y guardar
}
else
{
conversor->Read(CH_CRUCERO, &DR); // Leer del conversor canal crucero
yk = (DR*(conversor->Get_Vmax()-conversor->Get_Vmin())/pow(2,conversor->Get_Resolucion()))
/(conversor->s_ch1->Get_Ganancia()*conversor->s_ch1->Get_Ratio());
double ek = varcomp->Get_r() - yk; // Obtener referencia y calcular ek
varcomp->Set_uk(Calc_uk(ek)); // Calcular uk a partir de ek y guardar
}
// 4) Hacer retardo. De esta forma queda retardo absoluto
clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME,&t,NULL);
}
}