API tools faq
paste
Advertisement
MariaSam

Untitled

MariaSam
Jan 19th, 2020
93
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.96 KB | None | 0 0
raw download clone embed print report
  1. #include "mcu_support_package/inc/stm32f10x.h"
  2. #include "signal.h"
  3. #include "stdbool.h"
  4. #include "math.h"
  5.  
  6. #define dt 0.005
  7. #define PI 3.1415926
  8. #define sqrt_3 1.7320508075688772935274463415059
  9.  
  10. int current_A = 0;
  11. int current_B = 0;
  12. int current_C = 0;
  13. int sample_time = 0;
  14. int PI_current_period_q=0;
  15. int PI_current_period_d=0;
  16. int PI_velocity_period=0;
  17. static int angular_velocity = 0;
  18. static int desired_velocity = 100;//желаемая скорость
  19. static int desired_current=0;
  20. int prev_position = 0;
  21. int position = 0;
  22. int desired_voltage = 0;
  23. int final_position = 0;
  24. int desired_current_d;
  25. int desired_current_q;
  26. int desired_voltage_d = 0;
  27. int desired_voltage_q = 0;
  28. int voltage_A =0;
  29. int voltage_B=0;
  30. int voltage_C=0;
  31. int Id=0;
  32. int Iq=0;
  33.  
  34. void TIM1_UP_IRQHandler(void) {
  35. TIM_ClearFlag(TIM1, TIM_FLAG_Update);
  36. }
  37.  
  38. void TIM3_IRQHandler(void) {
  39. TIM_ClearFlag(TIM3, TIM_FLAG_Update);
  40. sample_time++;
  41. PI_current_period_q++;
  42. PI_current_period_d++;
  43. PI_velocity_period++;
  44. }
  45.  
  46. float PI_velocity(const float error, const float Kp, const float Ki){//ПИ регулятор, возвращающий желаемый ток
  47. float ext_value = 0;
  48. static float integral = 0;
  49. integral += error * PI_velocity_period/100000;
  50. ext_value = Kp * error + Ki * integral;
  51.  
  52. //30% от максимального значения на выходе
  53. if (integral > 60) integral = 60;
  54. if (integral < -60) integral = -60;
  55. return ext_value;
  56. }
  57.  
  58. float PI_current_q(const float error, const float Kp, const float Ki ){ //ПИ регулятор, возвращающий желаемое напряжение
  59. float ext_value = 0;
  60. static float integral = 0;
  61. integral += error * PI_current_period_q/100000;
  62. ext_value= Kp * error + Ki * integral;
  63.  
  64. //30% от максимального значения на выходе
  65. if (integral > 180) integral= 180;
  66. if (integral < -180) integral = -180;
  67. return ext_value;
  68. }
  69.  
  70. float PI_current_d(const float error, const float Kp, const float Ki ){ //ПИ регулятор, возвращающий желаемое напряжение
  71. float ext_value = 0;
  72. static float integral = 0;
  73. integral += error * PI_current_period_d/100000;
  74. ext_value= Kp * error + Ki * integral;
  75.  
  76. //30% от максимального значения на выходе
  77. if (integral > 230) integral= 230;
  78. if (integral < -230) integral = -230;
  79. return ext_value;
  80. }
  81.  
  82. void ADC1_2_IRQHandler(void) {//ловим токи инжектированными каналами АЦП
  83.  
  84. current_A = ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_1) - 2048;
  85. current_B = ADC_GetInjectedConversionValue(ADC1,ADC_InjectedChannel_2) - 2048;
  86. current_C = -current_A - current_B;
  87. }
  88.  
  89. void init_all() {
  90. RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
  91. RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
  92. RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
  93. RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
  94. RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
  95. RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
  96. RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
  97.  
  98. //настройка TIM1 и TIM3
  99. GPIO_InitTypeDef GPIOA_pwn;
  100. GPIOA_pwn.GPIO_Mode = GPIO_Mode_AF_PP;
  101. GPIOA_pwn.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9|GPIO_Pin_10;
  102. GPIOA_pwn.GPIO_Speed = GPIO_Speed_50MHz;
  103. GPIO_Init(GPIOA, &GPIOA_pwn);
  104.  
  105. TIM_TimeBaseInitTypeDef TIM1_init;
  106. TIM_TimeBaseStructInit( & TIM1_init);
  107. TIM1_init.TIM_Prescaler = 72-1;
  108. TIM1_init.TIM_Period = 500; //2000 раз в секунду
  109. TIM1_init.TIM_CounterMode = TIM_CounterMode_Up;
  110. TIM_TimeBaseInit(TIM1, &TIM1_init);
  111. TIM_CtrlPWMOutputs(TIM1,ENABLE);
  112.  
  113. TIM_TimeBaseInitTypeDef TIM3_init;
  114. TIM_TimeBaseStructInit( & TIM3_init);
  115. TIM3_init.TIM_Prescaler = 72-1;
  116. TIM3_init.TIM_Period = 10; //100 000 раз в секунду
  117. TIM3_init.TIM_CounterMode = TIM_CounterMode_Up;
  118. TIM_TimeBaseInit(TIM3, & TIM3_init);
  119.  
  120. TIM_OCInitTypeDef PA8_channel_pwn;
  121. TIM_OCStructInit( & PA8_channel_pwn);
  122. PA8_channel_pwn.TIM_OCMode = TIM_OCMode_PWM1;
  123. PA8_channel_pwn.TIM_OutputState = TIM_OutputState_Enable;
  124. TIM_OC1Init(TIM1, &PA8_channel_pwn);
  125.  
  126. TIM_OCInitTypeDef PA9_channel_pwn;
  127. TIM_OCStructInit( & PA9_channel_pwn);
  128. PA9_channel_pwn.TIM_OCMode = TIM_OCMode_PWM1;
  129. PA9_channel_pwn.TIM_OutputState = TIM_OutputState_Enable;
  130. TIM_OC2Init(TIM1, &PA9_channel_pwn);
  131.  
  132. TIM_OCInitTypeDef PA10_channel_pwn;
  133. TIM_OCStructInit( & PA10_channel_pwn);
  134. PA10_channel_pwn.TIM_OCMode = TIM_OCMode_PWM1;
  135. PA10_channel_pwn.TIM_OutputState = TIM_OutputState_Enable;
  136. TIM_OC3Init(TIM1, &PA10_channel_pwn);
  137.  
  138. //настройка АЦП
  139. GPIO_InitTypeDef PortA_adc;
  140. PortA_adc.GPIO_Mode = GPIO_Mode_AIN;
  141. PortA_adc.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1;
  142. GPIO_Init(GPIOA, &PortA_adc);
  143.  
  144. ADC_InitTypeDef ADC;
  145. ADC.ADC_Mode = ADC_Mode_Independent;
  146. ADC.ADC_ScanConvMode = ENABLE;
  147. ADC.ADC_ContinuousConvMode = ENABLE;
  148. ADC.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
  149. ADC.ADC_DataAlign = ADC_DataAlign_Right;
  150. ADC.ADC_NbrOfChannel = 2;
  151. ADC_Init(ADC1,&ADC);
  152. ADC_InjectedSequencerLengthConfig(ADC1,2);
  153. ADC_InjectedChannelConfig(ADC1,ADC_Channel_0,1,ADC_SampleTime_55Cycles5);
  154. ADC_InjectedChannelConfig(ADC1,ADC_Channel_1,2,ADC_SampleTime_55Cycles5);
  155.  
  156. ADC_ExternalTrigInjectedConvConfig( ADC1, ADC_ExternalTrigInjecConv_None );
  157.  
  158. //калибровка АЦП
  159. ADC_ResetCalibration(ADC1);
  160. while(ADC_GetResetCalibrationStatus(ADC1)){;}
  161. ADC_StartCalibration(ADC1);
  162. while(ADC_GetCalibrationStatus(ADC1)){;}
  163.  
  164. //включение прерываний
  165. ADC_ITConfig(ADC1,ADC_IT_EOC,ENABLE);
  166. TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
  167. TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
  168. NVIC_EnableIRQ(ADC1_2_IRQn);
  169. NVIC_EnableIRQ(TIM1_UP_IRQn);
  170. NVIC_EnableIRQ(TIM3_IRQn);
  171.  
  172. TIM_Cmd(TIM1, ENABLE);
  173. TIM_Cmd(TIM3, ENABLE);
  174. ADC_Cmd(ADC1,ENABLE);
  175.  
  176. ADC_AutoInjectedConvCmd(ADC1,ENABLE);
  177. ADC_SoftwareStartConvCmd(ADC1, ENABLE);
  178. }
  179.  
  180.  
  181.  
  182.  
  183. int main(void){
  184.  
  185. init_all();
  186.  
  187. while (1) {
  188. position = TIM2->CNT ; //положение ротора
  189. int position_difference = position - prev_position;
  190. if(position_difference > 512){
  191. final_position -= 1024;
  192. final_position += position_difference;
  193. }
  194. else {
  195. if (position_difference < -512){
  196. final_position += 1024;
  197. final_position += position_difference;
  198. }
  199. else final_position += position_difference;
  200. }
  201. if(sample_time >= dt * 100000){//срабатывает раз в 0.005 с
  202. angular_velocity = (int) 2 * PI * 100000 * final_position / (1024 * sample_time);//угловая скорость
  203. sample_time = 0;
  204. final_position = 0;
  205. }
  206. prev_position = position;
  207.  
  208. //переход от 3-х фазной системы к 2-х фазной
  209. double theta = 2*PI * position / 1024;
  210. double I_alpha = (double) current_A;//ось A = ось alpha
  211. double I_beta = (double)((current_B - current_C)/sqrt_3);//ось beta
  212. //проекции на оси ротора
  213. //поворот декартовой системы координат
  214. Id = (int) (I_beta * sin(theta)+ I_alpha * cos(theta));
  215. Iq = (int) (I_beta * cos(theta)-I_alpha * sin(theta));
  216.  
  217. desired_current_q = PI_velocity(desired_velocity - angular_velocity, 1.0, 8.0);//регулировка скорости
  218. desired_voltage_d = PI_current_d(desired_current_d - Id, 1.0, 3.0);//регулировка тока по оси d
  219. desired_voltage_q = PI_current_q(desired_current_q - Iq, 5.0, 1.0);//регулировка тока по оси q
  220.  
  221.  
  222. double U_alpha = desired_voltage_d * cos(theta) - desired_voltage_q * sin(theta);
  223. double U_beta = desired_voltage_q * cos(theta) + desired_voltage_d * sin(theta);
  224.  
  225.  
  226. voltage_A = (int)(250+250 * U_alpha / 12000.0);
  227. voltage_B = (int)(250+250 * 0.5 * (U_beta * sqrt_3 - U_alpha) / 12000.0);
  228. voltage_C = (int)(250-0.5 * 250 * (U_alpha + U_beta * sqrt_3) / 12000.0);
  229.  
  230. TIM_SetCompare1(TIM1, voltage_A);
  231. TIM_SetCompare2(TIM1, voltage_B);
  232. TIM_SetCompare3(TIM1, voltage_C);
  233. }
  234.  
  235. return 0;
  236. }
  237.  
  238.  
  239.  
  240. // классический ассерт для STM32
  241. #ifdef USE_FULL_ASSERT
  242. void assert_failed(uint8_t * file, uint32_t line)
  243. {
  244. /* User can add his own implementation to report the file name and line number,
  245. ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  246.  
  247. (void)file;
  248. (void)line;
  249.  
  250. while(1){};
  251. }
  252. #endif
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!