Gustavo Gran / Mbed 2 deprecated LQR_With_Integrator_Implicito

ver 1

Dependencies:   mbed QEI PTC3471 USBDevice

main.cpp@0:f4d8c80475a0, 2017-08-25 (annotated)

Committer:
lcaepusp
Date:
Fri Aug 25 14:22:44 2017 +0000
Revision:
0:f4d8c80475a0
Child:
1:e2066df0a7a6
Template base para uso de controles. Est? inclu?do as interrup??es para trava de seguran?a (n?o alterar exceto quando solicitado).

Who changed what in which revision?

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