Доброго дня!
проект- стабилизация площадки по одной оси.
В принципе 2 вопроса,
- почему в терминале пишет, что гира не инициализирована, хотя работает?
22:56:55.977 → Калибровка гироскопа… Не двигайте датчик!
22:57:04.043 → Смещения: -473.00, 245.00, 8.00
22:57:04.075 → MPU6050 ошибка - плата лежит без движений но показания расчетные уезжают…
22:57:04.299 → -----Roll: -0.38 rollOutput: 36.33
…
22:57:30.819 → -----Roll: -2.82 rollOutput: 40.40
#include <Wire.h>
#include <Servo.h>
#include <MPU6050.h>
MPU6050 mpu;
Servo servoRoll;
// Пин для сервопривода крена
#define SERVO_ROLL_PIN 9
// Параметры PID для крена
typedef struct {
double Kp, Ki, Kd;
double integral, prevError;
} PID;
PID rollPID = {1.3, // определяет, насколько сильно сервопривод будет реагировать на текущую ошибку. Уменьшение Kp сделает реакцию менее агрессивной.
0.05, //отвечает за устранение статической ошибки, но слишком большой Ki может привести к колебаниям. Уменьшение Ki сделает реакцию на медленные изменения менее агрессивной.
0.3, //пытается предсказать будущее поведение ошибки и замедлить движение сервопривода при приближении к целевой точке. Увеличение Kd поможет сгладить движение и уменьшить колебания.
0, 0}; // Настройте эти значения! {1.5, 0.05, 0.3, 0, 0};
// Фильтрация для крена
float compFilterCoeff = 0.98; // Коэффициент комплементарного фильтра
float dt = 0.02; // Время между измерениями (20 мс)
float rollAngle = 0;
/*
Фильтрация данных: Комплементарный фильтр уже используется, но можно попробовать еще больше его настроить, чтобы отфильтровывать шум.
*/
// Калибровка
#define CALIBRATION_SAMPLES 500
float gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0;
void setup() {
Serial.begin(19200);
// Инициализация сервопривода крена
servoRoll.attach(SERVO_ROLL_PIN);
centerServos();
// Инициализация MPU6050
Wire.begin();
mpu.initialize();
delay(500);
// Калибровка гироскопа
calibrateGyro();
// Проверка соединения
Serial.println(mpu.testConnection() ? "MPU6050 подключен" : "MPU6050 ошибка");
}
void loop() {
static uint32_t prevTime = millis();
// Чтение данных с датчика
int16_t ax, ay, az, gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// Учет калибровочных смещений
gx -= gyroXoffset;
gy -= gyroYoffset;
gz -= gyroZoffset;
// Расчет угла крена по акселерометру
float accelRoll = atan2(ay, az) * 180 / PI;
// Расчет угла крена по гироскопу (интегрирование)
float gyroRollRate = gx / 131;
rollAngle = compFilterCoeff * (rollAngle + gyroRollRate * dt) + (1 - compFilterCoeff) * accelRoll;
// ПИД-регуляция для крена
float rollOutput = computePID(&rollPID, 0 - rollAngle); // Цель - 0 градусов
rollOutput = 35 + constrain(rollOutput, -35, 35);
// Управление сервоприводом крена
servoRoll.write(rollOutput); // Ограничение ±60 градусов
// Отладка
Serial.print("-----Roll: ");
Serial.print(rollAngle);
Serial.print(" rollOutput: ");
Serial.println(rollOutput);
// Стабилизация частоты цикла
while (millis() - prevTime < dt * 1000) {}
prevTime = millis();
}
// ПИД-регулятор
float computePID(PID* pid, float error) {
pid->integral += error * dt;
pid->integral = constrain(pid->integral, -100, 100); // Антивиндруп
float derivative = (error - pid->prevError) / dt;
pid->prevError = error;
return pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;
}
// Калибровка гироскопа
void calibrateGyro() {
Serial.println("Калибровка гироскопа... Не двигайте датчик!");
delay(1000);
int16_t gx, gy, gz;
long gxSum = 0, gySum = 0, gzSum = 0;
for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
mpu.getRotation(&gx, &gy, &gz);
gxSum += gx;
gySum += gy;
gzSum += gz;
delay(5);
}
gyroXoffset = gxSum / CALIBRATION_SAMPLES;
gyroYoffset = gySum / CALIBRATION_SAMPLES;
gyroZoffset = gzSum / CALIBRATION_SAMPLES;
Serial.print("Смещения: ");
Serial.print(gyroXoffset);
Serial.print(", ");
Serial.print(gyroYoffset);
Serial.print(", ");
Serial.println(gyroZoffset);
}
// Установка серв в центральное положение
void centerServos() {
Serial.println(" Установка сервы в среднее положение!!");
servoRoll.write(35);
delay(1000);
}