Diff: main.cpp

Revision:

3:8736b05d400a

Parent:

2:61c074362ef1

Child:

4:cccd118ea410

--- a/main.cpp	Thu Aug 16 13:40:52 2018 +0000
+++ b/main.cpp	Tue Sep 27 15:31:55 2022 +0000
@@ -4,7 +4,7 @@
 #include "PTC3471.h"
 
 #define Ts 0.01                                                                 //periodo de amostragem
-#define pi 3.14159
+#define pi 3.141592653589793
 
 /******************************************************************************/
 /****************  Definição de Variaveis, Objetos e Funções ******************/
@@ -14,29 +14,25 @@
 
 Ticker Control_Interrupt;                                                       // Interrupção de Tempo para acionamento do algoritmo de controle
 
-//QEI Encoder_Motor (PTD0,PTB17,NC, 300, QEI::X4_ENCODING);                       // Objeto de leitura do encoder do motor
-QEI Encoder_Motor (PTB17,PTD0,NC, 300, QEI::X4_ENCODING);                       // Objeto de leitura do encoder do motor
-QEI Encoder_Pendulo (PTA12,PTA13,NC, 600, QEI::X4_ENCODING);                    // Objeto de leitura do encoder do pêndulo
+QEI Encoder_Motor (PTD0,PTB17,NC, 300, QEI::X4_ENCODING);   // Objeto de leitura do encoder do motor
+QEI Encoder_Pendulo (PTA12,PTA13,NC, 600, QEI::X4_ENCODING);// Objeto de leitura do encoder do pêndulo
 
-DigitalOut Horario(PTC1);                                                       // DigitalOut que sinaliza se deve virar o motor no sentido horário
-DigitalOut AntiHorario(PTD5); //                                                // DigitalOut que sinaliza se deve virar o motor no sentido anti-horário
-PwmOut Motor(PTD6); //                                                          // AnalogOut (PWM) que indica de 0 a 1 qual o módulo da tensão sobre o motor
+DigitalOut Horario(PTC1);                                   // DigitalOut que sinaliza se deve virar o motor no sentido horário
+DigitalOut AntiHorario(PTD5);                               // DigitalOut que sinaliza se deve virar o motor no sentido anti-horário
+PwmOut Motor(PTD6);      
+
 
 bool Flag_Controle = false;
 int PlotCount = 0;
 
 double phi0 = 0;                                                                // phi0 -> Ângulo lido pelo Encoder_Braco
-double phi1 = 0;                                                                // phi1 -> Ângulo lido pelo Encoder_Pendulo
+double phi1 = 0;   
 
 double th0 = 0;                                                                 // th0 -> Ângulo do braço
 double th1 = 0;                                                                 // th1 -> Ângulo do pêndulo
-double dth0 = 0;                                                                // dth0 -> Velocidade do braço
-double dth1 = 0;                                                                // dth1 -> Velocidade do pêndulo
 
 double th0_f = 0;                                                                 // th0 -> Ângulo do braço
 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 tau = 5e-2; 
 
@@ -46,9 +42,7 @@
 float  u=0;
                                                                                 
 void Init(void);                                                                // Função de Inicialização
-void Control_Function(void);                                                    // Função de flag do controle, a ser chamada pela interrupção
 void Sensor_Read(void);                                                         // Função de leitura dos sensores
-void Controle_Algoritmo(void);                                                  // Função que implementa o algoritmo de controle escolhido
 
 /******************************************************************************/
 /*************************** Corpo de Funções *********************************/
@@ -77,15 +71,12 @@
         if(Flag_Controle){
             
             Sensor_Read();                                                      // Executa a leitura dos sensores
-            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>=5){                                                  // Controla para que o printf ocorra apenas uma vez a cada 10 iterações
                 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("%f \t %f\n\r", phi0, phi1);
+               
             }
             
             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
@@ -93,48 +84,6 @@
     }
 }
 
-/************** Função de implementação do algoritmo de controle **************/
-// Nesta função você deve escrever a implementação do algoritmo de controle es-
-// colhido e do algoritmo de estimação das velocidades.
-// Caso necessite acesso a alguma variavel não medida ou alguma cons-
-// 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
-
-
-    // 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;  
-    
-    /** Inserir Calculo do Sinal de Controle **/
-    u = 0;
-
-
-    if(u>1)
-        u=1;
-    if(u<-1)
-        u=-1;
-    
-    if(u<0){
-        Motor = -u;
-        Horario = 1;
-        AntiHorario = 0;
-    }
-    else if(u>0){
-        Motor = u;
-        Horario = 0;
-        AntiHorario = 1;
-    }
-    else{
-        Motor = 0;
-        Horario = 0;
-        AntiHorario = 0;
-    }
-    
-}
 
 /************************* Função de Inicialização *****************************/
 // Esta função concentra todas as inicializações do sistema
@@ -179,7 +128,5 @@
 // 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){
-    
-    Flag_Controle = true;
-    
-}
\ No newline at end of file
+     Flag_Controle = true;
+ }
\ No newline at end of file