Готовая реализация

Robot_balancer/PID/Drivers/LSM6DS3.h

@@ -32,7 +32,7 @@ extern volatile int16_t gyro_x, gyro_y, gyro_z;
 // Коэффициенты фильтр
 #define GYRO_SCALE 65.5f     // Для +-500: 32768/500
 #define RAD_TO_DEG 57.29578f // Преобразование радиан в градусы
-#define DT 0.001f           // Период дискретизации (10 мс)
+#define DT 0.0001f           // Период дискретизации (100 мкс)
 
 // Настраиваемые коэффициенты фильтрации
 extern volatile float accel_filter_coeff; // Коэффициент ФНЧ для акселерометра (0.1-0.5)

Robot_balancer/PID/Drivers/StepperMotorDriver.c

@@ -0,0 +1,71 @@
+#include <StepperMotorDriver.h>
+
+int16_t speedStepper1 = 0;
+int16_t setSpeed1 = 200;
+int16_t speedStepper2 = 0;
+int16_t setSpeed2 = 200;
+
+#define minSpeed 10.0f
+#define maxSpeed 400.0f
+
+void SetStepper1RotateSpeed(int16_t* speedStepper1)
+{
+    if (abs(*speedStepper1) <= minSpeed | abs(*speedStepper1) >= maxSpeed)
+    {
+        TIM2->CCR1 = 0;
+        TIM2->EGR |= TIM_EGR_UG;
+        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 (abs(*speedStepper2) <= minSpeed & abs(*speedStepper2) >= maxSpeed)
+    {
+        TIM2->CCR1 = 0;
+        TIM2->EGR |= TIM_EGR_UG;
+        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;
+}

Robot_balancer/PID/Drivers/StepperMotorDriver.h

@@ -0,0 +1,16 @@
+#include "stm32g431xx.h"
+#include <math.h>
+
+#define N_FULL_STEP 200
+#define MICROSTEPPING 8
+#define F_CLK 170000000
+#define PWM_PSC 17
+#define Duty 0.05f
+
+extern int16_t speedStepper1;
+extern int16_t setSpeed1;
+extern int16_t speedStepper2;
+extern int16_t setSpeed2;
+
+void SetStepper1RotateSpeed(int16_t* speedStepper1);
+void SetStepper2RotateSpeed(int16_t* speedStepper2);

Robot_balancer/PID/Utils/PID.c

@@ -1,8 +1,8 @@
 #include "PID.h"
 
 float KP = 100;
-float KI = 1;
+float KI = 0;
-float KD = 1;
+float KD = 10;
 float integral = 0;
 float lastError = 0;
 float limit = 1000; //ограничение интегральной составляющей
@@ -40,4 +40,16 @@ float pid_update(float target, float current, float dt)
 //     }
 //     float output = pPart + iPart + dPart;
 //     return output; //Управляющее воздействие/крутящий момент
-// }
+// }
+
+#define Mc 0.21f //момент сцепления
+#define J 820.0f //инерция ротора
+#define dt 0.0001f
+
+float integral_perevod = 0;
+
+float perevod (float M){
+    integral_perevod += (M - Mc)*dt;
+    int omega = (int)(1.0f/J*integral_perevod);
+    return omega * 60.0f / 9.5493f;
+}

Robot_balancer/PID/Utils/PID.h

@@ -1,3 +1,5 @@
+#include <math.h>
+#include "StepperMotorDriver.h"
 float pid_update(float target, float current, float dt);
 float perevod(float pid_result);
 

Robot_balancer/PID/main.cpp

@@ -2,6 +2,7 @@
 #include <math.h>
 #include "LSM6DS3.h"
 #include "PID.h"
+#include <StepperMotorDriver.h>
 
 //---------------------------------------------------------------------------------------------------------------
 //
@@ -10,75 +11,9 @@
 //
 //---------------------------------------------------------------------------------------------------------------
 //---------------------------------------------------------------------------------------------------------------
-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;
+int res;
 float u;
 extern "C" void TIM4_IRQHandler(void)
 {
@@ -100,7 +35,7 @@ extern "C" void TIM4_IRQHandler(void)
         if (fabsf(u) < 0.001f){
             u = 0.001f; 
         }
-        res = (48/u);
+        res = perevod(u);
     }
 }
 //---------------------------------------------------------------------------------------------------------------
@@ -162,8 +97,8 @@ int main()
     GPIOA->AFR[0] |= (1 << GPIO_AFRL_AFSEL0_Pos);// установка бита режима альтернативной функции 
     GPIOB->AFR[0] |= (2 << GPIO_AFRL_AFSEL0_Pos);// установка бита режима альтернативной функции 
     //-----------------------------------------------------------------------------------------------------------
-    TIM4->PSC = 170 - 1;                   // Предделитель 170 МГц / 170 = 1000 кГц
+    TIM4->PSC = 17 - 1;                   // Предделитель 170 МГц / 17 = 10000 кГц
-    TIM4->ARR = 1000 - 1;                 // Автоматическая перезагрузка (0.001 секунда)
+    TIM4->ARR = 1000 - 1;                 // Автоматическая перезагрузка (0.0001 секунда)
     TIM4->DIER |= TIM_DIER_UIE;           // Разрешить прерывание по обновлению
     TIM4->CR1 |= TIM_CR1_CEN;             // Включить таймер
     NVIC_EnableIRQ(TIM4_IRQn);          // Включение прерывания