230 lines
12 KiB
C++
230 lines
12 KiB
C++
#include "stm32g431xx.h"
|
||
#include <math.h>
|
||
#include "LSM6DS3.h"
|
||
#include "PID.h"
|
||
|
||
//---------------------------------------------------------------------------------------------------------------
|
||
//
|
||
// PС11 - I2C3_SDA
|
||
// PA8 - I2C3_SCL
|
||
//
|
||
//---------------------------------------------------------------------------------------------------------------
|
||
//---------------------------------------------------------------------------------------------------------------
|
||
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;
|
||
float u;
|
||
extern "C" void TIM4_IRQHandler(void)
|
||
{
|
||
if (TIM4->SR & TIM_SR_UIF) // Проверить флаг обновления
|
||
{
|
||
TIM4->SR &= ~TIM_SR_UIF; // Сбросить флаг
|
||
|
||
counter++;
|
||
|
||
// Чтение данных акселерометра
|
||
LSM6DS3_ReadAccelerometer(&acc_x, &acc_y, &acc_z);
|
||
// Чтение данных гироскопа
|
||
LSM6DS3_ReadGyroscope(&gyro_x, &gyro_y, &gyro_z);
|
||
|
||
Filter_SensorData();
|
||
Complementary_Filter();
|
||
|
||
u = pid_update(target, angle_pitch, DT);
|
||
if (fabsf(u) < 0.001f){
|
||
u = 0.001f;
|
||
}
|
||
res = (48/u);
|
||
}
|
||
}
|
||
//---------------------------------------------------------------------------------------------------------------
|
||
|
||
//---------------------------------------------------------------------------------------------------------------
|
||
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
|
||
//-----------------------------------------------------------------------------------------------------------
|
||
GPIOA->OTYPER &= ~GPIO_OTYPER_OT8_Msk; // Сброс режима
|
||
GPIOC->OTYPER &= ~GPIO_OTYPER_OT11_Msk; // Сброс режима
|
||
GPIOA->OTYPER |= GPIO_OTYPER_OT8; // Установка в Open-Drain
|
||
GPIOC->OTYPER |= GPIO_OTYPER_OT11; // Установка в Open-Drain
|
||
//-----------------------------------------------------------------------------------------------------------
|
||
GPIOA->OSPEEDR &= ~GPIO_OSPEEDER_OSPEEDR8; // сброс скорости пина
|
||
GPIOC->OSPEEDR &= ~GPIO_OSPEEDER_OSPEEDR11; // сброс скорости пина
|
||
GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR8_1; // установка в высокую скорость
|
||
GPIOC->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR11_1; // установка в высокую скорость
|
||
//-----------------------------------------------------------------------------------------------------------
|
||
GPIOA->PUPDR &= ~GPIO_PUPDR_PUPD8_Msk; // очистка подтяжки
|
||
GPIOC->PUPDR &= ~GPIO_PUPDR_PUPD11_Msk; // очистка подтяжки
|
||
GPIOA->PUPDR |= GPIO_PUPDR_PUPD8_1; // вклчюение подтяжки PULL_UP
|
||
GPIOC->PUPDR |= GPIO_PUPDR_PUPD11_1; // вклчюение подтяжки PULL_UP
|
||
//-----------------------------------------------------------------------------------------------------------
|
||
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 МГц
|
||
(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)
|
||
; // задержка
|
||
//-----------------------------------------------------------------------------------------------------------
|
||
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);
|
||
}
|
||
|
||
}
|
||
}
|