Laboratório Controle Aplicado
/
Template_PTC3471_Geral_2020_V2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed QEI PTC3471 USBDevice
Diff: main.cpp
- Revision:
- 9:0882a5d7c0a3
- Parent:
- 8:2980205ae631
--- a/main.cpp Fri Nov 20 16:32:25 2020 +0000
+++ b/main.cpp Fri Nov 20 16:37:15 2020 +0000
@@ -27,7 +27,7 @@
double phi0 = 0; // phi0 -> Ângulo lido pelo Encoder_Braco
double phi1 = 0; // phi1 -> Ângulo lido pelo Encoder_Pendulo
-int16_t phi0_int = 0, phi1_int = 0; // Variáveis convertida para inteiro para ser
+int16_t phi0_int = 0, phi1_int = 0; // Variáveis convertida para inteiro para ser
// transmitida via serial (economia de banda)
double th0 = 0; // th0 -> Ângulo do braço
@@ -40,7 +40,7 @@
double dth0_f = 0; // dth0 -> Velocidade do braço c/ filtro
double dth1_f = 0; // dth1 -> Velocidade do pêndulo c/ filtro
-int16_t th0_f_int = 0, th1_f_int = 0; // Variável convertida para inteiro para ser
+int16_t th0_f_int = 0, th1_f_int = 0; // Variável convertida para inteiro para ser
int16_t dth0_f_int = 0, dth1_f_int = 0, u_int = 0; // transmitida via serial (economia de banda)
@@ -49,14 +49,15 @@
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 u = 0;
float K[4] = {0.0, 0.0, 0.0, 0.0};
float t_end = 30.0; // Duração do Experimento
-float tempo=0; // acumula o tempo de execução do experimento
+float tempo = 0; // Acumula o tempo de execução do experimento
+ // Esta variável pode ser usada para temporização da referência
-int N_plot = (int) t_end/(Ts*10); //Numero de amostras a ser armazenada para plot
-
+int N_plot = (int) t_end/(Ts*10); // Numero de amostras a ser armazenadas para plot
+
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
@@ -72,14 +73,14 @@
// 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
+// ser chamado durante a execução do controle e o uso de printfs deve ser
// esporádico.
-int main() {
-
-
- int ap = 0; // Indice dos vetores da dado
- int16_t th0_f_int[N_plot], th1_f_int[N_plot]; // Vetores para armazenar dados e
- int16_t dth0_f_int[N_plot], dth1_f_int[N_plot], u_int[N_plot]; // serem transmitidos via serial
+int main()
+{
+ int ap = 0; // Indice dos vetores de amostras
+ int ii = 0; // Indice para plot das amostras
+ int16_t th0_f_int[N_plot], th1_f_int[N_plot]; // Vetores para armazenar dados e
+ int16_t dth0_f_int[N_plot], dth1_f_int[N_plot], u_int[N_plot]; // serem transmitidos via serial
/*********************************************************************************/
/** Inicialização do algoritmo de proteção. NUNCA DEVE SER RETIRADO DO PROGRAMA **/
@@ -87,18 +88,18 @@
/**/ Protecao_Init(&Encoder_Motor, &Control_Interrupt, pi); /**/
/** Inicialização do algoritmo de proteção. NUNCA DEVE SER RETIRADO DO PROGRAMA **/
/*********************************************************************************/
-
+
Init();
while(1) {
-
- if(Flag_Controle){
-
+
+ if(Flag_Controle) {
+
Sensor_Read(); // Executa a leitura dos sensores
Controle_Algoritmo(); // Execução do seu algoritmo de controle
-
+
PlotCount++;
- if(PlotCount>=10){ // 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
+
// As variáveis serão multiplicadas por 1000 e convertidas para inteiro
// antes de serem trasmitidas. Ao receber, deve-se dividir por 1000 antes
// de fazer o plot. OBS: a precisão no gráfico será de 3 casas decimais
@@ -107,24 +108,23 @@
dth0_f_int[ap] = dth0_f*1000;
dth1_f_int[ap] = dth1_f*1000;
u_int[ap] = u*1000;
-
- ap = ap+1;
+
+ ap = ap + 1; // Prepara para a próxima amostra
PlotCount = 0;
- }
+ }
// Após t_end segundos, o experimento é interrompido e os dados são transmitidos via serial
if (tempo >= t_end) {
- int ii;
Control_Interrupt.detach();
Motor = 0;
Horario = 0;
AntiHorario = 0;
for (ii=0; ii<N_plot; ii++)
pc.printf("%d \t %d \t %d \t %d \t %d\n\r", th0_f_int[ii], th1_f_int[ii], dth0_f_int[ii], dth1_f_int[ii], u_int[ii]);
-
+
}
-
- 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
+
+ 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
}
}
}
@@ -133,18 +133,19 @@
// 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
+// tante não definida sinta-se livre para passa-las como argumento, definir
// como variavel global ou com um #define
-void Controle_Algoritmo(void){
-
+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;
-
+ dth1_f = (tau/(Ts+tau))*dth1_f + (Ts/(Ts+tau))*dth1;
+
/** Inserir Calculo do Sinal de Controle **/
u=-(K[0]*th0_f + K[1]*th1_f + K[2]*dth0_f + K[3]*dth1_f);
@@ -152,70 +153,71 @@
u=1;
if(u<-1)
u=-1;
-
- if(u<0){
+
+ if(u<0) {
Motor = -u;
Horario = 1;
AntiHorario = 0;
- }
- else if(u>0){
+ } else if(u>0) {
Motor = u;
Horario = 0;
AntiHorario = 1;
- }
- else{
+ } 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){
-
+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;
-
+void Sensor_Read(void)
+{
+
+ th0_a=th0;
+ th1_a=th1;
+
/** Leituras cruas dos ângulos do encoder **/
phi0 = Pi*Encoder_Motor.getPulses()/600.0; // (eventos_lidos/eventos_por_volta)*2*pi = angulo_em_radianos
phi1 = Pi*Encoder_Pendulo.getPulses()/1200.0; // (eventos_lidos/eventos_por_volta)*360 = 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
+ /** 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
+ 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;
-
+ 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
+// 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){
-
+void Control_Function(void)
+{
+
Flag_Controle = true;
- tempo = tempo+Ts;
+ tempo = tempo+Ts;
}
\ No newline at end of file