Diff: main.cpp

Revision:

7:e7cd972393d8

Parent:

6:f3fae203d0a7

--- a/main.cpp	Fri Sep 27 18:42:24 2019 +0000
+++ b/main.cpp	Fri Nov 08 18:12:59 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,16 +57,20 @@
 double th1_a = 0;                                                               // Valor de th1 um período de amostragem anterior
 
 float  u=0;
-float K1=-0.2667;
-float K2=-3.02;
-float K3=-0.1952;
-float K4=-0.3465;
-float Ki =-0.0015;
+//1)float Kc[6] = {-8.543577854910, -19.208421295822, 11.138274014683, 21.964973574347, -2.777217871809, -5.293135981137};
+//1)float Ki=-1.683953e-03;
+
+float Kc[6] = {-8.273809779163, -18.951802594041, 10.792122644776, 21.665142029349, -2.691366775102, -5.220463329945};
+float Ki=-1.686729e-03;
+
+//float Kc[6] = {-7.630529531494, -18.360961356689, 9.976207896704, 20.974809690121, -2.489669704209, -5.053156629986};
+//float Ki=-7.569496e-04;
+
 float v = 0;
 float vNext = 0;
 float rBraco = 0;
-float refBracoVal = 0.3;
-
+float refBracoVal = 45*(pi/180);
+//float refBracoVal = 0.6
                                                                                 
 void Init(void);                                                                // Função de Inicialização
 void Control_Function(void);                                                    // Função de flag do controle, a ser chamada pela interrupção
@@ -94,13 +102,15 @@
     while(1) {
         
         if(Flag_Controle){
-            if (countInterrupt>=1000){
-                rBraco=refBracoVal;
+            if (countInterrupt>=600 && countInterrupt < 1200){
+                rBraco = refBracoVal;
             }
-            if (countInterrupt>=2000){
-                rBraco=-refBracoVal;
+            else if (countInterrupt>=1200){
+                rBraco = -1*refBracoVal;
                 countInterrupt = 0;
             }
+
+//            rBraco = refBracoVal;
             Sensor_Read();                                                      // Executa a leitura dos sensores
             Controle_Algoritmo();                                               // Execução do seu algoritmo de controle
             
@@ -120,7 +130,6 @@
                 //pc.printf("%d\n\r", phi1_p);
                 //pc.printf("Theta_1: %d, dTheta_1: %d\n\r", th1_p, dth1_p);
                 //pc.printf("U: %d\n\r", u_p);
-                
             }
             
             Flag_Controle = false;                                                   // Sinaliza que deve-se esperar o próximo sinal da interrupção de tempo para executar o próximo passo de controle
@@ -135,18 +144,18 @@
 // tante não definida sinta-se livre para passa-las como argumento, definir 
 // como variavel global ou com um #define
 void Controle_Algoritmo(void){
-    
-    dth0 = (th0-th0_a)/Ts;                                               // Calculo das velocidades por backward
-    dth1 = (th1-th1_a)/Ts;                                               // É interessante propor outro método
-
+//    
+//    dth0 = (th0-th0_a)/Ts;                                               // Calculo das velocidades por backward
+//    dth1 = (th1-th1_a)/Ts;                                               // É interessante propor outro método
+//
 
     // Filtro (1/tau*s +1) nos derivadas
-    dth0_f = (tau/(Ts+tau))*dth0_f + (Ts/(Ts+tau))*dth0;
-    dth1_f = (tau/(Ts+tau))*dth1_f + (Ts/(Ts+tau))*dth1;  
+//    dth0_f = (tau/(Ts+tau))*dth0_f + (Ts/(Ts+tau))*dth0;
+//    dth1_f = (tau/(Ts+tau))*dth1_f + (Ts/(Ts+tau))*dth1;  
     
     /** Inserir Calculo do Sinal de Controle **/
     v = vNext;
-    u = -(K1*th0_f + K2*th1_f + K3*dth0_f + K4*dth1_f + Ki*v);
+    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) + Ki*v;
     vNext = v + rBraco - th0_f;
     //u=0;
 
@@ -204,6 +213,12 @@
                                                                                 // garantir que isso aconteça subido o pêndulo no sentido horário ou anti-horário fazemos
     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;
@@ -216,7 +231,6 @@
 // controle. O uso de uma interrupção para o acionamento da flag garante que
 // haja exatamente Ts segundos entre execuções.
 void Control_Function(void){
-    countInterrupt=countInterrupt+1;
+    countInterrupt +=1;
     Flag_Controle = true;
-    
 }
\ No newline at end of file