diff --git a/Robot_balancer/PID/Drivers/StepperMotorDriver.c b/Robot_balancer/PID/Drivers/StepperMotorDriver.c
new file mode 100644
index 0000000..e69de29
diff --git a/Robot_balancer/PID/Drivers/StepperMotorDriver.h b/Robot_balancer/PID/Drivers/StepperMotorDriver.h
new file mode 100644
index 0000000..e69de29
diff --git a/Robot_balancer/PID/PID.ewp b/Robot_balancer/PID/PID.ewp
index 35f2d95..bd757a4 100644
--- a/Robot_balancer/PID/PID.ewp
+++ b/Robot_balancer/PID/PID.ewp
@@ -2233,6 +2233,12 @@
$PROJ_DIR$\Drivers\LSM6DS3.h
+
+ $PROJ_DIR$\Drivers\StepperMotorDriver.c
+
+
+ $PROJ_DIR$\Drivers\StepperMotorDriver.h
+
Utils
diff --git a/Robot_balancer/PID/PID.ewt b/Robot_balancer/PID/PID.ewt
index eda162c..d8811f0 100644
--- a/Robot_balancer/PID/PID.ewt
+++ b/Robot_balancer/PID/PID.ewt
@@ -2959,6 +2959,12 @@
$PROJ_DIR$\Drivers\LSM6DS3.h
+
+ $PROJ_DIR$\Drivers\StepperMotorDriver.c
+
+
+ $PROJ_DIR$\Drivers\StepperMotorDriver.h
+
Utils
diff --git a/Robot_balancer/PID/main.cpp b/Robot_balancer/PID/main.cpp
index 033973b..ae34e16 100644
--- a/Robot_balancer/PID/main.cpp
+++ b/Robot_balancer/PID/main.cpp
@@ -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,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_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; // Сброс режима
@@ -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);
+ }
+
}
}