Diff: main.cpp

Revision:

6:e4b279738a2e

Parent:

5:ca75f4b6300c

--- a/main.cpp	Fri Oct 04 18:32:31 2019 +0000
+++ b/main.cpp	Fri Nov 08 18:18:53 2019 +0000
@@ -3,7 +3,7 @@
 #include "USBSerial.h"
 #include "PTC3471.h"
 
-#define Ts 0.01                                                                 //periodo de amostragem
+#define Ts 0.025                                                                 //periodo de amostragem
 #define pi 3.14159
 
 /******************************************************************************/
@@ -37,6 +37,10 @@
 double th1_f = 0;                                                                 // th1 -> Ângulo do pêndulo
 double dth0_f = 0;                                                                // dth0 -> Velocidade do braço
 double dth1_f = 0;                                                                // dth1 -> Velocidade do pêndulo
+double th0_f_1 = 0;
+double th1_f_1 = 0;
+double th0_f_2 = 0;
+double th1_f_2 = 0;
 
 int th0_p = 0;                                                                 // th0 -> Ângulo do braço
 int th1_p = 0;                                                                 // th1 -> Ângulo do pêndulo
@@ -53,10 +57,7 @@
 double th1_a = 0;                                                               // Valor de th1 um período de amostragem anterior
 
 float  u=0;
-float K1=-8.379621e-02;
-float K2=-2.866980e+00;
-float K3=-1.458506e-01;
-float K4=-3.332907e-01;
+float Kc[6] = {-7.419878862059, -18.178566554476, 9.713794094247, 20.761715610881, -2.425140320655, -5.001521706675};
                                                                                 
 void Init(void);                                                                // Função de Inicialização
 void Control_Function(void);                                                    // Função de flag do controle, a ser chamada pela interrupção
@@ -133,8 +134,8 @@
     dth1_f = (tau/(Ts+tau))*dth1_f + (Ts/(Ts+tau))*dth1;  
     
     /** Inserir Calculo do Sinal de Controle **/
-    u = -(K1*th0_f + K2*th1_f + K3*dth0_f + K4*dth1_f);
-    //u=0;
+    u = -(Kc[0]*th0_f + Kc[1]*th1_f + Kc[2]*th0_f_1 + Kc[3]*th1_f_1 + Kc[4]*th0_f_2 + Kc[5]*th1_f_2);
+    //u=0
 
     if(u>1)
         u=1;
@@ -191,6 +192,12 @@
     else if(phi1<=0)                                                             // th1 = th1-sgn(th1)*pi, onde sgn(x) é o sinal de x.
         th1 = phi1+pi;                                                           // Para ficar mais claro o funcionamento destes "if else" plote o sinal de th1 no tera term
     
+    th0_f_2 = th0_f_1;
+    th1_f_2 = th1_f_1;
+
+    th0_f_1 = th0_f;
+    th1_f_1 = th1_f;
+    
     // Filtro (1/tau*s +1) nos angulos
     th0_f = (tau/(Ts+tau))*th0_f + (Ts/(Ts+tau))*th0;
     th1_f = (tau/(Ts+tau))*th1_f + (Ts/(Ts+tau))*th1;