Advertisement
Guest User

Untitled

a guest
Jan 22nd, 2020
104
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 9.59 KB | None | 0 0
  1. #include "mcu_support_package/inc/stm32f10x.h"
  2. #include "signal.h"
  3. #include "stdbool.h"
  4. #define PI 3.1416
  5. #define VT 0.005
  6.  
  7. static const int32_t angle_sensor_resolution = 1024;
  8. static const double inertia_Kg_m_m = 0.000005;              // 5e-006
  9. static const double electrical_time_constant_s = 0.00005;   //5e-005;
  10. static const int32_t resistance_ohm = 1;
  11. static const double bemf_constant_V_Rad_s = 0.005;
  12. static const double torque_constant_Nm_A = 0.005;
  13. static const int32_t power_voltage_V = 5;
  14.  
  15. sig_atomic_t volatile current_1 = 0;
  16. sig_atomic_t volatile current_2 = 0;
  17. sig_atomic_t volatile sample_time = 0;
  18. sig_atomic_t volatile regulator_period = 0;
  19. sig_atomic_t volatile regulator_period_1 = 0;
  20.  
  21. int32_t angular_vel = 0;        //глобальная, чтобы отслеживать
  22. int32_t desired_vel = 500;      //глобальная, чтобы отслеживать
  23. int32_t desired_cur = 0;        //глобальная, чтобы отслеживать
  24. int32_t desired_voltage = 0;
  25. static const int tim3_period = 100000;
  26.  
  27. void Init_Peripheral(void);
  28.  
  29. void TIM1_UP_IRQHandler(void)
  30. {
  31.     TIM_ClearFlag(TIM1, TIM_FLAG_Update);
  32. }
  33.  
  34. void TIM3_IRQHandler(void)
  35. {
  36.     TIM_ClearFlag(TIM3, TIM_FLAG_Update);
  37.     sample_time++;
  38.     regulator_period++;
  39.     regulator_period_1++;
  40. }
  41.  
  42. int32_t PI_velocity(const double error)//ПИ регулятор, возвращающий желаемый ток
  43. {  
  44.     const double Kp = 10;
  45.     const double Ki = 10;
  46.     int32_t desired_val = 0;
  47.     static double error_sum = 0;
  48.     error_sum += error * regulator_period / tim3_period;
  49.     regulator_period = 0;
  50.     desired_val =(int32_t)( Kp * error + Ki * (error_sum) );
  51.      
  52.     //overflow protection 30% от максимального значения на выходе
  53.     double overflow = 100000;
  54.     if (error_sum > overflow)
  55.         error_sum = overflow;
  56.     if (error_sum < -overflow)
  57.         error_sum = -overflow;
  58.    
  59.     return desired_val;
  60. }
  61.  
  62. int32_t PI_current(const double error1)//ПИ регулятор, возвращающий желаемое напряжение
  63. {  
  64.     const double Kp_с = 10;
  65.     const double Ki_с = 10;
  66.     int32_t desired_val1 = 0;
  67.     static double error_sum1 = 0;
  68.     error_sum1 += error1 * regulator_period_1 / tim3_period;
  69.     regulator_period_1 = 0;
  70.     desired_val1 = (int32_t)( Kp_с * error1 + Ki_с * (error_sum1) );
  71.        
  72.     //overflow protection 30% от максимального значения на выходе
  73.     double overflow1 = 100000;
  74.     if (error_sum1 > overflow1)
  75.         error_sum1 = overflow1;
  76.     if (error_sum1 < -overflow1)
  77.         error_sum1 = -overflow1;
  78.    
  79.     return desired_val1;
  80. }
  81.  
  82. void ADC1_2_IRQHandler(void)
  83. {//ловим токи инжектированными каналами АЦП
  84.     current_1 = ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_1) - 2048;
  85.     current_2 = ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_2) - 2048;
  86. }
  87.  
  88. int main(void)
  89. {
  90.     Init_Peripheral();
  91.    
  92.     int32_t prev_position = 0;
  93.     int32_t position = 0;
  94.     double path = 0;
  95.     TIM_SetCompare1(TIM1, 0*50/100);
  96.     TIM_SetCompare2(TIM1, 0);       //подать 0.6U питания
  97.    
  98.     while (1)
  99.     {
  100.         position =  TIM2->CNT; //положение TIM_GetCounter(TIM2)
  101.         int32_t diff = (position - prev_position);
  102.         if (diff > (angle_sensor_resolution/2))
  103.         {
  104.             path -= angle_sensor_resolution;
  105.             path += diff;
  106.         }
  107.         else
  108.         {
  109.             if (diff < (-angle_sensor_resolution/2))
  110.             {
  111.                 path += angle_sensor_resolution;
  112.                 path += diff;
  113.             }
  114.             else path += diff;
  115.         }
  116.        
  117.         if (sample_time >= VT * tim3_period)
  118.         {   //срабатывает раз в 0.005 с
  119.             angular_vel = (int32_t)2 * PI * tim3_period * path / (angle_sensor_resolution * sample_time);//угловая скорость
  120.             sample_time = 0;
  121.             path = 0;
  122.         }
  123.         prev_position = position;//прежнее значение позиции
  124.            
  125.         desired_cur = PI_velocity(desired_vel - angular_vel);   //регулировка скорости
  126.         //desired_cur = 2048.0 * desired_cur / 3000.0;      //нормировка по максимальному значению на выходе
  127.        
  128.         if (desired_voltage > 0)
  129.         {//вращаемся вперёд
  130.             if(current_1 > current_2)
  131.                 desired_voltage = PI_current(desired_cur - current_1);
  132.             else desired_voltage = PI_current(desired_cur - current_2);//регулировка тока
  133.         }
  134.        
  135.         else
  136.         {//вращаемся назад
  137.             if(current_1 > current_2)
  138.                 desired_voltage = PI_current(desired_cur + current_1);
  139.             else desired_voltage = PI_current(desired_cur + current_2);//регулировка тока
  140.         }
  141.        
  142.         if (desired_voltage > power_voltage_V) desired_voltage = power_voltage_V;//ограничение по наибольшему значению на выходе
  143.         if (desired_voltage < -power_voltage_V) desired_voltage = -power_voltage_V;
  144.        
  145.         if (desired_voltage >= 0)
  146.         {
  147.             TIM_SetCompare1(TIM1, desired_voltage);
  148.             //TIM_SetCompare1(TIM1, desired_voltage * 50 / 9000);//нормировка по наибольшему значению на выходе
  149.             TIM_SetCompare2(TIM1, 0);
  150.         }
  151.         else
  152.         {
  153.             TIM_SetCompare1(TIM1, 0);
  154.             TIM_SetCompare2(TIM1, -desired_voltage);
  155.             //TIM_SetCompare2(TIM1,  -desired_voltage * 50 / 9000);
  156.         }
  157.     }
  158.     for (int32_t k = 0; k < 20000; k++) {;}
  159. };
  160.  
  161. void Init_Peripheral(void)
  162. {
  163.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
  164.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
  165.     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
  166.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
  167.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
  168.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
  169.    
  170.     GPIO_InitTypeDef PortA;
  171.     PortA.GPIO_Mode = GPIO_Mode_AF_PP;
  172.     PortA.GPIO_Pin = GPIO_Pin_8;
  173.     PortA.GPIO_Speed = GPIO_Speed_50MHz;
  174.     GPIO_Init(GPIOA, &PortA); //настройка шима TIM1CH1 PA8
  175.  
  176.     GPIO_InitTypeDef PA;
  177.     PA.GPIO_Mode = GPIO_Mode_AF_PP;
  178.     PA.GPIO_Pin = GPIO_Pin_9;
  179.     PA.GPIO_Speed = GPIO_Speed_50MHz;
  180.     GPIO_Init(GPIOA, &PA); //настройка шима TIM1CH2 PA9
  181.    
  182.     //настройка АЦП
  183.     PortA.GPIO_Mode = GPIO_Mode_AIN;
  184.     PortA.GPIO_Pin = GPIO_Pin_0;
  185.     GPIO_Init(GPIOA, &PortA); //настройка ADC1CH0
  186.    
  187.     PortA.GPIO_Mode = GPIO_Mode_AIN;
  188.     PortA.GPIO_Pin = GPIO_Pin_1;
  189.     GPIO_Init(GPIOA, &PortA); //настройка ADC1CH1
  190.    
  191.     //настройка инжектированных каналов
  192.     ADC_InitTypeDef adc;
  193.     adc.ADC_Mode = ADC_Mode_Independent;
  194.     adc.ADC_ScanConvMode = ENABLE;
  195.     adc.ADC_ContinuousConvMode = ENABLE;
  196.     adc.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
  197.     adc.ADC_DataAlign = ADC_DataAlign_Right;
  198.     adc.ADC_NbrOfChannel = 2;
  199.     ADC_Init(ADC1,&adc);
  200.     ADC_InjectedSequencerLengthConfig(ADC1,2);
  201.     ADC_InjectedChannelConfig(ADC1,ADC_Channel_0,1,ADC_SampleTime_55Cycles5);
  202.     ADC_InjectedChannelConfig(ADC1,ADC_Channel_1,2,ADC_SampleTime_55Cycles5);
  203.    
  204.     ADC_ExternalTrigInjectedConvConfig( ADC1, ADC_ExternalTrigInjecConv_None );
  205.    
  206.     RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
  207.    
  208.     TIM_TimeBaseInitTypeDef tim1_struct;
  209.     TIM_TimeBaseStructInit( & tim1_struct);
  210.     tim1_struct.TIM_Prescaler = 71;
  211.     tim1_struct.TIM_Period = 50;
  212.     tim1_struct.TIM_CounterMode = TIM_CounterMode_Up;
  213.     TIM_TimeBaseInit(TIM1, &tim1_struct); //настройка срабатывания таймера TIM1 20 000 раз в секунду
  214.     TIM_CtrlPWMOutputs(TIM1,ENABLE);
  215.    
  216.     TIM_TimeBaseInitTypeDef tim3_struct;
  217.     TIM_TimeBaseStructInit( & tim3_struct);
  218.     tim3_struct.TIM_Prescaler = 71;
  219.     tim3_struct.TIM_Period = 10;
  220.     tim3_struct.TIM_CounterMode = TIM_CounterMode_Up;
  221.     TIM_TimeBaseInit(TIM3, &tim3_struct); //настройка срабатывания таймера TIM3 100 000 раз в секунду
  222.    
  223.     TIM_OCInitTypeDef pa8_timer_channel;
  224.     TIM_OCStructInit( & pa8_timer_channel);
  225.     pa8_timer_channel.TIM_OCMode = TIM_OCMode_PWM1;
  226.     pa8_timer_channel.TIM_OutputState = TIM_OutputState_Enable;
  227.     TIM_OC1Init(TIM1, &pa8_timer_channel); //настройка ШИМ ножки для таймера TIM1CH1
  228.    
  229.     TIM_OCInitTypeDef pa9_timer_channel;
  230.     TIM_OCStructInit( & pa9_timer_channel);
  231.     pa9_timer_channel.TIM_OCMode = TIM_OCMode_PWM1;
  232.     pa9_timer_channel.TIM_OutputState = TIM_OutputState_Enable;
  233.     TIM_OC2Init(TIM1, &pa9_timer_channel); //настройка ШИМ ножки для таймера TIM1CH2
  234.    
  235.    
  236.     //включение прерываний
  237.     ADC_ITConfig(ADC1,ADC_IT_EOC,ENABLE);
  238.     TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
  239.     TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
  240.     NVIC_EnableIRQ(ADC1_2_IRQn);
  241.     NVIC_EnableIRQ(TIM1_UP_IRQn);
  242.     NVIC_EnableIRQ(TIM3_IRQn);
  243.    
  244.     TIM_Cmd(TIM1, ENABLE);
  245.     TIM_Cmd(TIM3, ENABLE);
  246.     ADC_Cmd(ADC1,ENABLE);
  247.  
  248.     //калибровка АЦП
  249.     ADC_ResetCalibration(ADC1);
  250.     while(ADC_GetResetCalibrationStatus(ADC1)){;}
  251.     ADC_StartCalibration(ADC1);
  252.     while(ADC_GetCalibrationStatus(ADC1)){;}
  253.        
  254.     ADC_AutoInjectedConvCmd(ADC1,ENABLE);
  255.     ADC_SoftwareStartConvCmd(ADC1, ENABLE);
  256.     //ADC_SoftwareStartInjectedConvCmd(ADC1,ENABLE);
  257. }
  258.  
  259. // классический ассерт для STM32
  260. #ifdef USE_FULL_ASSERT
  261.     void assert_failed(uint8_t * file, uint32_t line)
  262.     {
  263.         /* User can add his own implementation to report the file name and line number,
  264.          ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  265.          
  266.         (void)file;
  267.         (void)line;
  268.  
  269.         while(1){};
  270.     }
  271. #endif
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement