From aa36b579a453d84dd83796f37f51ff0dbd646aa0 Mon Sep 17 00:00:00 2001 From: Vlad Date: Mon, 28 Jul 2025 11:13:06 +0300 Subject: [PATCH] PID --- .vscode/iar-vsc.json | 19 + Robot_balancer/PID/Core/system_stm32g4xx.c | 444 +++++++++++---------- Robot_balancer/PID/main.cpp | 30 +- 3 files changed, 276 insertions(+), 217 deletions(-) create mode 100644 .vscode/iar-vsc.json diff --git a/.vscode/iar-vsc.json b/.vscode/iar-vsc.json new file mode 100644 index 0000000..5ba39a1 --- /dev/null +++ b/.vscode/iar-vsc.json @@ -0,0 +1,19 @@ +{ + "workspace": { + "path": "${workspaceFolder}\\Robot_balancer\\Robot_balancer.eww" + }, + "workspaces": { + "${workspaceFolder}\\Robot_balancer\\Robot_balancer.eww": { + "configs": { + "${workspaceFolder}\\Robot_balancer\\ACAR\\ACAR.ewp": "Debug", + "${workspaceFolder}\\Robot_balancer\\PID\\PID.ewp": "Debug" + }, + "selected": { + "path": "${workspaceFolder}\\Robot_balancer\\PID\\PID.ewp" + } + } + }, + "workbench": { + "path": "C:\\iar\\ewarm-9.60.3" + } +} \ No newline at end of file diff --git a/Robot_balancer/PID/Core/system_stm32g4xx.c b/Robot_balancer/PID/Core/system_stm32g4xx.c index 922e57a..87e4b6a 100644 --- a/Robot_balancer/PID/Core/system_stm32g4xx.c +++ b/Robot_balancer/PID/Core/system_stm32g4xx.c @@ -1,105 +1,105 @@ /** - ****************************************************************************** - * @file system_stm32g4xx.c - * @author MCD Application Team - * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File - * - * This file provides two functions and one global variable to be called from - * user application: - * - SystemInit(): This function is called at startup just after reset and - * before branch to main program. This call is made inside - * the "startup_stm32g4xx.s" file. - * - * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used - * by the user application to setup the SysTick - * timer or configure other parameters. - * - * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must - * be called whenever the core clock is changed - * during program execution. - * - * After each device reset the HSI (16 MHz) is used as system clock source. - * Then SystemInit() function is called, in "startup_stm32g4xx.s" file, to - * configure the system clock before to branch to main program. - * - * This file configures the system clock as follows: - *============================================================================= - *----------------------------------------------------------------------------- - * System Clock source | HSI - *----------------------------------------------------------------------------- - * SYSCLK(Hz) | 16000000 - *----------------------------------------------------------------------------- - * HCLK(Hz) | 16000000 - *----------------------------------------------------------------------------- - * AHB Prescaler | 1 - *----------------------------------------------------------------------------- - * APB1 Prescaler | 1 - *----------------------------------------------------------------------------- - * APB2 Prescaler | 1 - *----------------------------------------------------------------------------- - * PLL_M | 1 - *----------------------------------------------------------------------------- - * PLL_N | 16 - *----------------------------------------------------------------------------- - * PLL_P | 7 - *----------------------------------------------------------------------------- - * PLL_Q | 2 - *----------------------------------------------------------------------------- - * PLL_R | 2 - *----------------------------------------------------------------------------- - * Require 48MHz for RNG | Disabled - *----------------------------------------------------------------------------- - *============================================================================= - ****************************************************************************** - * @attention - * - * Copyright (c) 2019 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ + ****************************************************************************** + * @file system_stm32g4xx.c + * @author MCD Application Team + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File + * + * This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32g4xx.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * After each device reset the HSI (16 MHz) is used as system clock source. + * Then SystemInit() function is called, in "startup_stm32g4xx.s" file, to + * configure the system clock before to branch to main program. + * + * This file configures the system clock as follows: + *============================================================================= + *----------------------------------------------------------------------------- + * System Clock source | HSI + *----------------------------------------------------------------------------- + * SYSCLK(Hz) | 16000000 + *----------------------------------------------------------------------------- + * HCLK(Hz) | 16000000 + *----------------------------------------------------------------------------- + * AHB Prescaler | 1 + *----------------------------------------------------------------------------- + * APB1 Prescaler | 1 + *----------------------------------------------------------------------------- + * APB2 Prescaler | 1 + *----------------------------------------------------------------------------- + * PLL_M | 1 + *----------------------------------------------------------------------------- + * PLL_N | 16 + *----------------------------------------------------------------------------- + * PLL_P | 7 + *----------------------------------------------------------------------------- + * PLL_Q | 2 + *----------------------------------------------------------------------------- + * PLL_R | 2 + *----------------------------------------------------------------------------- + * Require 48MHz for RNG | Disabled + *----------------------------------------------------------------------------- + *============================================================================= + ****************************************************************************** + * @attention + * + * Copyright (c) 2019 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ /** @addtogroup CMSIS - * @{ - */ + * @{ + */ /** @addtogroup stm32g4xx_system - * @{ - */ + * @{ + */ /** @addtogroup STM32G4xx_System_Private_Includes - * @{ - */ + * @{ + */ #include "stm32g4xx.h" -#if !defined (HSE_VALUE) - #define HSE_VALUE 24000000U /*!< Value of the External oscillator in Hz */ -#endif /* HSE_VALUE */ +#if !defined(HSE_VALUE) +#define HSE_VALUE 8000000U /*!< Value of the External oscillator in Hz */ +#endif /* HSE_VALUE */ -#if !defined (HSI_VALUE) - #define HSI_VALUE 16000000U /*!< Value of the Internal oscillator in Hz*/ -#endif /* HSI_VALUE */ +#if !defined(HSI_VALUE) +#define HSI_VALUE 16000000U /*!< Value of the Internal oscillator in Hz*/ +#endif /* HSI_VALUE */ /** - * @} - */ + * @} + */ /** @addtogroup STM32G4xx_System_Private_TypesDefinitions - * @{ - */ + * @{ + */ /** - * @} - */ + * @} + */ /** @addtogroup STM32G4xx_System_Private_Defines - * @{ - */ + * @{ + */ /************************* Miscellaneous Configuration ************************/ /* Note: Following vector table addresses must be defined in line with linker @@ -114,117 +114,160 @@ in Sram else user remap will be done in Flash. */ /* #define VECT_TAB_SRAM */ #if defined(VECT_TAB_SRAM) -#define VECT_TAB_BASE_ADDRESS SRAM_BASE /*!< Vector Table base address field. - This value must be a multiple of 0x200. */ -#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field. - This value must be a multiple of 0x200. */ +#define VECT_TAB_BASE_ADDRESS SRAM_BASE /*!< Vector Table base address field. \ + This value must be a multiple of 0x200. */ +#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field. \ + This value must be a multiple of 0x200. */ #else -#define VECT_TAB_BASE_ADDRESS FLASH_BASE /*!< Vector Table base address field. - This value must be a multiple of 0x200. */ -#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field. - This value must be a multiple of 0x200. */ -#endif /* VECT_TAB_SRAM */ -#endif /* USER_VECT_TAB_ADDRESS */ -/******************************************************************************/ -/** - * @} - */ +#define VECT_TAB_BASE_ADDRESS FLASH_BASE /*!< Vector Table base address field. \ + This value must be a multiple of 0x200. */ +#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field. \ + This value must be a multiple of 0x200. */ +#endif /* VECT_TAB_SRAM */ +#endif /* USER_VECT_TAB_ADDRESS */ + /******************************************************************************/ + /** + * @} + */ /** @addtogroup STM32G4xx_System_Private_Macros - * @{ - */ + * @{ + */ /** - * @} - */ + * @} + */ /** @addtogroup STM32G4xx_System_Private_Variables - * @{ - */ - /* The SystemCoreClock variable is updated in three ways: - 1) by calling CMSIS function SystemCoreClockUpdate() - 2) by calling HAL API function HAL_RCC_GetHCLKFreq() - 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency - Note: If you use this function to configure the system clock; then there - is no need to call the 2 first functions listed above, since SystemCoreClock - variable is updated automatically. - */ - uint32_t SystemCoreClock = HSI_VALUE; + * @{ + */ +/* The SystemCoreClock variable is updated in three ways: + 1) by calling CMSIS function SystemCoreClockUpdate() + 2) by calling HAL API function HAL_RCC_GetHCLKFreq() + 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency + Note: If you use this function to configure the system clock; then there + is no need to call the 2 first functions listed above, since SystemCoreClock + variable is updated automatically. +*/ +uint32_t SystemCoreClock = HSI_VALUE; - const uint8_t AHBPrescTable[16] = {0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U, 6U, 7U, 8U, 9U}; - const uint8_t APBPrescTable[8] = {0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U}; +const uint8_t AHBPrescTable[16] = {0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U, 6U, 7U, 8U, 9U}; +const uint8_t APBPrescTable[8] = {0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U}; /** - * @} - */ + * @} + */ /** @addtogroup STM32G4xx_System_Private_FunctionPrototypes - * @{ - */ + * @{ + */ /** - * @} - */ + * @} + */ /** @addtogroup STM32G4xx_System_Private_Functions - * @{ - */ + * @{ + */ /** - * @brief Setup the microcontroller system. - * @param None - * @retval None - */ + * @brief Setup the microcontroller system. + * @param None + * @retval None + */ + +void SystemClockInit() +{ + // 1. Включаем HSE + RCC->CR |= RCC_CR_HSEON; + while (!(RCC->CR & RCC_CR_HSERDY)) + ; + + // 2. Отключаем PLL + RCC->CR &= ~RCC_CR_PLLON; + while (RCC->CR & RCC_CR_PLLRDY) + ; + + // 3. Устанавливаем источник PLL — HSE + RCC->PLLCFGR &= ~RCC_PLLCFGR_PLLSRC; + RCC->PLLCFGR |= RCC_PLLCFGR_PLLSRC_HSE; + + // 4. Настраиваем PLLM, PLLN, PLLR + RCC->PLLCFGR = + (1U << RCC_PLLCFGR_PLLM_Pos) | // PLLM = 2 (код 1) + (85U << RCC_PLLCFGR_PLLN_Pos) | // PLLN = 85 + (0U << RCC_PLLCFGR_PLLR_Pos) | // PLLR = 2 (код 0) + RCC_PLLCFGR_PLLREN | // Включаем PLLR + RCC_PLLCFGR_PLLSRC_HSE; // Источник HSE + + // 5. Устанавливаем FLASH задержку (4 такта для 170 МГц) + FLASH->ACR |= FLASH_ACR_LATENCY_4WS; + + // 6. Включаем PLL + RCC->CR |= RCC_CR_PLLON; + while (!(RCC->CR & RCC_CR_PLLRDY)) + ; + + // 7. Переключаем системную частоту на PLL + RCC->CFGR &= ~RCC_CFGR_SW; + RCC->CFGR |= RCC_CFGR_SW_PLL; + while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL) + ; + + // 8. Обновляем переменную SystemCoreClock + SystemCoreClockUpdate(); +} void SystemInit(void) { - /* FPU settings ------------------------------------------------------------*/ - #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) - SCB->CPACR |= ((3UL << (10*2))|(3UL << (11*2))); /* set CP10 and CP11 Full Access */ - #endif + SystemClockInit(); +/* FPU settings ------------------------------------------------------------*/ +#if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << (10 * 2)) | (3UL << (11 * 2))); /* set CP10 and CP11 Full Access */ +#endif /* Configure the Vector Table location add offset address ------------------*/ #if defined(USER_VECT_TAB_ADDRESS) SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ -#endif /* USER_VECT_TAB_ADDRESS */ +#endif /* USER_VECT_TAB_ADDRESS */ } /** - * @brief Update SystemCoreClock variable according to Clock Register Values. - * The SystemCoreClock variable contains the core clock (HCLK), it can - * be used by the user application to setup the SysTick timer or configure - * other parameters. - * - * @note Each time the core clock (HCLK) changes, this function must be called - * to update SystemCoreClock variable value. Otherwise, any configuration - * based on this variable will be incorrect. - * - * @note - The system frequency computed by this function is not the real - * frequency in the chip. It is calculated based on the predefined - * constant and the selected clock source: - * - * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(**) - * - * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(***) - * - * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(***) - * or HSI_VALUE(*) multiplied/divided by the PLL factors. - * - * (**) HSI_VALUE is a constant defined in stm32g4xx_hal.h file (default value - * 16 MHz) but the real value may vary depending on the variations - * in voltage and temperature. - * - * (***) HSE_VALUE is a constant defined in stm32g4xx_hal.h file (default value - * 24 MHz), user has to ensure that HSE_VALUE is same as the real - * frequency of the crystal used. Otherwise, this function may - * have wrong result. - * - * - The result of this function could be not correct when using fractional - * value for HSE crystal. - * - * @param None - * @retval None - */ + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(**) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(***) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(***) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (**) HSI_VALUE is a constant defined in stm32g4xx_hal.h file (default value + * 16 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (***) HSE_VALUE is a constant defined in stm32g4xx_hal.h file (default value + * 24 MHz), user has to ensure that HSE_VALUE is same as the real + * frequency of the crystal used. Otherwise, this function may + * have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ void SystemCoreClockUpdate(void) { uint32_t tmp, pllvco, pllr, pllsource, pllm; @@ -232,35 +275,35 @@ void SystemCoreClockUpdate(void) /* Get SYSCLK source -------------------------------------------------------*/ switch (RCC->CFGR & RCC_CFGR_SWS) { - case 0x04: /* HSI used as system clock source */ - SystemCoreClock = HSI_VALUE; - break; + case 0x04: /* HSI used as system clock source */ + SystemCoreClock = HSI_VALUE; + break; - case 0x08: /* HSE used as system clock source */ - SystemCoreClock = HSE_VALUE; - break; + case 0x08: /* HSE used as system clock source */ + SystemCoreClock = HSE_VALUE; + break; - case 0x0C: /* PLL used as system clock source */ - /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN - SYSCLK = PLL_VCO / PLLR - */ - pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC); - pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U ; - if (pllsource == 0x02UL) /* HSI used as PLL clock source */ - { - pllvco = (HSI_VALUE / pllm); - } - else /* HSE used as PLL clock source */ - { - pllvco = (HSE_VALUE / pllm); - } - pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 8); - pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> 25) + 1U) * 2U; - SystemCoreClock = pllvco/pllr; - break; + case 0x0C: /* PLL used as system clock source */ + /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN + SYSCLK = PLL_VCO / PLLR + */ + pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC); + pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U; + if (pllsource == 0x02UL) /* HSI used as PLL clock source */ + { + pllvco = (HSI_VALUE / pllm); + } + else /* HSE used as PLL clock source */ + { + pllvco = (HSE_VALUE / pllm); + } + pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 8); + pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> 25) + 1U) * 2U; + SystemCoreClock = pllvco / pllr; + break; - default: - break; + default: + break; } /* Compute HCLK clock frequency --------------------------------------------*/ /* Get HCLK prescaler */ @@ -269,17 +312,14 @@ void SystemCoreClockUpdate(void) SystemCoreClock >>= tmp; } +/** + * @} + */ /** - * @} - */ + * @} + */ /** - * @} - */ - -/** - * @} - */ - - + * @} + */ diff --git a/Robot_balancer/PID/main.cpp b/Robot_balancer/PID/main.cpp index 4616e32..033973b 100644 --- a/Robot_balancer/PID/main.cpp +++ b/Robot_balancer/PID/main.cpp @@ -16,11 +16,11 @@ volatile uint32_t counter = 0; float res; float u; -extern "C" void TIM2_IRQHandler(void) +extern "C" void TIM4_IRQHandler(void) { - if (TIM2->SR & TIM_SR_UIF) // Проверить флаг обновления + if (TIM4->SR & TIM_SR_UIF) // Проверить флаг обновления { - TIM2->SR &= ~TIM_SR_UIF; // Сбросить флаг + TIM4->SR &= ~TIM_SR_UIF; // Сбросить флаг counter++; @@ -59,13 +59,13 @@ int main() //----------------------------------------------------------------------------------------------------------- GPIOC->BSRR = GPIO_BSRR_BS6; // установка в высокий C6 //------------------------------------------------------------------------- ---------------------------------- - RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN; // Включить тактирование TIM2 - TIM2->PSC = 16 - 1; // Предделитель 16 МГц / 16 = 1000 кГц - TIM2->ARR = 1000 - 1; // Автоматическая перезагрузка (0.001 секунда) - TIM2->DIER |= TIM_DIER_UIE; // Разрешить прерывание по обновлению - TIM2->CR1 |= TIM_CR1_CEN; // Включить таймер - NVIC_EnableIRQ(TIM2_IRQn); - NVIC_SetPriority(TIM2_IRQn, 15); // Уровень приоритета + RCC->APB1ENR1 |= RCC_APB1ENR1_TIM4EN; // Включить тактирование TIM2 + TIM4->PSC = 170 - 1; // Предделитель 170 МГц / 170 = 1000 кГц + TIM4->ARR = 1000 - 1; // Автоматическая перезагрузка (0.001 секунда) + TIM4->DIER |= TIM_DIER_UIE; // Разрешить прерывание по обновлению + TIM4->CR1 |= TIM_CR1_CEN; // Включить таймер + NVIC_EnableIRQ(TIM4_IRQn); + NVIC_SetPriority(TIM4_IRQn, 15); // Уровень приоритета //----------------------------------------------------------------------------------------------------------- GPIOA->OTYPER &= ~GPIO_OTYPER_OT8_Msk; // Сброс режима GPIOC->OTYPER &= ~GPIO_OTYPER_OT11_Msk; // Сброс режима @@ -91,11 +91,11 @@ int main() I2C3->CR1 &= ~I2C_CR1_PE; // Отключение I2C3 //----------------------------------------------------------------------------------------------------------- // 400 кГц - I2C3->TIMINGR = (0 << I2C_TIMINGR_PRESC_Pos) | // PRESC=0 (делитель 1) -> I2CCLK=16 МГц - (3 << I2C_TIMINGR_SCLDEL_Pos) | // SCLDEL=3 -> t_HD;STA=0.25 мкс - (1 << I2C_TIMINGR_SDADEL_Pos) | // SDADEL=1 -> t_HD;DAT=0.0625 мкс - (15 << I2C_TIMINGR_SCLH_Pos) | // SCLH=15 -> t_HIGH=1 мкс - (15 << I2C_TIMINGR_SCLL_Pos); // SCLL=15 -> t_LOW=1 мкс + I2C3->TIMINGR = (7 << I2C_TIMINGR_PRESC_Pos) | // PRESC=7 -> I2CCLK=21.25 МГц + (8 << I2C_TIMINGR_SCLDEL_Pos) | // SCLDEL=8 -> t_HD;STA=0.38 мкс + (4 << I2C_TIMINGR_SDADEL_Pos) | // SDADEL=4 -> t_HD;DAT=0.019 мкс + (16 << I2C_TIMINGR_SCLH_Pos) | // SCLH=16 -> t_HIGH=0.75 мкс + (32 << I2C_TIMINGR_SCLL_Pos); // SCLL=32 -> t_LOW=1.51 мкс //----------------------------------------------------------------------------------------------------------- I2C3->CR1 |= I2C_CR1_PE; // Включение I2C3 for (volatile int i = 0; i < 100000; ++i)