From bbf07cc2337613e0bcefb6425ccba9fef28f7065 Mon Sep 17 00:00:00 2001 From: Vlad Date: Tue, 29 Jul 2025 14:19:18 +0300 Subject: [PATCH] =?UTF-8?q?=D0=93=D0=BE=D1=82=D0=BE=D0=B2=D0=B0=D1=8F=20?= =?UTF-8?q?=D1=80=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7=D0=B0=D1=86=D0=B8=D1=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Robot_balancer/PID/Drivers/LSM6DS3.h | 2 +- .../PID/Drivers/StepperMotorDriver.c | 71 ++++++++++++++++++ .../PID/Drivers/StepperMotorDriver.h | 16 ++++ Robot_balancer/PID/Utils/PID.c | 18 ++++- Robot_balancer/PID/Utils/PID.h | 2 + Robot_balancer/PID/main.cpp | 75 ++----------------- 6 files changed, 110 insertions(+), 74 deletions(-) diff --git a/Robot_balancer/PID/Drivers/LSM6DS3.h b/Robot_balancer/PID/Drivers/LSM6DS3.h index e5d87dc..cf028d5 100644 --- a/Robot_balancer/PID/Drivers/LSM6DS3.h +++ b/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) diff --git a/Robot_balancer/PID/Drivers/StepperMotorDriver.c b/Robot_balancer/PID/Drivers/StepperMotorDriver.c index e69de29..ade4873 100644 --- a/Robot_balancer/PID/Drivers/StepperMotorDriver.c +++ b/Robot_balancer/PID/Drivers/StepperMotorDriver.c @@ -0,0 +1,71 @@ +#include + +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; +} \ No newline at end of file diff --git a/Robot_balancer/PID/Drivers/StepperMotorDriver.h b/Robot_balancer/PID/Drivers/StepperMotorDriver.h index e69de29..4527db6 100644 --- a/Robot_balancer/PID/Drivers/StepperMotorDriver.h +++ b/Robot_balancer/PID/Drivers/StepperMotorDriver.h @@ -0,0 +1,16 @@ +#include "stm32g431xx.h" +#include + +#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); \ No newline at end of file diff --git a/Robot_balancer/PID/Utils/PID.c b/Robot_balancer/PID/Utils/PID.c index 6990e2c..cf3c19a 100644 --- a/Robot_balancer/PID/Utils/PID.c +++ b/Robot_balancer/PID/Utils/PID.c @@ -1,8 +1,8 @@ #include "PID.h" float KP = 100; -float KI = 1; -float KD = 1; +float KI = 0; +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; //Управляющее воздействие/крутящий момент -// } \ No newline at end of file +// } + +#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; +} \ No newline at end of file diff --git a/Robot_balancer/PID/Utils/PID.h b/Robot_balancer/PID/Utils/PID.h index 619b169..c1745d6 100644 --- a/Robot_balancer/PID/Utils/PID.h +++ b/Robot_balancer/PID/Utils/PID.h @@ -1,3 +1,5 @@ +#include +#include "StepperMotorDriver.h" float pid_update(float target, float current, float dt); float perevod(float pid_result); diff --git a/Robot_balancer/PID/main.cpp b/Robot_balancer/PID/main.cpp index ae34e16..0430f54 100644 --- a/Robot_balancer/PID/main.cpp +++ b/Robot_balancer/PID/main.cpp @@ -2,6 +2,7 @@ #include #include "LSM6DS3.h" #include "PID.h" +#include //--------------------------------------------------------------------------------------------------------------- // @@ -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->ARR = 1000 - 1; // Автоматическая перезагрузка (0.001 секунда) + TIM4->PSC = 17 - 1; // Предделитель 170 МГц / 17 = 10000 кГц + TIM4->ARR = 1000 - 1; // Автоматическая перезагрузка (0.0001 секунда) TIM4->DIER |= TIM_DIER_UIE; // Разрешить прерывание по обновлению TIM4->CR1 |= TIM_CR1_CEN; // Включить таймер NVIC_EnableIRQ(TIM4_IRQn); // Включение прерывания