Advertisement
Guest User

Untitled

a guest
Dec 17th, 2017
67
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 11.71 KB | None | 0 0
  1. /**
  2. ******************************************************************************
  3. * File Name : main.c
  4. * Description : Main program body
  5. ******************************************************************************
  6. ** This notice applies to any and all portions of this file
  7. * that are not between comment pairs USER CODE BEGIN and
  8. * USER CODE END. Other portions of this file, whether
  9. * inserted by the user or by software development tools
  10. * are owned by their respective copyright owners.
  11. *
  12. * COPYRIGHT(c) 2017 STMicroelectronics
  13. *
  14. * Redistribution and use in source and binary forms, with or without modification,
  15. * are permitted provided that the following conditions are met:
  16. * 1. Redistributions of source code must retain the above copyright notice,
  17. * this list of conditions and the following disclaimer.
  18. * 2. Redistributions in binary form must reproduce the above copyright notice,
  19. * this list of conditions and the following disclaimer in the documentation
  20. * and/or other materials provided with the distribution.
  21. * 3. Neither the name of STMicroelectronics nor the names of its contributors
  22. * may be used to endorse or promote products derived from this software
  23. * without specific prior written permission.
  24. *
  25. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  26. * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  27. * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  28. * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  29. * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  30. * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  31. * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  32. * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  33. * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  34. * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  35. *
  36. ******************************************************************************
  37. */
  38.  
  39. /* Includes ------------------------------------------------------------------*/
  40. #include "main.h"
  41. #include "stm32f4xx_hal.h"
  42. #include "adc.h"
  43. #include "dac.h"
  44. #include "dma.h"
  45. #include "tim.h"
  46. #include "gpio.h"
  47.  
  48. /* USER CODE BEGIN Includes */
  49. #include "math.h"
  50.  
  51. #define GRID_FREQ (float)50 //grid frequency
  52. #define SINE_RES (float)500 //sine resolution (points)
  53. #define CNT_FREQ (float)80000000 // uC frequency
  54. #define TIM_PERIOD (float)((CNT_FREQ)/((SINE_RES)*(GRID_FREQ))) //ADC timer autoreload value
  55. #define PI (float)3.14149
  56. #define GRID_WN (float)2*PI*GRID_FREQ //grid frequency in radians
  57. #define CNT_PERIOD (float)1/CNT_FREQ
  58. #define Kp (float) 40.0
  59. #define KiTs (float) 0.8 //Ts=0,00008s0.4 albo 0.8
  60. #define Ts (float) 1/((SINE_RES)*(GRID_FREQ)) //dla 500probek- 40us, nowa wartosc z ADC
  61.  
  62. //Do sprawdzenia Ts inny , w integratorze integIn ,KITs
  63.  
  64.  
  65. /* USER CODE END Includes */
  66.  
  67. /* Private variables ---------------------------------------------------------*/
  68.  
  69. /* USER CODE BEGIN PV */
  70. /* Private variables ---------------------------------------------------------*/
  71.  
  72. ////--------------Notch filter-----------------
  73. const float b1=-2.0;
  74. const float b2=1.0006;
  75. const float a1=-1.9950;
  76. const float a2=0.9956;
  77.  
  78.  
  79. ////--------------------PLL variables-------------
  80. float Mycos=0.0;
  81. float Mysin=0.0;
  82. float theta=0.0;
  83. float Upd[3]={0.0,0.0,0.0};//phase detect
  84. float ynotch[3]={0.0,0.0,0.0};
  85. float ylf[2]={0.0,0.0};
  86. float wo=0.0;
  87. float sinusek=0.0;
  88. float error=0.0;
  89. float pi_integral=0.0;
  90. float integ_in=0.0;
  91. float integ_out=0.0;
  92.  
  93.  
  94.  
  95. ////---------ADC tables--------------------------
  96. uint16_t odczyt500[500];
  97. uint16_t odczyt2[2];
  98. uint16_t adc_test;
  99. uint32_t set_dac;
  100.  
  101.  
  102. //---------External variables and counters----------
  103. volatile uint32_t iterator_htim10=0;
  104. volatile uint32_t iterator_main=0;
  105. volatile uint32_t iterator_time=0;
  106. extern volatile uint32_t iterator_tc;
  107. extern volatile uint8_t dma_flag;
  108. extern DMA_HandleTypeDef hdma_adc1;
  109. extern ADC_HandleTypeDef hadc1;
  110.  
  111.  
  112.  
  113. /* USER CODE END PV */
  114.  
  115. /* Private function prototypes -----------------------------------------------*/
  116. void SystemClock_Config(void);
  117.  
  118. /* USER CODE BEGIN PFP */
  119. /* Private function prototypes -----------------------------------------------*/
  120. //void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
  121. // if(htim->Instance == TIM10){
  122. // iterator_htim10=iterator_tc;
  123. // // HAL_GPIO_TogglePin(LED3_GPIO_Port,LED3_Pin);
  124. // }
  125. //}
  126. /* USER CODE END PFP */
  127.  
  128. /* USER CODE BEGIN 0 */
  129.  
  130. /* USER CODE END 0 */
  131.  
  132. int main(void)
  133. {
  134.  
  135. /* USER CODE BEGIN 1 */
  136.  
  137. /* USER CODE END 1 */
  138.  
  139. /* MCU Configuration----------------------------------------------------------*/
  140.  
  141. /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  142. HAL_Init();
  143.  
  144. /* USER CODE BEGIN Init */
  145.  
  146. /* USER CODE END Init */
  147.  
  148. /* Configure the system clock */
  149. SystemClock_Config();
  150.  
  151. /* USER CODE BEGIN SysInit */
  152.  
  153. /* USER CODE END SysInit */
  154.  
  155. /* Initialize all configured peripherals */
  156. MX_GPIO_Init();
  157. MX_DMA_Init();
  158. MX_ADC1_Init();
  159. MX_DAC_Init();
  160. MX_TIM2_Init();
  161. MX_TIM4_Init();
  162. //MX_TIM10_Init();
  163. // MX_TIM3_Init();
  164.  
  165. /* USER CODE BEGIN 2 */
  166.  
  167.  
  168. HAL_GPIO_WritePin(LED_GPIO_Port,LED_Pin,GPIO_PIN_SET);
  169. HAL_GPIO_WritePin(LED2_GPIO_Port,LED2_Pin,GPIO_PIN_RESET);
  170. HAL_GPIO_WritePin(LED3_GPIO_Port,LED3_Pin,GPIO_PIN_RESET);
  171.  
  172. HAL_TIM_Base_Start(&htim2);//trigeruje adeczka
  173. //HAL_TIM_Base_Start(&htim4);//trigeruje daczka
  174. HAL_TIM_Base_Start(&htim3);//mierzy czas, taktowanie 100 000Hz, max 65ms
  175. // HAL_TIM_Base_Start_IT(&htim10);//pamietac zeby to wyłączyc
  176.  
  177. // HAL_DAC_Start_DMA(&hdac,DAC_CHANNEL_1,(uint32_t*)tablicaDAC,1000,DAC_ALIGN_12B_R);
  178. HAL_DAC_Start(&hdac,DAC_CHANNEL_1);
  179. HAL_ADC_MspInit(&hadc1);
  180. HAL_ADC_Start_DMA(&hadc1,(uint32_t*)odczyt2,2);
  181. __HAL_DMA_ENABLE_IT(&hdma_adc1, DMA_IT_TC||DMA_IT_HT);
  182. HAL_DMA_IRQHandler(&hdma_adc1);
  183. HAL_ADC_Start(&hadc1);
  184.  
  185. /* USER CODE END 2 */
  186.  
  187. /* Infinite loop */
  188. /* USER CODE BEGIN WHILE */
  189. uint8_t start=0;
  190.  
  191.  
  192. //SPRAWDZIC INNE TS!!!!!!!!!!!!!
  193. while (1)
  194. {
  195. if(dma_flag==1){
  196. if(start==0){
  197. __HAL_TIM_SET_COUNTER(&htim3,0x0000);
  198. start+=1;
  199. }
  200. //iterator_main+=1;
  201. dma_flag=0;
  202.  
  203. //-------------------------------------
  204. //----- INPUT SINUS--------------------
  205. float tempSin=0.0;
  206. tempSin=odczyt2[0]+odczyt2[1];
  207. tempSin=tempSin/2;//średnia z 2 odczytanych wartosci
  208. tempSin=tempSin/2048.0;
  209. tempSin=tempSin-1;
  210. sinusek=tempSin;//input sinus wave, range from -1 to 1
  211.  
  212. //-------------------------------------
  213. //----- Phase detect--------------------
  214. Upd[0]=sinusek*Mycos;
  215.  
  216. //-------------------------------------
  217. //----- NOTCH FILTER--------------------
  218. float tempNotch=0.0;
  219. tempNotch=Upd[0]+b1*Upd[1];
  220. tempNotch+=b2*Upd[2];
  221. tempNotch-=a1*ynotch[1];
  222. tempNotch-=a2*ynotch[2];
  223.  
  224.  
  225. Upd[2]=Upd[1];
  226. Upd[1]=Upd[0];
  227.  
  228. ynotch[2]=ynotch[1];
  229. ynotch[1]=tempNotch;//sprawdzic to jeszcze
  230. ynotch[0]=tempNotch;
  231. //-------------------------------------
  232. //----- PI LOOP FILTER--------------------
  233. // ylf[0]=ylf[1]+PI0*ynotch[0]+PI1*ynotch[1];
  234.  
  235. error=ynotch[0]-0.0; //jak cos -Fdb
  236. //if(abs(error)>0.9 && iterator_time>0){
  237. // __HAL_TIM_SET_COUNTER(&htim3,0x0000);
  238. // iterator_time=0;
  239. //}
  240.  
  241. pi_integral=pi_integral+error*KiTs;
  242. if(pi_integral>200.0) {//maksymalny syg z czesci całkujacej
  243. pi_integral=200.0;
  244. }else if (pi_integral<-200.0){
  245. pi_integral=-200.0;
  246. }
  247.  
  248. ylf[0]=Kp*error+pi_integral;
  249. if(ylf[0]>200.0) {//maksymalny syg sterujacy z regulatora
  250. ylf[0]=200.0;
  251. }else if (ylf[0]<-200.0){
  252. ylf[0]=-200.0;
  253. }
  254.  
  255.  
  256. //-------------------------------------
  257. //----- OMEGA0--------------------
  258. float omega_o=GRID_WN;
  259. omega_o=omega_o+ylf[0];
  260.  
  261. //-------------------------------------
  262. //----- INTEGRATION--------------------
  263.  
  264. float tempInt=0.0;
  265. float sample_time=Ts; //sprawdzic dla innej wartosci 40
  266.  
  267. tempInt=sample_time*(omega_o+integ_in);//?????
  268. tempInt+=integ_out;
  269.  
  270. if ( tempInt > 6.2831852) {
  271. tempInt = 0.0;
  272. }
  273. else if (tempInt < 0.0){
  274. tempInt = 0.0;
  275. }
  276.  
  277. integ_out=tempInt;
  278. integ_in=omega_o;
  279.  
  280. //-------------------------------------
  281. //----- PLL OUTPUT--------------------
  282. theta=integ_out;
  283. Mysin=sin(theta);
  284. Mycos=cos(theta);
  285.  
  286. float tempSin2=0.0;
  287. tempSin2=Mysin+1;
  288. tempSin2=tempSin2*1890;//do3V
  289. set_dac=(uint32_t)tempSin2;
  290. if(abs(error)<0.5 && iterator_time==0){
  291. iterator_time=__HAL_TIM_GET_COUNTER(&htim3);
  292. }
  293.  
  294. HAL_DAC_SetValue(&hdac,DAC_CHANNEL_1,DAC_ALIGN_12B_R,set_dac);
  295. //wyslac na daczku cos
  296.  
  297.  
  298.  
  299. }
  300. /* USER CODE END WHILE */
  301.  
  302. /* USER CODE BEGIN 3 */
  303.  
  304. }
  305. /* USER CODE END 3 */
  306.  
  307. }
  308.  
  309. /** System Clock Configuration
  310. */
  311. void SystemClock_Config(void)
  312. {
  313.  
  314. RCC_OscInitTypeDef RCC_OscInitStruct;
  315. RCC_ClkInitTypeDef RCC_ClkInitStruct;
  316.  
  317. /**Configure the main internal regulator output voltage
  318. */
  319. __HAL_RCC_PWR_CLK_ENABLE();
  320.  
  321. __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);
  322.  
  323. /**Initializes the CPU, AHB and APB busses clocks
  324. */
  325. RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  326. RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  327. RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  328. RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  329. RCC_OscInitStruct.PLL.PLLM = 4;
  330. RCC_OscInitStruct.PLL.PLLN = 80;
  331. RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  332. RCC_OscInitStruct.PLL.PLLQ = 2;
  333. RCC_OscInitStruct.PLL.PLLR = 2;
  334. if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  335. {
  336. _Error_Handler(__FILE__, __LINE__);
  337. }
  338.  
  339. /**Initializes the CPU, AHB and APB busses clocks
  340. */
  341. RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
  342. |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  343. RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  344. RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  345. RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  346. RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
  347.  
  348. if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  349. {
  350. _Error_Handler(__FILE__, __LINE__);
  351. }
  352.  
  353. /**Configure the Systick interrupt time
  354. */
  355. HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
  356.  
  357. /**Configure the Systick
  358. */
  359. HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
  360.  
  361. /* SysTick_IRQn interrupt configuration */
  362. HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
  363. }
  364.  
  365. /* USER CODE BEGIN 4 */
  366.  
  367. /* USER CODE END 4 */
  368.  
  369. /**
  370. * @brief This function is executed in case of error occurrence.
  371. * @param None
  372. * @retval None
  373. */
  374. void _Error_Handler(char * file, int line)
  375. {
  376. /* USER CODE BEGIN Error_Handler_Debug */
  377. /* User can add his own implementation to report the HAL error return state */
  378. while(1)
  379. {
  380. }
  381. /* USER CODE END Error_Handler_Debug */
  382. }
  383.  
  384. #ifdef USE_FULL_ASSERT
  385.  
  386. /**
  387. * @brief Reports the name of the source file and the source line number
  388. * where the assert_param error has occurred.
  389. * @param file: pointer to the source file name
  390. * @param line: assert_param error line source number
  391. * @retval None
  392. */
  393. void assert_failed(uint8_t* file, uint32_t line)
  394. {
  395. /* USER CODE BEGIN 6 */
  396. /* User can add his own implementation to report the file name and line number,
  397. ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  398. /* USER CODE END 6 */
  399.  
  400. }
  401.  
  402. #endif
  403.  
  404. /**
  405. * @}
  406. */
  407.  
  408. /**
  409. * @}
  410. */
  411.  
  412. /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement