Writing /var/lib/dokuwiki/data/meta/teaching/ie0624/proyectos/control_motores_sincronicos/codigo.meta failed
teaching:ie0624:proyectos:control_motores_sincronicos:codigo
Código Fuente Utilizado para la Implementación del Sistema de Control
Diagrama de Flujo
- A continuación se muestra el código fuente utilizado para la implementación de Sistema de Control, incluyendo la comunicación a través de USART, y las señales enviadas al puente H para el control de potencia.
Para una mejor comprensión de la lógica general, ver el diagrama de flujo del sistema implementado.
/* * Copyright (C) 2018 ARCOS-Lab Universidad de Costa Rica * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see <http://www.gnu.org/licenses/>. */ #include <libopencm3/stm32/rcc.h> #include <libopencm3/stm32/gpio.h> #include <libopencm3/stm32/adc.h> //analog digital converter #include <libopencm3/stm32/usart.h> #include <libopencm3/stm32/timer.h> #include <libopencm3/stm32/f3/nvic.h> #include "proyecto.h" #include <limits.h> #include <stdbool.h> #include <stdio.h> #include <stdlib.h> //pwm-related timer configuration #define SYSFREQ 64000000 //168MHz #define PWMFREQ 8000 //32000 #define PWMFREQ_F ((float )(PWMFREQ)) //32000.0f #define PRESCALE 1 //freq_CK_CNT=freq_CK_PSC/(PSC[15:0]+1) #define PWM_PERIOD_ARR (SYSFREQ/(PWMFREQ*PRESCALE)) #define LBLUE GPIOE, GPIO8 #define LRED GPIOE, GPIO9 #define LORANGE GPIOE, GPIO10 #define LGREEN GPIOE, GPIO11 #define LBLUE2 GPIOE, GPIO12 #define LRED2 GPIOE, GPIO13 #define LORANGE2 GPIOE, GPIO14 #define LGREEN2 GPIOE, GPIO15 #define LD4 GPIOE, GPIO8 #define LD3 GPIOE, GPIO9 #define LD5 GPIOE, GPIO10 #define LD7 GPIOE, GPIO11 #define LD9 GPIOE, GPIO12 #define LD10 GPIOE, GPIO13 #define LD8 GPIOE, GPIO14 #define LD6 GPIOE, GPIO15 void leds_init(void) { rcc_peripheral_enable_clock(&RCC_AHBENR, RCC_AHBENR_IOPEEN); gpio_mode_setup(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO8| GPIO9| GPIO10| GPIO11| GPIO12| GPIO13| GPIO14| GPIO15);//PE8-15 } void system_init(void) { rcc_clock_setup_hsi(&rcc_hsi_configs[RCC_CLOCK_HSI_64MHZ]); leds_init(); } //timer ######################################################################################## void tim_init(void) { /* Enable TIM1 clock. and Port E clock (for outputs) */ rcc_periph_clock_enable(RCC_TIM1); rcc_periph_clock_enable(RCC_GPIOE); //Set TIM1 channel (and complementary) output to alternate function push-pull'. //f4 TIM1=> GIO9: CH1, GPIO11: CH2, GPIO13: CH3 //f4 TIM1=> GIO8: CH1N, GPIO10: CH2N, GPIO12: CH3N gpio_mode_setup(GPIOE, GPIO_MODE_AF,GPIO_PUPD_NONE,GPIO9); gpio_set_af(GPIOE, GPIO_AF2, GPIO9); /* Reset TIM1 pepheral. */ rcc_periph_reset_pulse(RST_TIM1); /* Timer global mode: * - No divider * - Alignment edge * - Direction up */ timer_set_mode(TIM1, TIM_CR1_CKD_CK_INT, //For dead time and filter sampling, not important for now. TIM_CR1_CMS_CENTER_3, //TIM_CR1_CMS_EDGE //TIM_CR1_CMS_CENTER_1 //TIM_CR1_CMS_CENTER_2 //TIM_CR1_CMS_CENTER_3 la frequencia del pwm se divide a la mitad. TIM_CR1_DIR_UP); timer_set_prescaler(TIM1, PRESCALE); //1 = disabled (max speed) timer_set_repetition_counter(TIM1, 0); //disabled timer_enable_preload(TIM1); timer_continuous_mode(TIM1); /* Period (32kHz). */ timer_set_period(TIM1, PWM_PERIOD_ARR); //ARR (value compared against main counter to reload counter aka: period of counter) /* Configure break and deadtime. */ timer_set_enabled_off_state_in_idle_mode(TIM1); timer_set_enabled_off_state_in_run_mode(TIM1); timer_disable_break(TIM1); timer_set_break_polarity_high(TIM1); timer_disable_break_automatic_output(TIM1); timer_set_break_lock(TIM1, TIM_BDTR_LOCK_OFF); /* Disable outputs. */ timer_disable_oc_output(TIM1, TIM_OC1); timer_disable_oc_output(TIM1, TIM_OC1N); timer_disable_oc_output(TIM1, TIM_OC2); timer_disable_oc_output(TIM1, TIM_OC2N); timer_disable_oc_output(TIM1, TIM_OC3); timer_disable_oc_output(TIM1, TIM_OC3N); /* -- OC1 and OC1N configuration -- */ /* Configure global mode of line 1. */ timer_enable_oc_preload(TIM1, TIM_OC1); timer_set_oc_mode(TIM1, TIM_OC1, TIM_OCM_PWM1); /* Configure OC1. */ timer_set_oc_polarity_high(TIM1, TIM_OC1); timer_set_oc_idle_state_unset(TIM1, TIM_OC1); //When idle (braked) put 0 on output /* Configure OC1N. */ timer_set_oc_polarity_high(TIM1, TIM_OC1N); timer_set_oc_idle_state_unset(TIM1, TIM_OC1N); //####### /* Set the capture compare value for OC1. */ //######### /* ARR reload enable. */ timer_enable_preload(TIM1); /* * Enable preload of complementary channel configurations and * update on COM event. */ timer_disable_preload_complementry_enable_bits(TIM1); /* Enable outputs in the break subsystem. */ timer_enable_break_main_output(TIM1); /* Generate update event to reload all registers before starting*/ timer_generate_event(TIM1, TIM_EGR_UG); timer_set_oc_mode(TIM1, TIM_OC1, TIM_OCM_PWM1); timer_enable_oc_output(TIM1, TIM_OC1 ); timer_disable_oc_output (TIM1, TIM_OC1N); /* Counter enable. */ timer_enable_counter(TIM1); //enable capture compare interrupt timer_enable_update_event(TIM1); timer_enable_irq(TIM1, TIM_DIER_UIE); nvic_enable_irq(NVIC_TIM1_UP_TIM16_IRQ); } //ADC ######################################################################################## static void adc_setup(void) { //ADC rcc_periph_clock_enable(RCC_ADC12); rcc_periph_clock_enable(RCC_GPIOA); //ADC gpio_mode_setup(GPIOA, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO0);//PA0 config to work with USER botton gpio_mode_setup(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO1); adc_power_off(ADC1); adc_set_clk_prescale(ADC1, ADC_CCR_CKMODE_DIV2); adc_set_single_conversion_mode(ADC1); adc_disable_external_trigger_regular(ADC1); adc_set_right_aligned(ADC1); /* We want to read the temperature sensor, so we have to enable it. */ adc_enable_temperature_sensor(); adc_set_sample_time_on_all_channels(ADC1, ADC_SMPR_SMP_61DOT5CYC); uint8_t channel_array[] = { 2 }; /* ADC1_IN2 (PA1) */ adc_set_regular_sequence(ADC1, 1, channel_array); adc_set_resolution(ADC1, ADC_CFGR1_RES_12_BIT); adc_power_on(ADC1); /* Wait for ADC starting up. */ int i; for (i = 0; i < 800000; i++) __asm__("nop"); } void tim1_up_tim16_isr(void) { //Timer overflow interrupt, PWM frecuency // Clear the update interrupt flag timer_clear_flag(TIM1, TIM_SR_UIF); gpio_toggle(GPIOE, GPIO12); /* LED on/off */ } //usart ######################################################################################## static void usart_setup(void) { /* Enable clocks for GPIO port A (for GPIO_USART2_TX) and USART2. */ rcc_periph_clock_enable(RCC_USART2); rcc_periph_clock_enable(RCC_GPIOA); /* Setup GPIO pin GPIO_USART2_TX/GPIO9 on GPIO port A for transmit. */ gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO2 | GPIO3); gpio_set_af(GPIOA, GPIO_AF7, GPIO2| GPIO3); /* Setup UART parameters. */ usart_set_baudrate(USART2, 115200); usart_set_databits(USART2, 8); usart_set_stopbits(USART2, USART_STOPBITS_1); usart_set_mode(USART2, USART_MODE_TX_RX); usart_set_parity(USART2, USART_PARITY_NONE); usart_set_flow_control(USART2, USART_FLOWCONTROL_NONE); /* Finally enable the USART. */ usart_enable(USART2); } //print numbers func. with USART static void my_usart_print_int(uint32_t usart, int16_t value)//print sent values { int8_t i; int8_t nr_digits = 0; char buffer[25]; if (value < 0){ value = value*(-1); } if (value == 0) { usart_send_blocking(usart, '0'); } while (value > 0) { buffer[nr_digits++] = "0123456789"[value % 10]; value /= 10; } for (i = nr_digits-1; i >= 0; i--) { usart_send_blocking(usart, buffer[i]); } usart_send_blocking(usart, '\r'); usart_send_blocking(usart, '\n'); } //funcion para esperar tiempos static void delay(int t){ for(int i= 0; i<10000*t; i++) __asm__("nop"); } //funcion que recibe un valor entre -99% y 99% para el valor de referencia static valor_de_referencia(){ uint16_t ret=0; int16_t in_data[4]={0}; uint8_t i=0; int new_data=0; while(i<4){ switch (new_data){ case 0: //Si la primera tecla no es - o +, no avanza in_data[0]=usart_recv_blocking(USART2); if(in_data[0]== 0x2B || in_data[0]== 0x2D){ usart_send_blocking(USART2, in_data[i]); i++; new_data=1; } break; case 1: //Si la segunda tecla no es un numero no avanza in_data[1]=usart_recv_blocking(USART2); if(in_data[1]>= 0x30 && in_data[1]<= 0x39){ usart_send_blocking(USART2, in_data[i]); i++; new_data=2; } break; case 2://Si la tercer tecla no es un numero no avanza in_data[2]=usart_recv_blocking(USART2); if(in_data[2]>= 0x30 && in_data[2]<= 0x39){ usart_send_blocking(USART2, in_data[i]); i++; new_data=3; } break; case 3: //Si la cuarta tecla no es un enter no finaliza in_data[3]=usart_recv_blocking(USART2); if(in_data[3]==0x0D){ i++; } break; } } if(in_data[0]==0x2B){ret=100;} //si es + ret=ret+(in_data[1]-48)*10+(in_data[2]-48); //si es + queda 1XX, si es - queda 0XX //regresa el valor convertido a un int, si es mayor a 100 es positivo, y si es menor a 100 es negativo. +50 = 150; -50 = 050 return ret; } //main ######################################################################################## int main(void) { double duty; double kp=0.3; double Ti=40; double rango=4096; double e; double I=0.0; double Inew=0.0; double Iant=0.0; double control; double vel; double P; int16_t temp; bool sentido; int16_t ref;//% del valor deseado, int cambio_sentido; system_init(); tim_init(); adc_setup(); usart_setup(); ref=valor_de_referencia(); if(ref>100){ sentido=true;//+ ref=ref-100;//Para que quede un numero entre 0 y 100. El signo se almacena con la variable 'sentido' } else { sentido=false;//- ref=ref*(-1); } cambio_sentido=ref; while (1){ //Imprimir ref usart_send_blocking(USART2, '\n'); usart_send_blocking(USART2, 'R'); usart_send_blocking(USART2, 'e'); usart_send_blocking(USART2, 'f'); usart_send_blocking(USART2, ':'); usart_send_blocking(USART2, ' '); if(sentido==true){ usart_send_blocking(USART2, '+'); } else { usart_send_blocking(USART2, '-'); } my_usart_print_int(USART2, ref); //ADC-USART //Conversion analogica a digital en el pin PA1 adc_start_conversion_regular(ADC1); while (!(adc_eoc(ADC1))); temp=adc_read_regular(ADC1); if (gpio_get(GPIOA, GPIO0)) {//Si se presiona el boton de user ref=valor_de_referencia(); //Se va a la funcion para introducir un nuevo valor de referencia if(ref>100){ sentido=true;//+ ref=ref-100; } else { sentido=false;//- ref=ref*(-1); } if (cambio_sentido*ref<0){//si entre el ultimo valor de referencia y el actual hay un cambio de signo, apaga las senales para que el motor se detenga, luego de un tiempo vuelve a funcionar gpio_clear(GPIOE, GPIO14); gpio_clear(GPIOE, GPIO11); delay(5000); } cambio_sentido=ref; } //Imprimer el valor actual de la tension del tacometro pero entre el rango de 0 a 4096 (conversion analogica) usart_send_blocking(USART2, '\n'); usart_send_blocking(USART2, 'T'); usart_send_blocking(USART2, 'e'); usart_send_blocking(USART2, 'm'); usart_send_blocking(USART2, 'p'); usart_send_blocking(USART2, ':'); usart_send_blocking(USART2, ' '); my_usart_print_int(USART2, temp); //Control PI //Calculo de error normalizado e=((double)ref/(double)100) - ((double)temp/(double)4096);//se convierte result a double de las divisiones usart_send_blocking(USART2, 'e'); if (e>0){ usart_send_blocking(USART2, '+'); } else { usart_send_blocking(USART2, '-'); } my_usart_print_int(USART2, (int)(e*10000)); //Control proporcional P = e*kp; usart_send_blocking(USART2, 'P'); my_usart_print_int(USART2, (int)(P*10000)); //Anti windup if (control > 1.0){ control=1.0; I=Iant; } if (control < -1.0){ control=-1.0; I=Iant; } if(control>0){//el duty siempre tiene que ser positivo duty=control; } else { duty=control*(-1); } // Control Integral usart_send_blocking(USART2, 'I'); my_usart_print_int(USART2, (int)(I)*10000); //Senal de control PI control = P + I; usart_send_blocking(USART2, 'c'); if (control>0){ usart_send_blocking(USART2, '+'); } else { usart_send_blocking(USART2, '-'); } my_usart_print_int(USART2, (int)(control*10000)); //PWM timer_set_oc_value(TIM1, TIM_OC1, PWM_PERIOD_ARR*duty);//duty_cycle*pwm_period_ARR); //habilitar out1 e out2 segun el signo de control if((control)>0.001){ gpio_clear(GPIOE, GPIO14); gpio_set(GPIOE, GPIO11);//LD7 } if((control)<-0.001){ gpio_clear(GPIOE, GPIO11); gpio_set(GPIOE, GPIO14); } if ((control)<0.00001 && (control)>-0.00001){ gpio_clear(GPIOE, GPIO11); gpio_clear(GPIOE, GPIO14); gpio_set(GPIOE, GPIO15); //indica control cerca de cero } //Actualizar los valores Iant, I, Inew Iant = I; Inew = I + (kp/Ti)*e; I=Inew; vel=(temp/rango)*(3.25)*(680.0/5.0); //velocidad del motor calculado con el factor de conversion 680rpm/5V //Se imprime el valor de la velocidad usart_send_blocking(USART2, '\n'); usart_send_blocking(USART2, 'V'); usart_send_blocking(USART2, 'e'); usart_send_blocking(USART2, 'l'); usart_send_blocking(USART2, ':'); usart_send_blocking(USART2, ' '); my_usart_print_int(USART2, vel); } }
- NOTA: El código fuente utilizado para la implementación del sistema de control, está diseñado para una alimentación de
24Vpor lo que en caso de querer ejecutarse a12Vy que se realice una lectura correctar por parte del microcontrolador, se debe de ir a la sección en donde se calcula el error normalizado
//Calculo de error normalizado e=((double)ref/(double)100) - ((double)temp/(double)4096);
y cambiar el valor 4096 por 2048. Para valores distintos de tensión, se debe de escalar este valor y realizar los ajustes necesarios al divisor de tensiones de manera que la entrada analógica sólo reciba valores de entrada entre los 0V y 3,3V.
teaching/ie0624/proyectos/control_motores_sincronicos/codigo.txt · Last modified: 2022/09/20 00:08 (external edit)