diff --git a/Robot_balancer/PID/Drivers/I2C.c b/Robot_balancer/PID/Drivers/I2C.c new file mode 100644 index 0000000..8114ae1 --- /dev/null +++ b/Robot_balancer/PID/Drivers/I2C.c @@ -0,0 +1,88 @@ +#include + +void I2C3_Stop() +{ + I2C3->CR2 |= I2C_CR2_STOP; // установка бит стоп + + while (!(I2C3->ISR & I2C_ISR_STOPF)) + __NOP(); // Ждём остановку + I2C3->ICR |= I2C_ICR_STOPCF; // Сброс флага STOP + + I2C3->CR2 = 0; // сброс настроек паредачи-приёма +} +//--------------------------------------------------------------------------------------------------------------- +// +// чтение +// +void I2C3_Read(unsigned char Address, unsigned char *Data, unsigned char Size) +{ + while (I2C3->ISR & I2C_ISR_BUSY) + __NOP(); // Ждём освобождения линии I2C3 + //----------------------------------------------------------------------------------------------------------- + // I2C3->CR2 &= ~(I2C_CR2_SADD_Msk); // сброс адреса Slave + // I2C3->CR2 &= ~(I2C_CR2_NBYTES_Msk); // сброс кол-ва принимаемых байт I2C3 + + I2C3->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_NBYTES_Msk); // сокращение вызовов команд для установки битов в один вызов; + //----------------------------------------------------------------------------------------------------------- + // I2C3->CR2 |= (Address << (I2C_CR2_SADD_Pos)) & I2C_CR2_SADD_Msk; // Установка адреса Slave + // I2C3->CR2 |= (Size << (I2C_CR2_NBYTES_Pos)) & I2C_CR2_NBYTES_Msk; // Установка кол-ва байтов принимаемых + // I2C3->CR2 |= I2C_CR2_RD_WRN; // режим чтения + + I2C3->CR2 |= ((Address << (I2C_CR2_SADD_Pos)) & I2C_CR2_SADD_Msk) | // сокращение вызовов команд для установки битов в один вызов; + ((Size << (I2C_CR2_NBYTES_Pos)) & I2C_CR2_NBYTES_Msk) | + I2C_CR2_RD_WRN; + //----------------------------------------------------------------------------------------------------------- + I2C3->CR2 &= ~(I2C_CR2_AUTOEND | I2C_CR2_RELOAD); // отключение автозавершения и автоповторения + I2C3->CR2 |= I2C_CR2_AUTOEND; // включение автозавершения + //----------------------------------------------------------------------------------------------------------- + I2C3->CR2 |= I2C_CR2_START; // старт чтения + //----------------------------------------------------------------------------------------------------------- + while (Size--) // Проход по байтам + { + while (!(I2C3->ISR & I2C_ISR_RXNE)) + __NOP(); // Ждём байт + *Data++ = I2C3->RXDR; // Запись байта + } + //----------------------------------------------------------------------------------------------------------- + while (!(I2C3->ISR & I2C_ISR_STOPF)) + __NOP(); // Ждём остановку + I2C3->ICR |= I2C_ICR_STOPCF; // Сброс флага STOP +} +//--------------------------------------------------------------------------------------------------------------- +// +// запись +// +void I2C3_Write(unsigned char Address, unsigned char *Data, unsigned char Size) +{ + while (I2C3->ISR & I2C_ISR_BUSY) + __NOP(); // Ждём освобождения линии I2C3 + //----------------------------------------------------------------------------------------------------------- + // I2C3->CR2 &= ~(I2C_CR2_SADD_Msk); // сброс адреса Slave + // I2C3->CR2 &= ~(I2C_CR2_NBYTES_Msk); // сброс кол-ва записываемых байт I2C3 + + I2C3->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_NBYTES_Msk); // сокращение вызовов команд для установки битов в один вызов; + //----------------------------------------------------------------------------------------------------------- + // I2C3->CR2 |= (Address << (I2C_CR2_SADD_Pos)) & I2C_CR2_SADD_Msk; // Установка адреса Slave + // I2C3->CR2 |= (Size << (I2C_CR2_NBYTES_Pos)) & I2C_CR2_NBYTES_Msk; // Установка кол-ва байтов отдаваемых + // I2C3->CR2 |= I2C_CR2_RD_WRN; // режим записи + + I2C3->CR2 |= ((Address << (I2C_CR2_SADD_Pos)) & I2C_CR2_SADD_Msk) | // сокращение вызовов команд для установки битов в один вызов; + ((Size << (I2C_CR2_NBYTES_Pos)) & I2C_CR2_NBYTES_Msk); + I2C3->CR2 &= ~I2C_CR2_RD_WRN; // режим записи + //----------------------------------------------------------------------------------------------------------- + I2C3->CR2 &= ~(I2C_CR2_AUTOEND | I2C_CR2_RELOAD); // отключение автозавершения и автоповторения + I2C3->CR2 |= I2C_CR2_AUTOEND; // включение автозавершения + //----------------------------------------------------------------------------------------------------------- + I2C3->CR2 |= I2C_CR2_START; // старт записи + //----------------------------------------------------------------------------------------------------------- + while (Size--) // проход по байтам + { + while (!(I2C3->ISR & I2C_ISR_TXE)) + __NOP(); // Ждём байт + I2C3->TXDR = *Data++; // Запись байта + } + //----------------------------------------------------------------------------------------------------------- + while (!(I2C3->ISR & I2C_ISR_STOPF)) + __NOP(); // Ждём остановку + I2C3->ICR |= I2C_ICR_STOPCF; // Сброс флага STOP +} \ No newline at end of file diff --git a/Robot_balancer/PID/Drivers/I2C.h b/Robot_balancer/PID/Drivers/I2C.h new file mode 100644 index 0000000..33d1b44 --- /dev/null +++ b/Robot_balancer/PID/Drivers/I2C.h @@ -0,0 +1,4 @@ +#include "stm32g431xx.h" +void I2C3_Stop(); +void I2C3_Read(unsigned char Address, unsigned char *Data, unsigned char Size); +void I2C3_Write(unsigned char Address, unsigned char *Data, unsigned char Size); diff --git a/Robot_balancer/PID/Drivers/LSM6DS3.c b/Robot_balancer/PID/Drivers/LSM6DS3.c new file mode 100644 index 0000000..a07fc6e --- /dev/null +++ b/Robot_balancer/PID/Drivers/LSM6DS3.c @@ -0,0 +1,134 @@ +#include "LSM6DS3.h" + + +volatile float accel_filter_coeff = 0.3f; // Коэффициент ФНЧ для акселерометра (0.1-0.5) +volatile float gyro_filter_coeff = 0.2f; // Коэффициент ФНЧ для гироскопа (0.1-0.3) +volatile float comp_filter_coeff = 0.93f; // Коэффициент комплементарного фильтра (0.9-0.98) + +volatile int16_t filt_acc_x, filt_acc_y, filt_acc_z; +volatile int16_t filt_gyro_x, filt_gyro_y, filt_gyro_z; +volatile int16_t angle_pitch, angle_roll; +// Функция для чтения одного регистра +uint8_t LSM6DS3_ReadRegister(uint8_t reg) +{ + uint8_t value; + I2C3_Write(LSM6DS3_ADDRESS, ®, 1); // Отправляем адрес регистра + I2C3_Read(LSM6DS3_ADDRESS, &value, 1); // Читаем значение регистра + return value; +} +//--------------------------------------------------------------------------------------------------------------- +// Функция для записи в регистр +void LSM6DS3_WriteRegister(uint8_t reg, uint8_t value) +{ + uint8_t data[2] = {reg, value}; + I2C3_Write(LSM6DS3_ADDRESS, data, 2); +} +//--------------------------------------------------------------------------------------------------------------- +// Инициализация LSM6DS3 +void LSM6DS3_Init() +{ + // Проверка WHO_AM_I (должен вернуть 0x69) + uint8_t who_am_i = LSM6DS3_ReadRegister(LSM6DS3_WHO_AM_I); + if (who_am_i != 0x69) + { + // Ошибка - датчик не ответил + while (1) + ; + } + + // Настройка акселерометра (104 Гц, +-2g) + LSM6DS3_WriteRegister(LSM6DS3_CTRL1_XL, 0x40); + + // Настройка гироскопа (104 Гц, 245 dps) + LSM6DS3_WriteRegister(LSM6DS3_CTRL2_G, 0x40); +} +//--------------------------------------------------------------------------------------------------------------- +// Чтение данных акселерометра +void LSM6DS3_ReadAccelerometer(volatile int16_t *x, volatile int16_t *y, volatile int16_t *z) +{ + uint8_t data[6]; + uint8_t reg = LSM6DS3_OUTX_L_A; // Начинаем чтение с первого регистра данных акселерометра + + I2C3_Write(LSM6DS3_ADDRESS, ®, 1); + I2C3_Read(LSM6DS3_ADDRESS, data, 6); + + *x = (int16_t)((data[1] << 8) | data[0]); + *y = (int16_t)((data[3] << 8) | data[2]); + *z = (int16_t)((data[5] << 8) | data[4]); +} +//--------------------------------------------------------------------------------------------------------------- +// Чтение данных гироскопа +void LSM6DS3_ReadGyroscope(volatile int16_t *x, volatile int16_t *y, volatile int16_t *z) +{ + uint8_t data[6]; + uint8_t reg = LSM6DS3_OUTX_L_G; // Начинаем чтение с первого регистра данных гироскопа + + I2C3_Write(LSM6DS3_ADDRESS, ®, 1); + I2C3_Read(LSM6DS3_ADDRESS, data, 6); + + *x = (int16_t)((data[1] << 8) | data[0]); + *y = (int16_t)((data[3] << 8) | data[2]); + *z = (int16_t)((data[5] << 8) | data[4]); +} + +volatile int16_t acc_x, acc_y, acc_z; +volatile int16_t gyro_x, gyro_y, gyro_z; +//--------------------------------------------------------------------------------------------------------------- +void Filter_SensorData() +{ + // Более быстрая фильтрация акселерометра + filt_acc_x += accel_filter_coeff * (acc_x - filt_acc_x); + filt_acc_y += accel_filter_coeff * (acc_y - filt_acc_y); + filt_acc_z += accel_filter_coeff * (acc_z - filt_acc_z); + + // Фильтрация гироскопа с преобразованием + filt_gyro_x += gyro_filter_coeff * ((gyro_x / GYRO_SCALE) - filt_gyro_x); + filt_gyro_y += gyro_filter_coeff * ((gyro_y / GYRO_SCALE) - filt_gyro_y); + filt_gyro_z += gyro_filter_coeff * ((gyro_z / GYRO_SCALE) - filt_gyro_z); +} + +// Быстрый расчет углов по акселерометру +void Calculate_Accel_Angles() +{ + // Быстрая нормализация + float inv_norm = 1.0f / sqrt(filt_acc_x * filt_acc_x + filt_acc_y * filt_acc_y + filt_acc_z * filt_acc_z); + float ax = filt_acc_x * inv_norm; + float ay = filt_acc_y * inv_norm; + float az = filt_acc_z * inv_norm; + + // Оптимизированный расчет углов + angle_pitch = -atan2(ay, az) * RAD_TO_DEG; + angle_roll = atan2(ax, sqrt(ay * ay + az * az)) * RAD_TO_DEG; +} + +// Ускоренный комплементарный фильтр +void Complementary_Filter() +{ + static float pitch = 0, roll = 0; + + // Получаем углы из акселерометра + Calculate_Accel_Angles(); + + // Интегрируем данные гироскопа с небольшим усилением + pitch += filt_gyro_x * DT * 1.1f; // +10% к скорости отклика + roll += filt_gyro_y * DT * 1.1f; + + // Комплементарный фильтр с динамическими коэффициентами + float alpha = comp_filter_coeff; + + // Автоматическая подстройка коэффициента при быстром движении + if (fabsf(filt_gyro_x) > 20.0f || fabsf(filt_gyro_y) > 20.0f) + { + alpha = 0.96f; // Больше доверия гироскопу при быстром движении + } + + pitch = pitch * alpha + angle_pitch * (1.0f - alpha); + roll = roll * alpha + angle_roll * (1.0f - alpha); + + // Ограничение углов + pitch = fmod(pitch, 360.0f); + roll = fmod(roll, 360.0f); + + angle_pitch = pitch; + angle_roll = roll; +} \ No newline at end of file diff --git a/Robot_balancer/PID/Drivers/LSM6DS3.h b/Robot_balancer/PID/Drivers/LSM6DS3.h new file mode 100644 index 0000000..e5d87dc --- /dev/null +++ b/Robot_balancer/PID/Drivers/LSM6DS3.h @@ -0,0 +1,49 @@ +#include "stm32g431xx.h" +#include "I2C.h" +#include +#define LSM6DS3_ADDRESS 0xD6 + +// Регистры LSM6DS3 +#define LSM6DS3_WHO_AM_I 0x0F +#define LSM6DS3_CTRL1_XL 0x10 +#define LSM6DS3_CTRL2_G 0x11 +#define LSM6DS3_OUTX_L_G 0x22 +#define LSM6DS3_OUTX_H_G 0x23 +#define LSM6DS3_OUTY_L_G 0x24 +#define LSM6DS3_OUTY_H_G 0x25 +#define LSM6DS3_OUTZ_L_G 0x26 +#define LSM6DS3_OUTZ_H_G 0x27 +#define LSM6DS3_OUTX_L_A 0x28 // Правильное название регистра для акселерометра +#define LSM6DS3_OUTX_H_A 0x29 +#define LSM6DS3_OUTY_L_A 0x2A +#define LSM6DS3_OUTY_H_A 0x2B +#define LSM6DS3_OUTZ_L_A 0x2C +#define LSM6DS3_OUTZ_H_A 0x2D + +uint8_t LSM6DS3_ReadRegister(uint8_t reg); +void LSM6DS3_WriteRegister(uint8_t reg, uint8_t value); +void LSM6DS3_Init(); +void LSM6DS3_ReadAccelerometer(volatile int16_t *x, volatile int16_t *y, volatile int16_t *z); +void LSM6DS3_ReadGyroscope(volatile int16_t *x, volatile int16_t *y, volatile int16_t *z); + +extern volatile int16_t acc_x, acc_y, acc_z; +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 мс) + +// Настраиваемые коэффициенты фильтрации +extern volatile float accel_filter_coeff; // Коэффициент ФНЧ для акселерометра (0.1-0.5) +extern volatile float gyro_filter_coeff; // Коэффициент ФНЧ для гироскопа (0.1-0.3) +extern volatile float comp_filter_coeff; // Коэффициент комплементарного фильтра (0.9-0.98) + +extern volatile int16_t filt_acc_x, filt_acc_y, filt_acc_z; +extern volatile int16_t filt_gyro_x, filt_gyro_y, filt_gyro_z; +extern volatile int16_t angle_pitch, angle_roll; + +void Filter_SensorData(); +void Calculate_Accel_Angles(); +void Complementary_Filter(); + diff --git a/Robot_balancer/PID/PID.ewp b/Robot_balancer/PID/PID.ewp index 05ec020..35f2d95 100644 --- a/Robot_balancer/PID/PID.ewp +++ b/Robot_balancer/PID/PID.ewp @@ -358,6 +358,7 @@ CCIncludePath2 $PROJ_DIR$/Core $PROJ_DIR$/Drivers + $PROJ_DIR$/Utils