likhenko/Robot_balancer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

PID

Browse Source
This commit is contained in:
Vlad
2025-07-28 11:13:06 +03:00
parent abbd7981a9
commit aa36b579a4
3 changed files with 276 additions and 217 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
.vscode/iar-vsc.json vendored Normal file
View File

@ -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"
}
}

444
Robot_balancer/PID/Core/system_stm32g4xx.c
View File

@ -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;
}
/**
* @}
*/
/**
* @}
*/
* @}
*/
/**
* @}
*/
/**
* @}
*/
* @}
*/

30
Robot_balancer/PID/main.cpp
View File

@ -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)