====== 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.
/*
* 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 .
*/
#include
#include
#include //analog digital converter
#include
#include
#include
#include "proyecto.h"
#include
#include
#include
#include
//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 ''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
//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|Tabla de Contenidos]]