New version for the new pendulum
Dependencies: PTC3471 QEI USBDevice mbed
main.cpp
00001 #include "mbed.h" 00002 #include "QEI.h" 00003 #include "USBSerial.h" 00004 #include "PTC3471.h" //V. 2022 REV Sentido 00005 00006 #define Ts 0.01 //periodo de amostragem 00007 #define pi 3.141592653589793 00008 00009 /******************************************************************************/ 00010 /**************** Definição de Variaveis, Objetos e Funções ******************/ 00011 /******************************************************************************/ 00012 USBSerial pc; // Objeto de comunicação serial com o TeraTerm 00013 00014 Ticker Control_Interrupt; // Interrupção de Tempo para acionamento do algoritmo de controle 00015 00016 QEI Encoder_Motor (PTB17,PTD0,NC, 300, QEI::X4_ENCODING); // Objeto de leitura do encoder do motor 00017 QEI Encoder_Pendulo (PTA13,PTA12,NC, 600, QEI::X4_ENCODING);// Objeto de leitura do encoder do pêndulo 00018 00019 DigitalOut Horario(PTC1); // DigitalOut que sinaliza se deve virar o motor no sentido horário 00020 DigitalOut AntiHorario(PTD5); // DigitalOut que sinaliza se deve virar o motor no sentido anti-horário 00021 PwmOut Motor(PTD6); // D.C. do PWM [0, 1]: porcentagem de tensão sobre o motor 00022 00023 bool Flag_Controle = false; 00024 int PlotCount = 0; 00025 00026 double phi0 = 0; // phi0 -> Angulo lido pelo Encoder_Braco 00027 double phi1 = 0; // phi1 -> Angulo lido pelo Encoder_Pendulo 00028 double dphi1 = 0; 00029 00030 double th0 = 0; // th0 -> Angulo do braço 00031 double th1 = 0; // th1 -> Angulo do pêndulo 00032 double dth0 = 0; // dth0 -> Velocidade do braço 00033 double dth1 = 0; // dth1 -> Velocidade do pêndulo 00034 00035 double th0_f = 0; // th0 -> Angulo do braço filtrado 00036 double th1_f = 0; // th1 -> Angulo do pêndulo filtrado 00037 double dth0_f = 0; // dth0 -> Velocidade do braço 00038 double dth1_f = 0; // dth1 -> Velocidade do pêndulo 00039 00040 int16_t phi0_int = 0, phi1_int = 0; // Variáveis convertidas para inteiro para serem 00041 int16_t th0_f_int = 0, th1_f_int = 0; // transmitidas via serial 00042 int16_t dth0_f_int = 0, dth1_f_int = 0, u_int = 0; // (economia de memória e de banda) 00043 00044 double tau = 7e-2; // Cte de tempo do FPB dos estados 00045 00046 double th0_a = 0; // Valor de th0 um período de amostragem anterior 00047 double th1_a = 0; // Valor de th1 um período de amostragem anterior 00048 double phi1_a = 0; // Valor de phi1 um período de amostragem anterior 00049 00050 00051 float K[4] = {0.0, 0.0, 0.0, 0.0}; // Ganhos do controlador 00052 float u=0.0; // Inicialização da lei de controle 00053 00054 float t_end = 30.0; // Duração do Experimento 00055 float tempo = 0; // Acumula o tempo de execução do experimento 00056 // Esta variável pode ser usada para temporização da referência 00057 00058 int N_plot = (int) t_end/(Ts*10); // Numero de amostras armazenadas para plot 00059 00060 00061 void Init(void); // Função de Inicialização 00062 void Control_Function(void); // Função de flag do controle, a ser chamada pela interrupção 00063 void Sensor_Read(void); // Função de leitura dos sensores 00064 void Controle_Algoritmo(void); // Função que implementa o algoritmo de controle escolhido 00065 00066 /******************************************************************************/ 00067 /*************************** Corpo de Funções *********************************/ 00068 /******************************************************************************/ 00069 00070 /*************************** Função Principal *********************************/ 00071 // A main chama todas as inicializações e então aguarda o sinal de que deve 00072 // realizar controle. Esse sinal é dado pela flag "Controle" e é setada por uma 00073 // interrupção de tempo. 00074 // 00075 // Para garantir a execução imediata do algoritmo de controle nenhum wait deve 00076 // ser chamado durante a execução do controle e o uso de printfs deve ser 00077 // esporádico. 00078 int main() 00079 { 00080 int ap = 0; // Indice dos vetores de amostras 00081 int ii = 0; // Indice para plot das amostras 00082 int16_t th0_f_int[N_plot], th1_f_int[N_plot]; // Vetores para armazenar dados e 00083 int16_t dth0_f_int[N_plot], dth1_f_int[N_plot], u_int[N_plot];// serem transmitidos via serial 00084 00085 /*********************************************************************************/ 00086 /** Inicialização do algoritmo de proteção. NUNCA DEVE SER RETIRADO DO PROGRAMA **/ 00087 /**/ wait(5); /**/ 00088 /**/ /**/ 00089 /**/ Protecao_Init(&Encoder_Motor, &Control_Interrupt, pi); /**/ 00090 /** Inicialização do algoritmo de proteção. NUNCA DEVE SER RETIRADO DO PROGRAMA **/ 00091 /*********************************************************************************/ 00092 00093 Init(); 00094 00095 while(1) { 00096 00097 if(Flag_Controle) { 00098 00099 Sensor_Read(); // Executa a leitura dos sensores 00100 Controle_Algoritmo(); // Executa a lei de controle 00101 00102 PlotCount++; 00103 if(PlotCount>=10) { // Controla para que o printf ocorra a cada 10 iterações 00104 00105 // As variáveis serão multiplicadas por 1000 e convertidas para inteiro 00106 // antes de serem trasmitidas. Ao receber, deve-se dividir por 1000 antes 00107 // de fazer o plot. OBS: a precisão no gráfico será de 3 casas decimais 00108 th0_f_int[ap] = th0_f*1000; 00109 th1_f_int[ap] = th1_f*1000; 00110 dth0_f_int[ap] = dth0_f*1000; 00111 dth1_f_int[ap] = dth1_f*1000; 00112 u_int[ap] = u*1000; 00113 00114 ap = ap + 1; // Prepara para a próxima amostra 00115 PlotCount = 0; 00116 } 00117 // Após t_end segundos, o experimento é interrompido e os dados são transmitidos via serial 00118 if (tempo >= t_end) { 00119 Control_Interrupt.detach(); 00120 Motor = 0; 00121 Horario = 0; 00122 AntiHorario = 0; 00123 for (ii=0; ii<N_plot; ii++) 00124 pc.printf("%d \t %d \t %d \t %d \t %d\n\r", th0_f_int[ii], th1_f_int[ii], 00125 dth0_f_int[ii], dth1_f_int[ii], u_int[ii]); 00126 00127 } 00128 00129 Flag_Controle = false; // Sinaliza que deve-se esperar o próximo sinal da interrupção 00130 // de tempo para executar o próximo passo de controle 00131 } 00132 } 00133 } 00134 00135 00136 00137 /************** Função de implementação do algoritmo de controle **************/ 00138 // Nesta função você deve escrever a implementação do algoritmo de controle es- 00139 // colhido e do algoritmo de estimação das velocidades. 00140 // Caso necessite acesso a alguma variavel não medida ou alguma cons- 00141 // tante não definida sinta-se livre para passa-las como argumento, definir 00142 // como variavel global ou com um #define 00143 void Controle_Algoritmo(void) 00144 { 00145 00146 dth0 = (th0-th0_a)/Ts; // Calculo das velocidades por backward 00147 dth1 = (th1-th1_a)/Ts; // É interessante propor outro método 00148 00149 // Filtro (1/tau*s +1) nos derivadas 00150 dth0_f = (tau/(Ts+tau))*dth0_f + (Ts/(Ts+tau))*dth0; 00151 dth1_f = (tau/(Ts+tau))*dth1_f + (Ts/(Ts+tau))*dth1; 00152 00153 u=-((K[0]*th0_f)+(K[1]*th1_f)+(K[2]*dth0_f)+(K[3]*dth1_f)); 00154 00155 if(u>0.5) 00156 u=0.5; 00157 if(u<-0.5) 00158 u=-0.5; 00159 00160 if(u<0) { 00161 Motor = -u; 00162 Horario = 1; 00163 AntiHorario = 0; 00164 } else if(u>0) { 00165 Motor = u; 00166 Horario = 0; 00167 AntiHorario = 1; 00168 } else { 00169 Motor = 0; 00170 Horario = 0; 00171 AntiHorario = 0; 00172 } 00173 } 00174 00175 /************************* Função de Inicialização *****************************/ 00176 // Esta função concentra todas as inicializações do sistema 00177 void Init(void) 00178 { 00179 Motor.period(0.0001); 00180 Horario = 0; 00181 AntiHorario = 0; 00182 Motor = 0.0; 00183 Control_Interrupt.attach(&Control_Function, Ts); 00184 } 00185 00186 /********************** Função de leitura dos sensores *************************/ 00187 // Cada vez que esta função é chamada deve-se calcular os ângulos e velocidades 00188 // angulares por algum método conhecido 00189 void Sensor_Read(void) 00190 { 00191 th0_a=th0; 00192 th1_a=th1; 00193 phi1_a=phi1; 00194 00195 /** Leituras cruas dos ângulos do encoder **/ 00196 phi0 = pi*Encoder_Motor.getPulses()/600.0; 00197 phi1 = pi*Encoder_Pendulo.getPulses()/1200.0; 00198 00199 th0 = phi0; 00200 /** Tratamento do ângulo lido para ser zero na vertical para cima **/ 00201 // Como o encoder é incremental quando inicializamos o programa com o pêndulo na posição 00202 if(phi1>0) // vertical para baixo esta passa a ser lida como 0º. Porém, para o algoritmo de controle 00203 th1 = phi1-pi; // funcionar corretamente 0º deve ser o pêndulo na posição vertical para cima. Para 00204 // garantir que isso aconteça subido o pêndulo no sentido horário ou anti-horário fazemos 00205 else if(phi1<=0) // th1 = th1-sgn(th1)*pi, onde sgn(x) é o sinal de x. 00206 th1 = phi1+pi; 00207 00208 // Filtro (1/tau*s +1) nos angulos 00209 th0_f = (tau/(Ts+tau))*th0_f + (Ts/(Ts+tau))*th0; 00210 th1_f = (tau/(Ts+tau))*th1_f + (Ts/(Ts+tau))*th1; 00211 00212 } 00213 00214 /**************** Função de flag do algoritmo de controle ******************/ 00215 // Esta função avisa a main quando executar o próximo passo do algoritmo de 00216 // controle. O uso de uma interrupção para o acionamento da flag garante que 00217 // haja exatamente Ts segundos entre execuções. 00218 void Control_Function(void) 00219 { 00220 00221 Flag_Controle = true; 00222 tempo = tempo+Ts; 00223 }
Generated on Tue Sep 27 2022 19:34:31 by 1.7.2