Полное управление шаговыми моторами

.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"
+  }
+}

Robot_balancer/PID/Core/system_stm32g4xx.c

@@ -77,12 +77,12 @@
 
 #include "stm32g4xx.h"
 
-#if !defined  (HSE_VALUE)
+#if !defined(HSE_VALUE)
-  #define HSE_VALUE     24000000U /*!< Value of the External oscillator in Hz */
+#define HSE_VALUE 8000000U /*!< Value of the External oscillator in Hz */
 #endif                     /* HSE_VALUE */
 
-#if !defined  (HSI_VALUE)
+#if !defined(HSI_VALUE)
-  #define HSI_VALUE    16000000U /*!< Value of the Internal oscillator in Hz*/
+#define HSI_VALUE 16000000U /*!< Value of the Internal oscillator in Hz*/
 #endif                      /* HSI_VALUE */
 
 /**
@@ -114,19 +114,19 @@
      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.
+#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.
+#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.
+#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.
+#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 */
-/******************************************************************************/
+                                         /******************************************************************************/
-/**
+                                         /**
                                           * @}
                                           */
 
@@ -141,18 +141,18 @@
 /** @addtogroup STM32G4xx_System_Private_Variables
  * @{
  */
-  /* The SystemCoreClock variable is updated in three ways:
+/* 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;
+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 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 APBPrescTable[8] = {0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U};
 
 /**
  * @}
@@ -176,12 +176,55 @@
  * @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 ------------------------------------------------------------*/
+  SystemClockInit();
-  #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+/* FPU settings ------------------------------------------------------------*/
-    SCB->CPACR |= ((3UL << (10*2))|(3UL << (11*2)));  /* set CP10 and CP11 Full Access */
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
-  #endif
+  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)
@@ -245,7 +288,7 @@ void SystemCoreClockUpdate(void)
        SYSCLK = PLL_VCO / PLLR
        */
     pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC);
-      pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U ;
+    pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U;
     if (pllsource == 0x02UL) /* HSI used as PLL clock source */
     {
       pllvco = (HSI_VALUE / pllm);
@@ -256,7 +299,7 @@ void SystemCoreClockUpdate(void)
     }
     pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 8);
     pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> 25) + 1U) * 2U;
-      SystemCoreClock = pllvco/pllr;
+    SystemCoreClock = pllvco / pllr;
     break;
 
   default:
@@ -269,7 +312,6 @@ void SystemCoreClockUpdate(void)
   SystemCoreClock >>= tmp;
 }
 
-
 /**
  * @}
  */
@@ -281,5 +323,3 @@ void SystemCoreClockUpdate(void)
 /**
  * @}
  */
-
-

Robot_balancer/PID/PID.ewp

@@ -2233,6 +2233,12 @@
         <file>
             <name>$PROJ_DIR$\Drivers\LSM6DS3.h</name>
         </file>
+        <file>
+            <name>$PROJ_DIR$\Drivers\StepperMotorDriver.c</name>
+        </file>
+        <file>
+            <name>$PROJ_DIR$\Drivers\StepperMotorDriver.h</name>
+        </file>
     </group>
     <group>
         <name>Utils</name>

Robot_balancer/PID/PID.ewt

@@ -2959,6 +2959,12 @@
         <file>
             <name>$PROJ_DIR$\Drivers\LSM6DS3.h</name>
         </file>
+        <file>
+            <name>$PROJ_DIR$\Drivers\StepperMotorDriver.c</name>
+        </file>
+        <file>
+            <name>$PROJ_DIR$\Drivers\StepperMotorDriver.h</name>
+        </file>
     </group>
     <group>
         <name>Utils</name>

Robot_balancer/PID/main.cpp

@@ -10,17 +10,81 @@
 //
 //---------------------------------------------------------------------------------------------------------------
 //---------------------------------------------------------------------------------------------------------------
+int16_t speedStepper1 = 0;
+int16_t setSpeed1 = 200;
+int16_t speedStepper2 = 0;
+int16_t setSpeed2 = 200;
 
+#define N_FULL_STEP 200
+#define MICROSTEPPING 8
+#define F_CLK 170000000
+#define PWM_PSC 17
+#define Duty 0.05f
 //---------------------------------------------------------------------------------------------------------------
+void SetStepper1RotateSpeed(int16_t* speedStepper1)
+{
+    if (*speedStepper1 == 0)
+        return;
+
+    // Управление направлением
+    if (*speedStepper1 > 0)
+        GPIOA->BSRR = GPIO_BSRR_BR1;  // DIR = 0 (LOW)
+    else
+        GPIOA->BSRR = GPIO_BSRR_BS1;  // DIR = 1 (HIGH)
+
+    uint32_t absSpeed = (*speedStepper1 > 0) ? *speedStepper1 : -(*speedStepper1);
+
+    uint32_t F_set = (N_FULL_STEP * MICROSTEPPING * absSpeed) / 60;
+    if (F_set == 0) return;
+
+    uint32_t arr_set = F_CLK / (PWM_PSC * F_set);
+    uint32_t ccr_set = (uint32_t)(arr_set * Duty);
+
+    if (arr_set < 10) arr_set = 10;
+    if (ccr_set < 1) ccr_set = 1;
+
+    TIM2->ARR = arr_set - 1;
+    TIM2->CCR1 = ccr_set;
+
+    TIM2->EGR |= TIM_EGR_UG;
+}
+//---------------------------------------------------------------------------------------------------------------
+void SetStepper2RotateSpeed(int16_t* speedStepper2)
+{
+    if (*speedStepper2 == 0)
+        return;
+
+    // Управление направлением
+    if (*speedStepper2 > 0)
+        GPIOB->BSRR = GPIO_BSRR_BR1;  // DIR = 0 (LOW)
+    else
+        GPIOB->BSRR = GPIO_BSRR_BS1;  // DIR = 1 (HIGH)
+
+    uint32_t absSpeed = (*speedStepper2 > 0) ? *speedStepper2 : -(*speedStepper2);
+
+    uint32_t F_set = (N_FULL_STEP * MICROSTEPPING * absSpeed) / 60;
+    if (F_set == 0) return;
+
+    uint32_t arr_set = F_CLK / (PWM_PSC * F_set);
+    uint32_t ccr_set = (uint32_t)(arr_set * Duty);
+
+    if (arr_set < 10) arr_set = 10;
+    if (ccr_set < 1) ccr_set = 1;
+
+    TIM3->ARR = arr_set - 1;
+    TIM3->CCR3 = ccr_set;
+
+    TIM3->EGR |= TIM_EGR_UG;
+}
 //---------------------------------------------------------------------------------------------------------------
 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++;
 
@@ -46,26 +110,32 @@ int main()
 {
     //-----------------------------------------------------------------------------------------------------------
     RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;  // тактирование порт A
+    RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;  // тактирование порт В
     RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;  // тактирование порт C
     RCC->APB1ENR1 |= RCC_APB1ENR1_I2C3EN; // тактирование I2C3
+    RCC->APB1ENR1 |= RCC_APB1ENR1_TIM4EN; // Включить тактирование TIM4 
+    RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN; // Включить тактирование TIM2
+    RCC->APB1ENR1 |= RCC_APB1ENR1_TIM3EN; // Включить тактирование TIM3
     //-----------------------------------------------------------------------------------------------------------
     GPIOC->MODER &= ~GPIO_MODER_MODE6_Msk;  // очистка пина C6 - использую под LED
     GPIOC->MODER &= ~GPIO_MODER_MODE11_Msk; // очистка пина C11
     GPIOA->MODER &= ~GPIO_MODER_MODE8_Msk;  // очистка пина A8
+    GPIOA->MODER &= ~(GPIO_MODER_MODE0_Msk); // очистка пина PA0
+    GPIOB->MODER &= ~(GPIO_MODER_MODE0_Msk); // очистка пина PB0
+    GPIOA->MODER &= ~(GPIO_MODER_MODE1_Msk); // очистка пина PA1
+    GPIOB->MODER &= ~(GPIO_MODER_MODE1_Msk); // очистка пина PB1
     //-----------------------------------------------------------------------------------------------------------
     GPIOC->MODER |= GPIO_MODER_MODE6_0;  // установка бита выхода
     GPIOA->MODER |= GPIO_MODER_MODE8_1;  // установка бита режима альтернативной функции
     GPIOC->MODER |= GPIO_MODER_MODE11_1; // установка бита режима альтернативной функции
+    GPIOA->MODER |= (GPIO_MODER_MODE0_1);// установка бита режима альтернативной функции 
+    GPIOB->MODER |= (GPIO_MODER_MODE0_1);// установка бита режима альтернативной функции 
+    GPIOA->MODER |= (GPIO_MODER_MODE1_0);// установка бита выхода
+    GPIOB->MODER |= (GPIO_MODER_MODE1_0);// установка бита выхода
     //-----------------------------------------------------------------------------------------------------------
+    GPIOA->BSRR = GPIO_BSRR_BR1; // установка в низкий A1
+    GPIOB->BSRR = GPIO_BSRR_BR1; // установка в низкий B1
     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); // Уровень приоритета
     //-----------------------------------------------------------------------------------------------------------
     GPIOA->OTYPER &= ~GPIO_OTYPER_OT8_Msk;  // Сброс режима
     GPIOC->OTYPER &= ~GPIO_OTYPER_OT11_Msk; // Сброс режима
@@ -84,18 +154,52 @@ int main()
     //-----------------------------------------------------------------------------------------------------------
     GPIOA->AFR[1] &= ~GPIO_AFRH_AFSEL8_Msk;  // сброс альтернативной функции
     GPIOC->AFR[1] &= ~GPIO_AFRH_AFSEL11_Msk; // сброс альтернативной функции
+    GPIOA->AFR[0] &= ~(GPIO_AFRL_AFRL0); // сброс альтернативной функции 
+    GPIOB->AFR[0] &= ~(GPIO_AFRL_AFRL0); // сброс альтернативной функции
     //-----------------------------------------------------------------------------------------------------------
     GPIOA->AFR[1] |= (2U << GPIO_AFRH_AFSEL8_Pos);  // установка AF2
     GPIOC->AFR[1] |= (8U << GPIO_AFRH_AFSEL11_Pos); // Установка AF8
+    GPIOA->AFR[0] |= (1 << GPIO_AFRL_AFSEL0_Pos);// установка бита режима альтернативной функции 
+    GPIOB->AFR[0] |= (2 << GPIO_AFRL_AFSEL0_Pos);// установка бита режима альтернативной функции 
     //-----------------------------------------------------------------------------------------------------------
+    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); // Уровень приоритета
+    //-----------------------------------------------------------------------------------------------------------
+    TIM2->PSC = 17 - 1; // Предделитель 170 МГц / 170 = 1000 кГц
+    TIM2->ARR = 1875 - 1; // Автоматическая перезагрузка 
+    TIM2->CCR1 = 900; // Заполненность ШИМ
+    TIM2->CCMR1 &= ~TIM_CCMR1_OC1M_Msk; // Очистка режима работы канала 1
+    TIM2->CCMR1 |= (6 << TIM_CCMR1_OC1M_Pos); //Установка режима канала 1 в режим PWM Mode 1
+    TIM2->CCMR1 |= TIM_CCMR1_OC1PE; // preload для канала 1
+    TIM2->CCER |= TIM_CCER_CC1E; // Выход для канала 1
+    TIM2->CR1 |= TIM_CR1_ARPE; // Автоперезагрузка и счет
+    TIM2->EGR |= TIM_EGR_UG; // Обновления регистров
+    TIM2->CR1 |= TIM_CR1_CEN; // Запуск таймера
+    //-----------------------------------------------------------------------------------------------------------
+    TIM3->PSC = 17 - 1; // Предделитель 170 МГц / 170 = 1000 кГц
+    TIM3->ARR = 1875 - 1; // Автоматическая перезагрузка 
+    TIM3->CCR3 = 900; // Заполненность ШИМ
+    TIM3->CCMR2 &= ~TIM_CCMR2_OC3M_Msk; // Очистка режима работы канала 2
+    TIM3->CCMR2 |= (6 << TIM_CCMR2_OC3M_Pos); //Установка режима канала 2 в режим PWM Mode 1
+    TIM3->CCMR2 |= TIM_CCMR2_OC3PE; // preload для канала 2
+    TIM3->CCER |= TIM_CCER_CC3E; // Выход для канала 2
+    TIM3->CR1 |= TIM_CR1_ARPE; // Автоперезагрузка и счет
+    TIM3->EGR |= TIM_EGR_UG; // Обновления регистров
+    TIM3->CR1 |= TIM_CR1_CEN; // Запуск таймера
+    //-----------------------------------------------------------------------------------------------------------
+
     I2C3->CR1 &= ~I2C_CR1_PE; // Отключение I2C3
     //-----------------------------------------------------------------------------------------------------------
     // 400 кГц
-    I2C3->TIMINGR = (0 << I2C_TIMINGR_PRESC_Pos) |  // PRESC=0 (делитель 1) -> I2CCLK=16 МГц
+    I2C3->TIMINGR = (7 << I2C_TIMINGR_PRESC_Pos) |  // PRESC=7 -> I2CCLK=21.25 МГц
-                    (3 << I2C_TIMINGR_SCLDEL_Pos) | // SCLDEL=3 -> t_HD;STA=0.25 мкс
+                    (4 << I2C_TIMINGR_SCLDEL_Pos) | // SCLDEL=8 -> t_HD;STA=0.38 мкс
-                    (1 << I2C_TIMINGR_SDADEL_Pos) | // SDADEL=1 -> t_HD;DAT=0.0625 мкс
+                    (0 << I2C_TIMINGR_SDADEL_Pos) | // SDADEL=4 -> t_HD;DAT=0.019 мкс
-                    (15 << I2C_TIMINGR_SCLH_Pos) |  // SCLH=15 -> t_HIGH=1 мкс
+                    (25 << I2C_TIMINGR_SCLH_Pos) |  // SCLH=16 -> t_HIGH=0.75 мкс
-                    (15 << I2C_TIMINGR_SCLL_Pos);   // SCLL=15 -> t_LOW=1 мкс
+                    (25 << I2C_TIMINGR_SCLL_Pos);   // SCLL=32 -> t_LOW=1.51 мкс
     //-----------------------------------------------------------------------------------------------------------
     I2C3->CR1 |= I2C_CR1_PE; // Включение I2C3
     for (volatile int i = 0; i < 100000; ++i)
@@ -104,8 +208,22 @@ int main()
     while (I2C3->ISR & I2C_ISR_BUSY)
         __NOP(); // Ждём освобождения линии I2C3
     //-----------------------------------------------------------------------------------------------------------
+    //-----------------------------------------------------------------------------------------------------------
+    
     LSM6DS3_Init();
     while (1)
     {
+        if(setSpeed1 != speedStepper1)
+        {
+            speedStepper1 = setSpeed1;
+            SetStepper1RotateSpeed(&speedStepper1);
+        }
+
+        if(setSpeed2 != speedStepper2)
+        {
+            speedStepper2 = setSpeed2;
+            SetStepper2RotateSpeed(&speedStepper2);
+        }
+
     }
 }