Diff: main.cpp

Revision:

4:0a1ba647bce7

Parent:

3:933fafe31262

Child:

5:ca75f4b6300c

--- a/main.cpp	Fri Aug 23 18:05:56 2019 +0000
+++ b/main.cpp	Fri Aug 30 20:54:15 2019 +0000
@@ -42,6 +42,8 @@
 int th1_p = 0;                                                                 // th1 -> Ângulo do pêndulo
 int dth0_p = 0;                                                                // dth0 -> Velocidade do braço
 int dth1_p = 0;
+int phi0_p = 0;
+int phi1_p = 0;
 int u_p = 0;
 
 double tau = 5e-2; 
@@ -50,10 +52,10 @@
 double th1_a = 0;                                                               // Valor de th1 um período de amostragem anterior
 
 float  u=0;
-float K1 = -0.084;
-float K2 = -2.878;
-float K3 = -0.136;
-float K4 = -0.338;
+float K1=-1.494698e-01;
+float K2=-2.820671e+00;
+float K3=-1.629279e-01;
+float K4=-3.205845e-01;
                                                                                 
 void Init(void);                                                                // Função de Inicialização
 void Control_Function(void);                                                    // Função de flag do controle, a ser chamada pela interrupção
@@ -77,7 +79,7 @@
     /*********************************************************************************/
     /** Inicialização do algoritmo de proteção. NUNCA DEVE SER RETIRADO DO PROGRAMA **/
     /**/                                wait(5);                                   /**/
-    /**/     //    Protecao_Init(&Encoder_Motor, &Control_Interrupt, pi);        /**/
+    /**/         Protecao_Init(&Encoder_Motor, &Control_Interrupt, pi);            /**/
     /** Inicialização do algoritmo de proteção. NUNCA DEVE SER RETIRADO DO PROGRAMA **/
     /*********************************************************************************/
     
@@ -96,10 +98,13 @@
                 th1_p = th1*1000;                                                                 // th1 -> Ângulo do pêndulo
                 dth0_p = dth0*1000;                                                                // dth0 -> Velocidade do braço
                 dth1_p = dth1*1000;
+                phi0_p = phi0*1000;
+                phi1_p = phi1*1000;
                 u_p = u * 1000;
                  
                 PlotCount = 0;
                 pc.printf("%d\t%d\t%d\t%d\t%d\n\r", th0_p, dth0_p, th1_p, dth1_p, u_p);
+                //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);
                 
@@ -127,8 +132,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.1;
+    u = -(K1*th0_f + K2*th1_f + K3*dth0_f + K4*dth1_f);
+    //u=0;
 
     if(u>1)
         u=1;