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-28 15:00:53 +03:00
parent aa36b579a4
commit 95b0d059ce
5 changed files with 142 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
Robot_balancer/PID/Drivers/StepperMotorDriver.c Normal file
View File

0
Robot_balancer/PID/Drivers/StepperMotorDriver.h Normal file
View File

6
Robot_balancer/PID/PID.ewp
View File

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

6
Robot_balancer/PID/PID.ewt
View File

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

142
Robot_balancer/PID/main.cpp
View File

@ -10,8 +10,72 @@
//
//---------------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------
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;
@ -46,27 +110,33 @@ 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_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; // Сброс режима
GPIOA->OTYPER |= GPIO_OTYPER_OT8; // Установка в Open-Drain
@ -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 = (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 мкс
(4 << I2C_TIMINGR_SCLDEL_Pos) | // SCLDEL=8 -> t_HD;STA=0.38 мкс
(0 << I2C_TIMINGR_SDADEL_Pos) | // SDADEL=4 -> t_HD;DAT=0.019 мкс
(25 << I2C_TIMINGR_SCLH_Pos) | // SCLH=16 -> t_HIGH=0.75 мкс
(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);
}
}
}