Diff: main.cpp

Revision:

0:f4d8c80475a0

Child:

1:e2066df0a7a6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Aug 25 14:22:44 2017 +0000
@@ -0,0 +1,187 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "USBSerial.h"
+#include "PTC3471.h"
+
+#define Ts 0.01                                                                 //periodo de amostragem
+#define pi 3.14159
+
+/******************************************************************************/
+/****************  Definição de Variaveis, Objetos e Funções ******************/
+/******************************************************************************/
+
+USBSerial  pc;                                                                  // Objeto de comunicação serial com o TeraTerm
+
+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_Pendulo (PTA12,PTA13,NC, 500, 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(PTD6);                                                   // DigitalOut que sinaliza se deve virar o motor no sentido anti-horário
+PwmOut Motor(PTD5);                                                             // AnalogOut (PWM) que indica de 0 a 1 qual o módulo da tensão sobre o motor
+
+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 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; 
+
+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;
+                                                                                
+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 *********************************/
+/******************************************************************************/
+
+/*************************** Função Principal *********************************/
+// A main chama todas as inicializações e então aguarda o sinal de que deve
+// realizar controle. Esse sinal é dado pela flag "Controle" e é setada por uma
+// interrupção de tempo.
+//
+// Para garantir a execução imediata do algoritmo de controle nenhum wait deve
+// ser chamado durante a execução do controle e o uso de printfs deve ser 
+// esporádico.
+int main() {
+
+    /*********************************************************************************/
+    /** Inicialização do algoritmo de proteção. NUNCA DEVE SER RETIRADO DO PROGRAMA **/
+    /**/                                wait(5);                                   /**/
+    /**/         Protecao_Init(&Encoder_Motor, &Control_Interrupt, pi/2);        /**/
+    /** Inicialização do algoritmo de proteção. NUNCA DEVE SER RETIRADO DO PROGRAMA **/
+    /*********************************************************************************/
+    
+    Init();
+    while(1) {
+        
+        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
+                
+                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("Theta_0: %f\n\r", th0*180/pi);
+                pc.printf("Theta_1: %f\n\r", th1*180/pi);
+                
+            }
+            
+            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
+        }
+    }
+}
+
+/************** 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;  
+    
+    u=0;
+
+
+    
+    if(u>1)
+        u=1;
+    if(u<-1)
+        u=-1;
+    
+    if(u<0){
+        Motor = -u;
+        Horario = 0;
+        AntiHorario = 1;
+    }
+    else if(u>0){
+        Motor = u;
+        Horario = 1;
+        AntiHorario = 0;
+    }
+    else{
+        Motor = 0;
+        Horario = 0;
+        AntiHorario = 0;
+    }
+    
+}
+
+/************************* Função de Inicialização *****************************/
+// Esta função concentra todas as inicializações do sistema
+void Init(void){
+    
+    Motor.period(0.0001);
+    Horario = 0;
+    AntiHorario = 0;
+    Motor = 0.0;
+    Control_Interrupt.attach(&Control_Function, Ts);
+   
+}
+
+/********************** Função de leitura dos sensores *************************/
+// Cada vez que esta função é chamada deve-se calcular os ângulos e velocidades
+// angulares por algum método conhecido
+void Sensor_Read(void){
+    
+    th0_a=th0;                                                    
+    th1_a=th1;                                                    
+    
+    /** Leituras cruas dos ângulos do encoder **/
+    phi0 = pi*Encoder_Motor.getPulses()/600.0;                                   // (pulsos_lidos/pulsos_por_volta)*pi = angulo_em_radianos
+    phi1 = pi*Encoder_Pendulo.getPulses()/1000.0;                                // (pulsos_lidos/pulsos_por_volta)*180 = angulo_em_graus
+    
+    th0 = phi0;
+    /** Tratamento do ângulo lido para ser zero na vertical para cima **/       // Como o encoder é incremental quando inicializamos o programa com o pêndulo na posição
+    if(phi1>0)                                                                   // vertical para baixo esta passa a ser lida como 0º. Porém, para o algoritmo de controle
+        th1 = phi1-pi;                                                           // funcionar corretamente 0º deve ser o pêndulo na posição vertical para cima. Para 
+                                                                                // 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
+    
+    // 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;    
+    
+}
+
+/**************** Função de flag do algoritmo de controle ******************/
+// Esta função avisa a main quando executar o próximo passo do algoritmo de 
+// 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