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

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

Browse Source
This commit is contained in:
Vlad
2025-07-29 14:19:18 +03:00
parent 95b0d059ce
commit bbf07cc233
6 changed files with 110 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Robot_balancer/PID/Drivers/LSM6DS3.h
View File

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

71
Robot_balancer/PID/Drivers/StepperMotorDriver.c
View File

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

16
Robot_balancer/PID/Drivers/StepperMotorDriver.h
View File

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

18
Robot_balancer/PID/Utils/PID.c
View File

@ -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; //Управляющее воздействие/крутящий момент
// }
// }
#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;
}

2
Robot_balancer/PID/Utils/PID.h
View File

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

75
Robot_balancer/PID/main.cpp
View File

@ -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->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); // Включение прерывания