Laboratório Controle Aplicado / Mbed 2 deprecated Template_PTC3471_Atrito_braco_2022

Versão 2022

Dependencies:   QEI USBDevice mbed

main.cpp@2:70151b51aba8, 2022-09-27 (annotated)

Committer:
lcaepusp
Date:
Tue Sep 27 16:48:42 2022 +0000
Revision:
2:70151b51aba8
Parent:
1:8ae261460176
Child:
3:0a6596c337e8
V2022 rev

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lcaepusp 0:5d7acae44d76 1 #include "mbed.h"
lcaepusp 0:5d7acae44d76 2 #include "QEI.h"
lcaepusp 0:5d7acae44d76 3 #include "USBSerial.h"
lcaepusp 0:5d7acae44d76 4 //#include "PTC3471.h"
lcaepusp 0:5d7acae44d76 5
lcaepusp 0:5d7acae44d76 6 #define Ts 0.01 //periodo de amostragem
lcaepusp 2:70151b51aba8 7 #define pi 3.141592653589793
lcaepusp 0:5d7acae44d76 8
lcaepusp 0:5d7acae44d76 9 /******************************************************************************/
lcaepusp 0:5d7acae44d76 10 /**************** Definição de Variaveis, Objetos e Funções ******************/
lcaepusp 0:5d7acae44d76 11 /******************************************************************************/
lcaepusp 0:5d7acae44d76 12
lcaepusp 0:5d7acae44d76 13 USBSerial pc; // Objeto de comunicação serial com o TeraTerm
lcaepusp 0:5d7acae44d76 14
lcaepusp 0:5d7acae44d76 15 Ticker Control_Interrupt; // Interrupção de Tempo para acionamento do algoritmo de controle
lcaepusp 0:5d7acae44d76 16
lcaepusp 2:70151b51aba8 17 QEI Encoder_Motor (PTD0,PTB17,NC, 300, QEI::X4_ENCODING); // Objeto de leitura do encoder do motor
lcaepusp 2:70151b51aba8 18 QEI Encoder_Pendulo (PTA12,PTA13,NC, 600, QEI::X4_ENCODING);// Objeto de leitura do encoder do pêndulo
lcaepusp 0:5d7acae44d76 19
lcaepusp 2:70151b51aba8 20 DigitalOut Horario(PTC1); // DigitalOut que sinaliza se deve virar o motor no sentido horário
lcaepusp 2:70151b51aba8 21 DigitalOut AntiHorario(PTD5); // DigitalOut que sinaliza se deve virar o motor no sentido anti-horário
lcaepusp 2:70151b51aba8 22 PwmOut Motor(PTD6); // D.C. do PWM [0, 1]: porcentagem de tensão sobre o motor
lcaepusp 0:5d7acae44d76 23
lcaepusp 0:5d7acae44d76 24 char c;
lcaepusp 0:5d7acae44d76 25 int gotRead = 0;
lcaepusp 0:5d7acae44d76 26 bool Flag_Controle = false;
lcaepusp 0:5d7acae44d76 27 int PlotCount = 0;
lcaepusp 0:5d7acae44d76 28
lcaepusp 0:5d7acae44d76 29 double phi0 = 0; // phi0 -> Ângulo lido pelo Encoder_Braco
lcaepusp 0:5d7acae44d76 30 double phi1 = 0; // phi1 -> Ângulo lido pelo Encoder_Pendulo
lcaepusp 0:5d7acae44d76 31
lcaepusp 0:5d7acae44d76 32 double th0 = 0; // th0 -> Ângulo do braço
lcaepusp 0:5d7acae44d76 33 double th1 = 0; // th1 -> Ângulo do pêndulo
lcaepusp 0:5d7acae44d76 34 double dth0 = 0; // dth0 -> Velocidade do braço
lcaepusp 0:5d7acae44d76 35 double dth1 = 0; // dth1 -> Velocidade do pêndulo
lcaepusp 0:5d7acae44d76 36
lcaepusp 0:5d7acae44d76 37 double th0_a = 0; // Valor de th0 um período de amostragem anterior
lcaepusp 0:5d7acae44d76 38 double th1_a = 0; // Valor de th1 um período de amostragem anterior
lcaepusp 0:5d7acae44d76 39
lcaepusp 0:5d7acae44d76 40 float u=0;
lcaepusp 0:5d7acae44d76 41
lcaepusp 0:5d7acae44d76 42 void Init(void); // Função de Inicialização
lcaepusp 0:5d7acae44d76 43 void Control_Function(void); // Função de flag do controle, a ser chamada pela interrupção
lcaepusp 0:5d7acae44d76 44 void Sensor_Read(void); // Função de leitura dos sensores
lcaepusp 0:5d7acae44d76 45 void Controle_Algoritmo(void); // Função que implementa o algoritmo de controle escolhido
lcaepusp 0:5d7acae44d76 46 void Serialcallback(void); // Função que implementa o algoritmo de controle escolhido
lcaepusp 0:5d7acae44d76 47
lcaepusp 0:5d7acae44d76 48 /******************************************************************************/
lcaepusp 0:5d7acae44d76 49 /*************************** Corpo de Funções *********************************/
lcaepusp 0:5d7acae44d76 50 /******************************************************************************/
lcaepusp 0:5d7acae44d76 51
lcaepusp 0:5d7acae44d76 52 /*************************** Função Principal *********************************/
lcaepusp 0:5d7acae44d76 53 // A main chama todas as inicializações e então aguarda o sinal de que deve
lcaepusp 0:5d7acae44d76 54 // realizar controle. Esse sinal é dado pela flag "Controle" e é setada por uma
lcaepusp 0:5d7acae44d76 55 // interrupção de tempo.
lcaepusp 0:5d7acae44d76 56 //
lcaepusp 0:5d7acae44d76 57 // Para garantir a execução imediata do algoritmo de controle nenhum wait deve
lcaepusp 0:5d7acae44d76 58 // ser chamado durante a execução do controle e o uso de printfs deve ser
lcaepusp 0:5d7acae44d76 59 // esporádico.
lcaepusp 0:5d7acae44d76 60 int main() {
lcaepusp 0:5d7acae44d76 61
lcaepusp 0:5d7acae44d76 62 /*********************************************************************************/
lcaepusp 0:5d7acae44d76 63 /** Inicialização do algoritmo de proteção. NUNCA DEVE SER RETIRADO DO PROGRAMA **/
lcaepusp 0:5d7acae44d76 64 /**/ wait(5); /**/
lcaepusp 0:5d7acae44d76 65 /**/ //Protecao_Init(&Encoder_Motor, &Control_Interrupt, pi/2); /**/
lcaepusp 0:5d7acae44d76 66 /** Inicialização do algoritmo de proteção. NUNCA DEVE SER RETIRADO DO PROGRAMA **/
lcaepusp 0:5d7acae44d76 67 /*********************************************************************************/
lcaepusp 0:5d7acae44d76 68
lcaepusp 0:5d7acae44d76 69 Init();
lcaepusp 0:5d7acae44d76 70 while(1) {
lcaepusp 0:5d7acae44d76 71
lcaepusp 0:5d7acae44d76 72 if(gotRead == 1){
lcaepusp 0:5d7acae44d76 73 gotRead = 0;
lcaepusp 0:5d7acae44d76 74 switch(c) {
lcaepusp 0:5d7acae44d76 75 case 'a':
lcaepusp 0:5d7acae44d76 76 u = u+0.01;
lcaepusp 2:70151b51aba8 77 //pc.printf("u: %f\n\r", u);
lcaepusp 0:5d7acae44d76 78 break;
lcaepusp 0:5d7acae44d76 79 case 'd':
lcaepusp 0:5d7acae44d76 80 u = u-0.01;
lcaepusp 2:70151b51aba8 81 //pc.printf("u: %f\n\r", u);
lcaepusp 0:5d7acae44d76 82 break;
lcaepusp 0:5d7acae44d76 83 case 's':
lcaepusp 0:5d7acae44d76 84 u = 0;
lcaepusp 2:70151b51aba8 85 //pc.printf("u: %f\n\r", u);
lcaepusp 0:5d7acae44d76 86 break;
lcaepusp 0:5d7acae44d76 87 }
lcaepusp 0:5d7acae44d76 88 }
lcaepusp 0:5d7acae44d76 89
lcaepusp 0:5d7acae44d76 90 if(Flag_Controle){
lcaepusp 0:5d7acae44d76 91
lcaepusp 0:5d7acae44d76 92 Sensor_Read(); // Executa a leitura dos sensores
lcaepusp 0:5d7acae44d76 93 Controle_Algoritmo(); // Execução do seu algoritmo de controle
lcaepusp 0:5d7acae44d76 94
lcaepusp 0:5d7acae44d76 95 PlotCount++;
lcaepusp 0:5d7acae44d76 96 if(PlotCount>=100){ // Controla para que o printf ocorra apenas uma vez a cada 10 iterações
lcaepusp 0:5d7acae44d76 97
lcaepusp 0:5d7acae44d76 98 PlotCount = 0;
lcaepusp 1:8ae261460176 99 pc.printf("%f \t %f\n\r", u, dth0);
lcaepusp 1:8ae261460176 100
lcaepusp 0:5d7acae44d76 101 }
lcaepusp 0:5d7acae44d76 102
lcaepusp 0:5d7acae44d76 103 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
lcaepusp 0:5d7acae44d76 104 }
lcaepusp 0:5d7acae44d76 105 }
lcaepusp 0:5d7acae44d76 106 }
lcaepusp 0:5d7acae44d76 107
lcaepusp 0:5d7acae44d76 108 /************** Função de implementação do algoritmo de controle **************/
lcaepusp 0:5d7acae44d76 109 // Nesta função você deve escrever a implementação do algoritmo de controle es-
lcaepusp 0:5d7acae44d76 110 // colhido e do algoritmo de estimação das velocidades.
lcaepusp 0:5d7acae44d76 111 // Caso necessite acesso a alguma variavel não medida ou alguma cons-
lcaepusp 0:5d7acae44d76 112 // tante não definida sinta-se livre para passa-las como argumento, definir
lcaepusp 0:5d7acae44d76 113 // como variavel global ou com um #define
lcaepusp 0:5d7acae44d76 114 void Controle_Algoritmo(void){
lcaepusp 0:5d7acae44d76 115
lcaepusp 0:5d7acae44d76 116 dth0 = (th0-th0_a)/Ts; // Calculo das velocidades por backward
lcaepusp 0:5d7acae44d76 117 dth1 = (th1-th1_a)/Ts; // É interessante propor outro método
lcaepusp 0:5d7acae44d76 118
lcaepusp 2:70151b51aba8 119 if(u>0.22)
lcaepusp 2:70151b51aba8 120 u=0.22;
lcaepusp 2:70151b51aba8 121 if(u<-0.22)
lcaepusp 2:70151b51aba8 122 u=-0.22;
lcaepusp 0:5d7acae44d76 123
lcaepusp 0:5d7acae44d76 124 if(u<0){
lcaepusp 0:5d7acae44d76 125 Motor = -u;
lcaepusp 2:70151b51aba8 126 Horario = 1;
lcaepusp 2:70151b51aba8 127 AntiHorario = 0;
lcaepusp 0:5d7acae44d76 128 }
lcaepusp 0:5d7acae44d76 129 else if(u>0){
lcaepusp 0:5d7acae44d76 130 Motor = u;
lcaepusp 2:70151b51aba8 131 Horario = 0;
lcaepusp 2:70151b51aba8 132 AntiHorario = 1;
lcaepusp 0:5d7acae44d76 133 }
lcaepusp 0:5d7acae44d76 134 else{
lcaepusp 0:5d7acae44d76 135 Motor = 0;
lcaepusp 0:5d7acae44d76 136 Horario = 0;
lcaepusp 0:5d7acae44d76 137 AntiHorario = 0;
lcaepusp 0:5d7acae44d76 138 }
lcaepusp 0:5d7acae44d76 139
lcaepusp 0:5d7acae44d76 140 }
lcaepusp 0:5d7acae44d76 141
lcaepusp 0:5d7acae44d76 142 /************************* Função de Inicialização *****************************/
lcaepusp 0:5d7acae44d76 143 // Esta função concentra todas as inicializações do sistema
lcaepusp 0:5d7acae44d76 144 void Init(void){
lcaepusp 0:5d7acae44d76 145
lcaepusp 0:5d7acae44d76 146 Motor.period(0.0001);
lcaepusp 0:5d7acae44d76 147 Horario = 0;
lcaepusp 0:5d7acae44d76 148 AntiHorario = 0;
lcaepusp 0:5d7acae44d76 149 Motor = 0.0;
lcaepusp 0:5d7acae44d76 150 Control_Interrupt.attach(&Control_Function, Ts);
lcaepusp 0:5d7acae44d76 151 pc.attach(&Serialcallback);
lcaepusp 0:5d7acae44d76 152
lcaepusp 0:5d7acae44d76 153 }
lcaepusp 0:5d7acae44d76 154
lcaepusp 0:5d7acae44d76 155 /********************** Função de leitura dos sensores *************************/
lcaepusp 0:5d7acae44d76 156 // Cada vez que esta função é chamada deve-se calcular os ângulos e velocidades
lcaepusp 0:5d7acae44d76 157 // angulares por algum método conhecido
lcaepusp 0:5d7acae44d76 158 void Sensor_Read(void){
lcaepusp 0:5d7acae44d76 159
lcaepusp 0:5d7acae44d76 160 th0_a=th0;
lcaepusp 0:5d7acae44d76 161 th1_a=th1;
lcaepusp 0:5d7acae44d76 162
lcaepusp 0:5d7acae44d76 163 /** Leituras cruas dos ângulos do encoder **/
lcaepusp 0:5d7acae44d76 164 phi0 = pi*Encoder_Motor.getPulses()/600.0; // (pulsos_lidos/pulsos_por_volta)*pi = angulo_em_radianos
lcaepusp 0:5d7acae44d76 165 phi1 = pi*Encoder_Pendulo.getPulses()/1000.0; // (pulsos_lidos/pulsos_por_volta)*180 = angulo_em_graus
lcaepusp 0:5d7acae44d76 166
lcaepusp 0:5d7acae44d76 167 th0 = phi0;
lcaepusp 0:5d7acae44d76 168 /** 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
lcaepusp 0:5d7acae44d76 169 if(phi1>0) // vertical para baixo esta passa a ser lida como 0º. Porém, para o algoritmo de controle
lcaepusp 0:5d7acae44d76 170 th1 = phi1-pi; // funcionar corretamente 0º deve ser o pêndulo na posição vertical para cima. Para
lcaepusp 0:5d7acae44d76 171 // garantir que isso aconteça subido o pêndulo no sentido horário ou anti-horário fazemos
lcaepusp 0:5d7acae44d76 172 else if(phi1<=0) // th1 = th1-sgn(th1)*pi, onde sgn(x) é o sinal de x.
lcaepusp 0:5d7acae44d76 173 th1 = phi1+pi; // Para ficar mais claro o funcionamento destes "if else" plote o sinal de th1 no tera term
lcaepusp 0:5d7acae44d76 174 // e experimente zerá-lo pelo sentido horário e pelo anti-horário
lcaepusp 0:5d7acae44d76 175
lcaepusp 0:5d7acae44d76 176 }
lcaepusp 0:5d7acae44d76 177
lcaepusp 0:5d7acae44d76 178 /**************** Função de flag do algoritmo de controle ******************/
lcaepusp 0:5d7acae44d76 179 // Esta função avisa a main quando executar o próximo passo do algoritmo de
lcaepusp 0:5d7acae44d76 180 // controle. O uso de uma interrupção para o acionamento da flag garante que
lcaepusp 0:5d7acae44d76 181 // haja exatamente Ts segundos entre execuções.
lcaepusp 0:5d7acae44d76 182 void Control_Function(void){
lcaepusp 0:5d7acae44d76 183
lcaepusp 0:5d7acae44d76 184 Flag_Controle = true;
lcaepusp 0:5d7acae44d76 185
lcaepusp 0:5d7acae44d76 186 }
lcaepusp 0:5d7acae44d76 187
lcaepusp 0:5d7acae44d76 188 void Serialcallback(){
lcaepusp 0:5d7acae44d76 189 // Note: you need to actually read from the serial to clear the RX interrupt
lcaepusp 0:5d7acae44d76 190 if(pc.readable())
lcaepusp 0:5d7acae44d76 191 c = pc.getc();
lcaepusp 0:5d7acae44d76 192 //memcpy(&Entrada, &buffer, sizeof(INPUT));
lcaepusp 0:5d7acae44d76 193 gotRead = 1;
lcaepusp 0:5d7acae44d76 194 }