Diff: main.cpp

Revision:

3:933fafe31262

Parent:

2:61c074362ef1

Child:

4:0a1ba647bce7

--- a/main.cpp	Thu Aug 16 13:40:52 2018 +0000
+++ b/main.cpp	Fri Aug 23 18:05:56 2019 +0000
@@ -38,12 +38,22 @@
 double dth0_f = 0;                                                                // dth0 -> Velocidade do braço
 double dth1_f = 0;                                                                // dth1 -> Velocidade do pêndulo
 
+int th0_p = 0;                                                                 // th0 -> Ângulo do braço
+int th1_p = 0;                                                                 // th1 -> Ângulo do pêndulo
+int dth0_p = 0;                                                                // dth0 -> Velocidade do braço
+int dth1_p = 0;
+int u_p = 0;
+
 double tau = 5e-2; 
 
 double th0_a = 0;                                                               // Valor de th0 um período de amostragem anterior
 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;
                                                                                 
 void Init(void);                                                                // Função de Inicialização
 void Control_Function(void);                                                    // Função de flag do controle, a ser chamada pela interrupção
@@ -67,7 +77,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 **/
     /*********************************************************************************/
     
@@ -80,11 +90,18 @@
             Controle_Algoritmo();                                               // Execução do seu algoritmo de controle
             
             PlotCount++;
-            if(PlotCount>=100){                                                  // Controla para que o printf ocorra apenas uma vez a cada 10 iterações
-                
+            if(PlotCount>=10){
+                                                                  // Controla para que o printf ocorra apenas uma vez a cada 10 iterações
+                th0_p = th0*1000;                                                                 // th0 -> Ângulo do braço
+                th1_p = th1*1000;                                                                 // th1 -> Ângulo do pêndulo
+                dth0_p = dth0*1000;                                                                // dth0 -> Velocidade do braço
+                dth1_p = dth1*1000;
+                u_p = u * 1000;
+                 
                 PlotCount = 0;
-                pc.printf("Theta_0: %f, dTheta_0: %f\n\r", th0*180/pi, dth0*180/pi);
-                pc.printf("Theta_1: %f, dTheta_1: %f\n\r", th1*180/pi, dth1*180/pi);
+                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("Theta_1: %d, dTheta_1: %d\n\r", th1_p, dth1_p);
+                //pc.printf("U: %d\n\r", u_p);
                 
             }
             
@@ -110,8 +127,8 @@
     dth1_f = (tau/(Ts+tau))*dth1_f + (Ts/(Ts+tau))*dth1;  
     
     /** Inserir Calculo do Sinal de Controle **/
-    u = 0;
-
+    //u = -(K1*th0_f + K2*th1_f + K3*dth0_f + K4*dth1_f);
+    u=0.1;
 
     if(u>1)
         u=1;