ArcosWiki

User Tools

Site Tools

Trace:
Writing /var/lib/dokuwiki/data/meta/teaching/ie0624/proyectos/control_motores_sincronicos/codigo.meta failed
teaching:ie0624:proyectos:control_motores_sincronicos:codigo

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Next revision		Previous revision
teaching:ie0624:proyectos:control_motores_sincronicos:codigo [2018/12/17 03:07] – created microsteaching:ie0624:proyectos:control_motores_sincronicos:codigo [2022/09/20 00:08] (current) – external edit 127.0.0.1
Line 1: Line 1:
-===== Código Fuente Utilizado para la Implementación del Control =====+====== 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 [[teaching:ie0624:proyectos:control_motores_sincronicos:codigo:diagrama|diagrama de flujo]] del sistema implementado. 
 + 
 +<code C> 
 + 
 +/* 
 +   * 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); 
 + 
 +  
 +     } 
 +  } 
 + 
 + 
 +</code> 
 + 
 + 
 +  * **NOTA:** El código fuente utilizado para la implementación del sistema de control, está diseñado para una alimentación de ''24V'' por lo que en caso de querer ejecutarse a ''12V'' y 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  
 +  
 +<code C> 
 + //Calculo de error normalizado 
 +     e=((double)ref/(double)100) - ((double)temp/(double)4096); 
 +</code> 
 + 
 +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|Tabla de Contenidos]]
teaching/ie0624/proyectos/control_motores_sincronicos/codigo.1545016053.txt.gz · Last modified: 2022/09/20 00:08 (external edit)