GERMAN REINA
NUCLEO-F446RE ENCODER TIMER3 Y TIMER 2 HAL AND PWM. PID PARA UN PENDULO INVERTIDO CON 2 ENCODER DE CUADRATURA
Diff: main.cpp
- Revision:
- 0:95f924550159
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat May 21 04:17:35 2016 +0000
@@ -0,0 +1,564 @@
+/*PROYECTO DEL PENDULO INVERTIDO
+ * TARJETA= NUCLEO STM32F446
+ ENCODER EJE X PB_8 CH1 PB_9 CH2
+ ENCODER PENDULO PB_4 PB_5
+
+ FIN DE CARRERA MOTOR IZQ = A1
+ FIN DERECHA DER = A0
+
+ POLOLULU
+
+ nD2 = D6
+ M1DIR=D7
+ M1PWM= D11 (TIMER 1)
+ NsF=D12
+ GND=D13
+ VCC=VCC
+ */
+
+
+
+/*------------- THANKS DAVID LOWE FOR YOUR HELP-Your API/HAL use full ---------------------------------------
+ * Using STM32's counter peripherals to interface rotary encoders.
+ * Encoders are supported on F4xx's TIM1,2,3,4,5. TIM2 & TIM5 have 32bit count, others 16bit.
+ * Beware mbed uses TIM5 for system timer, SPI needs TIM1, others used for PWM.
+ * Check your platform's PeripheralPins.c & PeripheralNames.h if you need both PWM & encoders.
+ *
+ * Edit HAL_TIM_Encoder_MspInitFx.cpp to suit your mcu & board's available pinouts & pullups/downs.
+ *
+ * Thanks to:
+ * http://petoknm.wordpress.com/2015/01/05/rotary-encoder-and-stm32/
+ *
+ * References:
+ * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/user_manual/DM00122015.pdf
+ * http://www.st.com/st-web-ui/static/active/en/resource/technical/document/reference_manual/DM00096844.pdf
+ * http://www.st.com/web/en/resource/technical/document/application_note/DM00042534.pdf
+ * http://www.st.com/web/en/resource/technical/document/datasheet/DM00102166.pdf
+ *
+ * David Lowe Jan 2015
+ *
+
+ *Se complementa con el Timer 8 en la tarjeta nucleo
+ *Efectivamente el Timer 5 no se debe usar.
+ *Se Ajustaron los pines para la tarjeta para usar otros para los PWM
+
+ */
+
+#include "mbed.h"
+#include "Encoder.h"
+
+
+/****************************ENCODER INICIALIZACION *************************/
+//STM mbed bug: these macros are MISSING from stm32f3xx_hal_tim.h
+#ifdef TARGET_STM32F3
+#define __HAL_TIM_GET_COUNTER(__HANDLE__) ((__HANDLE__)->Instance->CNT)
+#define __HAL_TIM_IS_TIM_COUNTING_DOWN(__HANDLE__) (((__HANDLE__)->Instance->CR1 &(TIM_CR1_DIR)) == (TIM_CR1_DIR))
+#endif
+
+TIM_Encoder_InitTypeDef encoder2, encoder3, encoder4,encoder5, encoder8;
+TIM_HandleTypeDef timer2, timer3;
+
+/*************************************************************************/
+
+/*CONFIGURAMOS EL SHIELD DEL POLOLULU
+ Al coincidir con los pines del timer se modifica el codigo
+*/
+
+DigitalOut M1DIR(D7);
+PwmOut M1PWM(PA_7);
+//AnalogIn M1FB(A0);
+
+//DigitalOut M2DIR(D8);
+//PwmOut M2PWM(D10);
+//DigitalIn M2FB(A1);
+
+DigitalOut nD2(D6,1);
+DigitalIn nSF(D12);
+
+
+
+
+/***********PUERTO SERIAL*****************/
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+
+/*VALORES POR DEFECTO*/
+
+float VEL=0.0;
+
+/*VALORES DE CONFIGURACION DE LOS PINES*/
+
+InterruptIn FINIZQR(A1);
+InterruptIn FINDERM(A0);
+//DigitalIn FINIZQR(D5);
+//DigitalIn FINDERM(D6);
+
+int alarma=0;
+DigitalOut INDICA(LED2);
+DigitalIn INIT(PC_13);
+
+
+void toggle1(void);
+void toggle2(void);
+void inicializa(void);
+void centrar(void);
+
+/*-------------VARIABLES GLOBALES----------------------*/
+
+ float X=0;
+ float POS=0;
+ float A=0;
+ float ANG=0;
+ float LIMIN=300;//SI no inicializa toca manual 220
+ float LIMAX=7000; //Si no inciializa toca
+
+
+ float XMIN=0; //en las pruebas realizadas el 0 +200 pulsos
+ float XMAX=7200;; //en las pruebas aprox 7949
+ float MITAD=0;
+ int DATAPC=0;
+ int LEIDO=0;
+
+
+
+/*------------------VARIABLES DEL PID Discreto--------------------------*/
+
+
+float uc0,yn0,en0,cn0,uc1,yn1,cn1,en1,yn2,cn2,en2,yn3,cn3,en3;
+
+
+//Constantes
+
+float q0,q1,q2;
+
+/*-----------------PID DE PENDULO COMPLETO-------------------------------*/
+float PID_CODIGO(float MEDIDA,float SETPOINT,float DT);
+
+float KI;
+float KP;
+float KD;
+float ERR;
+float PREVI_ERR;
+float INTEGRAL;
+float DERIVATIVO;
+
+float DESEADO=180;
+
+float TM=0.01; // tiempo de muestreo
+
+
+float numero, cal_pwm;
+
+
+
+/*-------------------------------------------------PROGRAMA---------------------------------------------------------------*/
+
+int main()
+{
+ float SALIDAPID;
+
+
+
+ //inicializacion de las varibles
+ yn0=0;
+ cn0=0;
+ en0=0;
+ yn1=0;
+ cn1=0;
+ en1=0;
+ yn2=0;
+ cn2=0;
+ en2=0;
+ yn3=0;
+ cn3=0;
+ en3=0;
+
+ q0=0.0510;
+ q1=-0.00102;
+ q2=0;
+
+ INTEGRAL=0.0;
+ KI=0.5;
+ KP=13;
+ KD=0.25;
+ SALIDAPID=0.0;
+
+
+
+
+ //Configuramos el puerto de 96000 a 115200 para mayor velocidad
+ pc.baud(115200);
+
+
+ //counting on both A&B inputs, 4 ticks per cycle, full 32-bit count
+ EncoderInit(&encoder2, &timer2, TIM2, 0xffffffff, TIM_ENCODERMODE_TI12);
+
+ //counting on both A&B inputs, 4 ticks per cycle, full 32-bit count
+ EncoderInit(&encoder3, &timer3, TIM3, 0xffff, TIM_ENCODERMODE_TI12);
+
+ int16_t count3=0;
+ int32_t count2=0;
+ //int8_t dir2, dir3;
+
+ M1PWM.period_ms(10.0); // set PWM period to 10 ms
+
+ pc.printf("Listo iniciamos centrando el carro !\n");
+ wait(0.1);
+
+
+ inicializa();
+ centrar();
+ alarma=1;
+
+
+
+ pc.printf("Centre el pendulo y Presione reset para continuar \n\r");
+ //pc.printf("Press 'a' = aumentar, 'd' = disminuir 'i'=Izquierda 'o'=derecha 'r'=reset contadores 's'=para solo el PWM \n\r");
+ char c ='s';
+ if (INIT==0){
+
+ yn0=0;
+ cn0=0;
+ en0=0;
+ yn1=0;
+ cn1=0;
+ en1=0;
+ yn2=0;
+ cn2=0;
+ en2=0;
+ yn3=0;
+ cn3=0;
+ en3=0;
+ alarma=0;
+ VEL=0;
+ numero=0;
+ wait(0.5);
+ }
+
+
+ //Ciclo principal
+ while(1) {
+
+
+ //----Deje habilitado la lectura del teclado para pruebas
+ if(pc.readable()==1) {
+ wait(0.1);
+ c = pc.getc();
+ pc.putc(c);
+
+
+ if((c == 'a') && (VEL < 0.99)) {
+ VEL += 0.01;
+ M1PWM = VEL;
+ }
+ if((c == 'd') && (VEL > 0.01)) {
+ VEL -= 0.01;
+ M1PWM = VEL;
+ }
+ if (c == 'i') {
+ M1DIR=1;
+ }
+ if (c == 'o') {
+ M1DIR=0;
+ }
+ if (c == 'r') {
+ __HAL_TIM_SetCounter(&timer3, 0x0000);
+ __HAL_TIM_SetCounter(&timer2, 0x00000000);
+ }
+ if (c == 's') {
+ M1PWM = 0.0;
+ }
+
+ c='0';
+ }
+ //----fin rutina de teclado
+
+ INDICA=alarma;
+
+ FINIZQR.rise(&toggle1);
+ FINDERM.rise(&toggle2);
+
+
+ //Rutina de lectura de los encoder timer2=X y Timer3 = Angulo
+ //X es de 100 huecos por revolucion = X4 400 pulsos por revolucion
+ //X es de 500 huecos por revolucuon = X4 2000 pulsos por revolucion
+ // X aproximado de 7950 +/10% por temas mecanicos de hay que sea variable el centro
+ // MITAD = 3757 depende de la ubicacion de los sensores ((MAX-220)-(MIN+220))/2
+
+ //count2=TIM2->CNT;
+ //dir2=TIM2->CR1&TIM_CR1_DIR;
+ count2=__HAL_TIM_GET_COUNTER(&timer2);
+ //dir2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); //no se necesita para el calculo
+
+ //count3=TIM3->CNT;
+ //dir3=TIM3->CR1&TIM_CR1_DIR;
+ count3=__HAL_TIM_GET_COUNTER(&timer3);
+ //dir3 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer3); //no se necesita para el calculo
+
+
+
+
+ X=count2;//sensor de posicion
+ A=count3;// sensor de angulo
+
+ pc.printf("%6.0f %6.0f \t", X,A);
+
+ if (INIT==0){
+
+ yn0=0;
+ cn0=0;
+ en0=0;
+ yn1=0;
+ cn1=0;
+ en1=0;
+ yn2=0;
+ cn2=0;
+ en2=0;
+ yn3=0;
+ cn3=0;
+ en3=0;
+ alarma=0;
+ VEL=0;
+ numero=0;
+ wait(0.5);
+ }
+
+
+ //factor de conversion por longitud/pulsos (720mm/7949 =0.090577 mm* pulso LTOTAL/PULSOS
+ POS=X*720/7949;
+
+
+ //factor de conversion grados vuelta/pulsos (360grados/2000 pulsos)=0.18 grados por pulso
+ ANG=A*0.18;
+
+ pc.printf("POS: %6.3f ANGULO:%6.3f \t", POS,ANG);
+
+
+ if(alarma==0){
+
+ SALIDAPID=PID_CODIGO(ANG, DESEADO, TM);
+
+ VEL=SALIDAPID;
+
+
+ //MAXIMO DE LA TARJETA
+
+ if (VEL>100) { //Salida PID si es mayor que el MAX
+ VEL=99;
+ }
+ if (VEL<-100) { //Salida PID si es mayor que el MAX
+ VEL=-99;
+ }
+
+ if((VEL<=100)&&(VEL>=0)){
+ VEL=VEL*1; //Error es positivo entonces gire a la izquierda lado opuesto del motor
+ M1DIR=0;
+ }
+
+ if((VEL<0)&&(VEL>-100)){
+ VEL=VEL*-1; //Error es negativo entonces gire a la derecha lado opuesto del motor
+ M1DIR=1;
+ }
+
+
+ if (abs(VEL)<10){ //Salida PID si es menor que el MIN
+ VEL=1;
+ }
+
+ M1PWM=(float)VEL/100;//para controlar con las teclas por silas moscas
+
+
+ }
+ pc.printf("PID = %f \t", SALIDAPID);
+ pc.printf("REF:%6.2f ANG:%6.2f ERROR: %6.2f ACC:%6.2f Itera: %.0f\t",DESEADO, ANG, ERR,VEL,numero);
+
+
+
+
+ //pc.printf("El PWM %1.3f \t ",VEL);
+ pc.printf("Alama: %i \n \r",alarma);
+
+
+
+
+
+ /*************************************************************************************************/
+ numero=numero+1;
+
+
+ /****seguridad***************/
+ if ((X+300)>=LIMAX){
+ VEL=0;
+ M1PWM=0;
+ alarma=1;
+ }
+ if ((X-300)<=LIMIN){
+ VEL=0.0;
+ M1PWM=0.0;
+ alarma=1;
+ }
+
+
+
+
+
+ }
+}
+
+/*---------------------FUNCIONES-----------------------------------------------*/
+
+ /*---------------------INTERUPPCIONES------------------------------------------*/
+
+ void toggle1() {
+ wait(0.25);
+ VEL = 0.0;
+ M1PWM = 0.0;
+ alarma=1;
+ pc.printf("ALARMA FINAL IZQUIERDA\n\r");
+ }
+
+ void toggle2() {
+ wait(0.25);
+ VEL = 0.0;
+ alarma=1;
+ M1PWM = 0.0;
+ pc.printf("ALARMA FINAL DERECHA\n\r");
+ }
+
+/*--------------------------------FUCIONES DE INICIALIZACION------------------------------*/
+
+
+void inicializa(){
+ pc.printf("Funcion de Inicializacion iniciada\n\r");
+
+ M1PWM=0.1;
+ M1DIR=1;
+ while(FINDERM==0){
+ pc.printf("BUSCANCO DERECHA DEL MOTOR\n\r");
+ }
+ pc.printf("ENCONTRADA - REINICIANDO EL CONTADOR EN X\n\r");
+ __HAL_TIM_SetCounter(&timer2, 0x00000000);
+
+ M1PWM=0.1;
+ M1DIR=0;
+
+ while(FINIZQR==0){
+ pc.printf("BUSCANCO IZQUIERDA LADO OPUESTO\n\r");
+ }
+ M1PWM=0;
+ XMAX=__HAL_TIM_GET_COUNTER(&timer2);
+ M1PWM=0;
+ XMIN=0;
+ LIMIN=220;
+ LIMAX=XMAX-220;
+ MITAD=(LIMAX-LIMIN)/2;
+ pc.printf("Fijando los limites XMIN+220 y XMAX-220 por seguridad, XMIN:%6.0f XMAX:%6.0f MITAD:%6.1f \n\r",XMIN, XMAX, MITAD);
+
+ }
+
+void centrar(){
+ float error, pos;
+ int parada;
+ float itera;
+ parada=1;
+ itera=0;
+ pos=0;
+ error=0;
+
+ float K1, K2, K3, uk;
+
+ /*valores por defecto*/
+ //Kp=0.03;
+ //Ki=0;
+ //Kd=0;
+
+
+ /*parametros del PID discreto*/
+ // K1=Kp+(Ki/2)*Ts+(Kd/Ts);
+ // K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts);
+ // K3=Kd/Ts;
+
+ K1=0.19;
+ K2=0;
+ K3=0;
+
+
+
+ pos=__HAL_TIM_GET_COUNTER(&timer2);
+ error=MITAD-pos;
+
+ while(parada==1){
+
+ if (error>0){
+ //Error es positivo entonces gire a la izquierda lado opuesto del motor
+ M1DIR=0;
+ }
+ else if(error<=0){
+ //Error es negativo entonces gire a la derecha hacia el motor
+ M1DIR=1;
+ }
+
+ pos=__HAL_TIM_GET_COUNTER(&timer2);
+ error=MITAD-pos;
+
+ uk=error*K1;
+
+
+
+ pc.printf("BUSCA LA MITAD M:%6.0f P:%6.0f E:%6.1f Uk:%6.1f Itera:%6.0f\n\r",MITAD, pos, error,uk, itera);
+
+ if (abs(uk)>100) { //Salida si es mayor que el MAX
+ uk=80;}
+ else if (abs(uk)<20){ //Salida si es menor que el MIN
+ uk=5;
+ }
+
+ M1PWM=(float)uk/100; //Accion de control mapeada a PWM; salida de la tarjeta.
+
+
+
+ if (abs(error)<5){
+ parada=0;
+ }
+ if (itera>1000){
+ parada=0;
+ }
+
+ itera++;
+
+ }
+ M1PWM=0.0;
+ pc.printf("ENCONTRADO MITAD:%f POS:%f Error: %f Itera: %f\n\r",MITAD, pos, error, itera);
+
+
+}
+
+/******************************************PID*******************************************************/
+float PID_CODIGO(float MEDIDA,float SETPOINT,float DT){
+ float OUTPID;
+
+ ERR=SETPOINT-MEDIDA;
+ INTEGRAL=INTEGRAL+ERR*DT;
+ DERIVATIVO=(ERR-PREVI_ERR)/DT;
+ OUTPID=KP*ERR-KI*INTEGRAL+KD*DERIVATIVO;
+ PREVI_ERR=ERR;
+
+ //wait(DT);
+ return(OUTPID);
+
+
+ //previous_error = 0
+ //integral = 0
+ //start:
+ //error = setpoint - measured_value
+ //integral = integral + error*dt
+ //derivative = (error - previous_error)/dt
+ //output = Kp*error + Ki*integral + Kd*derivative
+ //previous_error = error
+ //wait(dt)
+ //goto start
+
+
+
+}