Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /**
- ******************************************************************************
- * File Name : main.c
- * Description : Main program body
- ******************************************************************************
- ** This notice applies to any and all portions of this file
- * that are not between comment pairs USER CODE BEGIN and
- * USER CODE END. Other portions of this file, whether
- * inserted by the user or by software development tools
- * are owned by their respective copyright owners.
- *
- * COPYRIGHT(c) 2017 STMicroelectronics
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- * 3. Neither the name of STMicroelectronics nor the names of its contributors
- * may be used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- ******************************************************************************
- */
- /* Includes ------------------------------------------------------------------*/
- #include "main.h"
- #include "stm32f4xx_hal.h"
- #include "adc.h"
- #include "dac.h"
- #include "dma.h"
- #include "tim.h"
- #include "gpio.h"
- /* USER CODE BEGIN Includes */
- #include "math.h"
- #define GRID_FREQ (float)50 //grid frequency
- #define SINE_RES (float)500 //sine resolution (points)
- #define CNT_FREQ (float)80000000 // uC frequency
- #define TIM_PERIOD (float)((CNT_FREQ)/((SINE_RES)*(GRID_FREQ))) //ADC timer autoreload value
- #define PI (float)3.14149
- #define GRID_WN (float)2*PI*GRID_FREQ //grid frequency in radians
- #define CNT_PERIOD (float)1/CNT_FREQ
- #define Kp (float) 40.0
- #define KiTs (float) 0.8 //Ts=0,00008s0.4 albo 0.8
- #define Ts (float) 1/((SINE_RES)*(GRID_FREQ)) //dla 500probek- 40us, nowa wartosc z ADC
- //Do sprawdzenia Ts inny , w integratorze integIn ,KITs
- /* USER CODE END Includes */
- /* Private variables ---------------------------------------------------------*/
- /* USER CODE BEGIN PV */
- /* Private variables ---------------------------------------------------------*/
- ////--------------Notch filter-----------------
- const float b1=-2.0;
- const float b2=1.0006;
- const float a1=-1.9950;
- const float a2=0.9956;
- ////--------------------PLL variables-------------
- float Mycos=0.0;
- float Mysin=0.0;
- float theta=0.0;
- float Upd[3]={0.0,0.0,0.0};//phase detect
- float ynotch[3]={0.0,0.0,0.0};
- float ylf[2]={0.0,0.0};
- float wo=0.0;
- float sinusek=0.0;
- float error=0.0;
- float pi_integral=0.0;
- float integ_in=0.0;
- float integ_out=0.0;
- ////---------ADC tables--------------------------
- uint16_t odczyt500[500];
- uint16_t odczyt2[2];
- uint16_t adc_test;
- uint32_t set_dac;
- //---------External variables and counters----------
- volatile uint32_t iterator_htim10=0;
- volatile uint32_t iterator_main=0;
- volatile uint32_t iterator_time=0;
- extern volatile uint32_t iterator_tc;
- extern volatile uint8_t dma_flag;
- extern DMA_HandleTypeDef hdma_adc1;
- extern ADC_HandleTypeDef hadc1;
- /* USER CODE END PV */
- /* Private function prototypes -----------------------------------------------*/
- void SystemClock_Config(void);
- /* USER CODE BEGIN PFP */
- /* Private function prototypes -----------------------------------------------*/
- //void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
- // if(htim->Instance == TIM10){
- // iterator_htim10=iterator_tc;
- // // HAL_GPIO_TogglePin(LED3_GPIO_Port,LED3_Pin);
- // }
- //}
- /* USER CODE END PFP */
- /* USER CODE BEGIN 0 */
- /* USER CODE END 0 */
- int main(void)
- {
- /* USER CODE BEGIN 1 */
- /* USER CODE END 1 */
- /* MCU Configuration----------------------------------------------------------*/
- /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
- HAL_Init();
- /* USER CODE BEGIN Init */
- /* USER CODE END Init */
- /* Configure the system clock */
- SystemClock_Config();
- /* USER CODE BEGIN SysInit */
- /* USER CODE END SysInit */
- /* Initialize all configured peripherals */
- MX_GPIO_Init();
- MX_DMA_Init();
- MX_ADC1_Init();
- MX_DAC_Init();
- MX_TIM2_Init();
- MX_TIM4_Init();
- //MX_TIM10_Init();
- // MX_TIM3_Init();
- /* USER CODE BEGIN 2 */
- HAL_GPIO_WritePin(LED_GPIO_Port,LED_Pin,GPIO_PIN_SET);
- HAL_GPIO_WritePin(LED2_GPIO_Port,LED2_Pin,GPIO_PIN_RESET);
- HAL_GPIO_WritePin(LED3_GPIO_Port,LED3_Pin,GPIO_PIN_RESET);
- HAL_TIM_Base_Start(&htim2);//trigeruje adeczka
- //HAL_TIM_Base_Start(&htim4);//trigeruje daczka
- HAL_TIM_Base_Start(&htim3);//mierzy czas, taktowanie 100 000Hz, max 65ms
- // HAL_TIM_Base_Start_IT(&htim10);//pamietac zeby to wyłączyc
- // HAL_DAC_Start_DMA(&hdac,DAC_CHANNEL_1,(uint32_t*)tablicaDAC,1000,DAC_ALIGN_12B_R);
- HAL_DAC_Start(&hdac,DAC_CHANNEL_1);
- HAL_ADC_MspInit(&hadc1);
- HAL_ADC_Start_DMA(&hadc1,(uint32_t*)odczyt2,2);
- __HAL_DMA_ENABLE_IT(&hdma_adc1, DMA_IT_TC||DMA_IT_HT);
- HAL_DMA_IRQHandler(&hdma_adc1);
- HAL_ADC_Start(&hadc1);
- /* USER CODE END 2 */
- /* Infinite loop */
- /* USER CODE BEGIN WHILE */
- uint8_t start=0;
- //SPRAWDZIC INNE TS!!!!!!!!!!!!!
- while (1)
- {
- if(dma_flag==1){
- if(start==0){
- __HAL_TIM_SET_COUNTER(&htim3,0x0000);
- start+=1;
- }
- //iterator_main+=1;
- dma_flag=0;
- //-------------------------------------
- //----- INPUT SINUS--------------------
- float tempSin=0.0;
- tempSin=odczyt2[0]+odczyt2[1];
- tempSin=tempSin/2;//średnia z 2 odczytanych wartosci
- tempSin=tempSin/2048.0;
- tempSin=tempSin-1;
- sinusek=tempSin;//input sinus wave, range from -1 to 1
- //-------------------------------------
- //----- Phase detect--------------------
- Upd[0]=sinusek*Mycos;
- //-------------------------------------
- //----- NOTCH FILTER--------------------
- float tempNotch=0.0;
- tempNotch=Upd[0]+b1*Upd[1];
- tempNotch+=b2*Upd[2];
- tempNotch-=a1*ynotch[1];
- tempNotch-=a2*ynotch[2];
- Upd[2]=Upd[1];
- Upd[1]=Upd[0];
- ynotch[2]=ynotch[1];
- ynotch[1]=tempNotch;//sprawdzic to jeszcze
- ynotch[0]=tempNotch;
- //-------------------------------------
- //----- PI LOOP FILTER--------------------
- // ylf[0]=ylf[1]+PI0*ynotch[0]+PI1*ynotch[1];
- error=ynotch[0]-0.0; //jak cos -Fdb
- //if(abs(error)>0.9 && iterator_time>0){
- // __HAL_TIM_SET_COUNTER(&htim3,0x0000);
- // iterator_time=0;
- //}
- pi_integral=pi_integral+error*KiTs;
- if(pi_integral>200.0) {//maksymalny syg z czesci całkujacej
- pi_integral=200.0;
- }else if (pi_integral<-200.0){
- pi_integral=-200.0;
- }
- ylf[0]=Kp*error+pi_integral;
- if(ylf[0]>200.0) {//maksymalny syg sterujacy z regulatora
- ylf[0]=200.0;
- }else if (ylf[0]<-200.0){
- ylf[0]=-200.0;
- }
- //-------------------------------------
- //----- OMEGA0--------------------
- float omega_o=GRID_WN;
- omega_o=omega_o+ylf[0];
- //-------------------------------------
- //----- INTEGRATION--------------------
- float tempInt=0.0;
- float sample_time=Ts; //sprawdzic dla innej wartosci 40
- tempInt=sample_time*(omega_o+integ_in);//?????
- tempInt+=integ_out;
- if ( tempInt > 6.2831852) {
- tempInt = 0.0;
- }
- else if (tempInt < 0.0){
- tempInt = 0.0;
- }
- integ_out=tempInt;
- integ_in=omega_o;
- //-------------------------------------
- //----- PLL OUTPUT--------------------
- theta=integ_out;
- Mysin=sin(theta);
- Mycos=cos(theta);
- float tempSin2=0.0;
- tempSin2=Mysin+1;
- tempSin2=tempSin2*1890;//do3V
- set_dac=(uint32_t)tempSin2;
- if(abs(error)<0.5 && iterator_time==0){
- iterator_time=__HAL_TIM_GET_COUNTER(&htim3);
- }
- HAL_DAC_SetValue(&hdac,DAC_CHANNEL_1,DAC_ALIGN_12B_R,set_dac);
- //wyslac na daczku cos
- }
- /* USER CODE END WHILE */
- /* USER CODE BEGIN 3 */
- }
- /* USER CODE END 3 */
- }
- /** System Clock Configuration
- */
- void SystemClock_Config(void)
- {
- RCC_OscInitTypeDef RCC_OscInitStruct;
- RCC_ClkInitTypeDef RCC_ClkInitStruct;
- /**Configure the main internal regulator output voltage
- */
- __HAL_RCC_PWR_CLK_ENABLE();
- __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);
- /**Initializes the CPU, AHB and APB busses clocks
- */
- RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
- RCC_OscInitStruct.HSEState = RCC_HSE_ON;
- RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
- RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
- RCC_OscInitStruct.PLL.PLLM = 4;
- RCC_OscInitStruct.PLL.PLLN = 80;
- RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
- RCC_OscInitStruct.PLL.PLLQ = 2;
- RCC_OscInitStruct.PLL.PLLR = 2;
- if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
- {
- _Error_Handler(__FILE__, __LINE__);
- }
- /**Initializes the CPU, AHB and APB busses clocks
- */
- RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
- |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
- RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
- RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
- RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
- RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
- if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
- {
- _Error_Handler(__FILE__, __LINE__);
- }
- /**Configure the Systick interrupt time
- */
- HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
- /**Configure the Systick
- */
- HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
- /* SysTick_IRQn interrupt configuration */
- HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
- }
- /* USER CODE BEGIN 4 */
- /* USER CODE END 4 */
- /**
- * @brief This function is executed in case of error occurrence.
- * @param None
- * @retval None
- */
- void _Error_Handler(char * file, int line)
- {
- /* USER CODE BEGIN Error_Handler_Debug */
- /* User can add his own implementation to report the HAL error return state */
- while(1)
- {
- }
- /* USER CODE END Error_Handler_Debug */
- }
- #ifdef USE_FULL_ASSERT
- /**
- * @brief Reports the name of the source file and the source line number
- * where the assert_param error has occurred.
- * @param file: pointer to the source file name
- * @param line: assert_param error line source number
- * @retval None
- */
- void assert_failed(uint8_t* file, uint32_t line)
- {
- /* USER CODE BEGIN 6 */
- /* 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) */
- /* USER CODE END 6 */
- }
- #endif
- /**
- * @}
- */
- /**
- * @}
- */
- /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement