Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "mcu_support_package/inc/stm32f10x.h"
- #include "signal.h"
- #include "stdbool.h"
- #define PI 3.1416
- #define VT 0.005
- static const int32_t angle_sensor_resolution = 1024;
- static const double inertia_Kg_m_m = 0.000005; // 5e-006
- static const double electrical_time_constant_s = 0.00005; //5e-005;
- static const int32_t resistance_ohm = 1;
- static const double bemf_constant_V_Rad_s = 0.005;
- static const double torque_constant_Nm_A = 0.005;
- static const int32_t power_voltage_V = 5;
- sig_atomic_t volatile current_1 = 0;
- sig_atomic_t volatile current_2 = 0;
- sig_atomic_t volatile sample_time = 0;
- sig_atomic_t volatile regulator_period = 0;
- sig_atomic_t volatile regulator_period_1 = 0;
- int32_t angular_vel = 0; //глобальная, чтобы отслеживать
- int32_t desired_vel = 500; //глобальная, чтобы отслеживать
- int32_t desired_cur = 0; //глобальная, чтобы отслеживать
- int32_t desired_voltage = 0;
- static const int tim3_period = 100000;
- void Init_Peripheral(void);
- void TIM1_UP_IRQHandler(void)
- {
- TIM_ClearFlag(TIM1, TIM_FLAG_Update);
- }
- void TIM3_IRQHandler(void)
- {
- TIM_ClearFlag(TIM3, TIM_FLAG_Update);
- sample_time++;
- regulator_period++;
- regulator_period_1++;
- }
- int32_t PI_velocity(const double error)//ПИ регулятор, возвращающий желаемый ток
- {
- const double Kp = 10;
- const double Ki = 10;
- int32_t desired_val = 0;
- static double error_sum = 0;
- error_sum += error * regulator_period / tim3_period;
- regulator_period = 0;
- desired_val =(int32_t)( Kp * error + Ki * (error_sum) );
- //overflow protection 30% от максимального значения на выходе
- double overflow = 100000;
- if (error_sum > overflow)
- error_sum = overflow;
- if (error_sum < -overflow)
- error_sum = -overflow;
- return desired_val;
- }
- int32_t PI_current(const double error1)//ПИ регулятор, возвращающий желаемое напряжение
- {
- const double Kp_с = 10;
- const double Ki_с = 10;
- int32_t desired_val1 = 0;
- static double error_sum1 = 0;
- error_sum1 += error1 * regulator_period_1 / tim3_period;
- regulator_period_1 = 0;
- desired_val1 = (int32_t)( Kp_с * error1 + Ki_с * (error_sum1) );
- //overflow protection 30% от максимального значения на выходе
- double overflow1 = 100000;
- if (error_sum1 > overflow1)
- error_sum1 = overflow1;
- if (error_sum1 < -overflow1)
- error_sum1 = -overflow1;
- return desired_val1;
- }
- void ADC1_2_IRQHandler(void)
- {//ловим токи инжектированными каналами АЦП
- current_1 = ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_1) - 2048;
- current_2 = ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_2) - 2048;
- }
- int main(void)
- {
- Init_Peripheral();
- int32_t prev_position = 0;
- int32_t position = 0;
- double path = 0;
- TIM_SetCompare1(TIM1, 0*50/100);
- TIM_SetCompare2(TIM1, 0); //подать 0.6U питания
- while (1)
- {
- position = TIM2->CNT; //положение TIM_GetCounter(TIM2)
- int32_t diff = (position - prev_position);
- if (diff > (angle_sensor_resolution/2))
- {
- path -= angle_sensor_resolution;
- path += diff;
- }
- else
- {
- if (diff < (-angle_sensor_resolution/2))
- {
- path += angle_sensor_resolution;
- path += diff;
- }
- else path += diff;
- }
- if (sample_time >= VT * tim3_period)
- { //срабатывает раз в 0.005 с
- angular_vel = (int32_t)2 * PI * tim3_period * path / (angle_sensor_resolution * sample_time);//угловая скорость
- sample_time = 0;
- path = 0;
- }
- prev_position = position;//прежнее значение позиции
- desired_cur = PI_velocity(desired_vel - angular_vel); //регулировка скорости
- //desired_cur = 2048.0 * desired_cur / 3000.0; //нормировка по максимальному значению на выходе
- if (desired_voltage > 0)
- {//вращаемся вперёд
- if(current_1 > current_2)
- desired_voltage = PI_current(desired_cur - current_1);
- else desired_voltage = PI_current(desired_cur - current_2);//регулировка тока
- }
- else
- {//вращаемся назад
- if(current_1 > current_2)
- desired_voltage = PI_current(desired_cur + current_1);
- else desired_voltage = PI_current(desired_cur + current_2);//регулировка тока
- }
- if (desired_voltage > power_voltage_V) desired_voltage = power_voltage_V;//ограничение по наибольшему значению на выходе
- if (desired_voltage < -power_voltage_V) desired_voltage = -power_voltage_V;
- if (desired_voltage >= 0)
- {
- TIM_SetCompare1(TIM1, desired_voltage);
- //TIM_SetCompare1(TIM1, desired_voltage * 50 / 9000);//нормировка по наибольшему значению на выходе
- TIM_SetCompare2(TIM1, 0);
- }
- else
- {
- TIM_SetCompare1(TIM1, 0);
- TIM_SetCompare2(TIM1, -desired_voltage);
- //TIM_SetCompare2(TIM1, -desired_voltage * 50 / 9000);
- }
- }
- for (int32_t k = 0; k < 20000; k++) {;}
- };
- void Init_Peripheral(void)
- {
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
- GPIO_InitTypeDef PortA;
- PortA.GPIO_Mode = GPIO_Mode_AF_PP;
- PortA.GPIO_Pin = GPIO_Pin_8;
- PortA.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOA, &PortA); //настройка шима TIM1CH1 PA8
- GPIO_InitTypeDef PA;
- PA.GPIO_Mode = GPIO_Mode_AF_PP;
- PA.GPIO_Pin = GPIO_Pin_9;
- PA.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(GPIOA, &PA); //настройка шима TIM1CH2 PA9
- //настройка АЦП
- PortA.GPIO_Mode = GPIO_Mode_AIN;
- PortA.GPIO_Pin = GPIO_Pin_0;
- GPIO_Init(GPIOA, &PortA); //настройка ADC1CH0
- PortA.GPIO_Mode = GPIO_Mode_AIN;
- PortA.GPIO_Pin = GPIO_Pin_1;
- GPIO_Init(GPIOA, &PortA); //настройка ADC1CH1
- //настройка инжектированных каналов
- ADC_InitTypeDef adc;
- adc.ADC_Mode = ADC_Mode_Independent;
- adc.ADC_ScanConvMode = ENABLE;
- adc.ADC_ContinuousConvMode = ENABLE;
- adc.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
- adc.ADC_DataAlign = ADC_DataAlign_Right;
- adc.ADC_NbrOfChannel = 2;
- ADC_Init(ADC1,&adc);
- ADC_InjectedSequencerLengthConfig(ADC1,2);
- ADC_InjectedChannelConfig(ADC1,ADC_Channel_0,1,ADC_SampleTime_55Cycles5);
- ADC_InjectedChannelConfig(ADC1,ADC_Channel_1,2,ADC_SampleTime_55Cycles5);
- ADC_ExternalTrigInjectedConvConfig( ADC1, ADC_ExternalTrigInjecConv_None );
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
- TIM_TimeBaseInitTypeDef tim1_struct;
- TIM_TimeBaseStructInit( & tim1_struct);
- tim1_struct.TIM_Prescaler = 71;
- tim1_struct.TIM_Period = 50;
- tim1_struct.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseInit(TIM1, &tim1_struct); //настройка срабатывания таймера TIM1 20 000 раз в секунду
- TIM_CtrlPWMOutputs(TIM1,ENABLE);
- TIM_TimeBaseInitTypeDef tim3_struct;
- TIM_TimeBaseStructInit( & tim3_struct);
- tim3_struct.TIM_Prescaler = 71;
- tim3_struct.TIM_Period = 10;
- tim3_struct.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseInit(TIM3, &tim3_struct); //настройка срабатывания таймера TIM3 100 000 раз в секунду
- TIM_OCInitTypeDef pa8_timer_channel;
- TIM_OCStructInit( & pa8_timer_channel);
- pa8_timer_channel.TIM_OCMode = TIM_OCMode_PWM1;
- pa8_timer_channel.TIM_OutputState = TIM_OutputState_Enable;
- TIM_OC1Init(TIM1, &pa8_timer_channel); //настройка ШИМ ножки для таймера TIM1CH1
- TIM_OCInitTypeDef pa9_timer_channel;
- TIM_OCStructInit( & pa9_timer_channel);
- pa9_timer_channel.TIM_OCMode = TIM_OCMode_PWM1;
- pa9_timer_channel.TIM_OutputState = TIM_OutputState_Enable;
- TIM_OC2Init(TIM1, &pa9_timer_channel); //настройка ШИМ ножки для таймера TIM1CH2
- //включение прерываний
- ADC_ITConfig(ADC1,ADC_IT_EOC,ENABLE);
- TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
- TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
- NVIC_EnableIRQ(ADC1_2_IRQn);
- NVIC_EnableIRQ(TIM1_UP_IRQn);
- NVIC_EnableIRQ(TIM3_IRQn);
- TIM_Cmd(TIM1, ENABLE);
- TIM_Cmd(TIM3, ENABLE);
- ADC_Cmd(ADC1,ENABLE);
- //калибровка АЦП
- ADC_ResetCalibration(ADC1);
- while(ADC_GetResetCalibrationStatus(ADC1)){;}
- ADC_StartCalibration(ADC1);
- while(ADC_GetCalibrationStatus(ADC1)){;}
- ADC_AutoInjectedConvCmd(ADC1,ENABLE);
- ADC_SoftwareStartConvCmd(ADC1, ENABLE);
- //ADC_SoftwareStartInjectedConvCmd(ADC1,ENABLE);
- }
- // классический ассерт для STM32
- #ifdef USE_FULL_ASSERT
- void assert_failed(uint8_t * file, uint32_t line)
- {
- /* User can add his own implementation to report the file name and line number,
- ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
- (void)file;
- (void)line;
- while(1){};
- }
- #endif
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement