Добавить starterkit.ino

starterkit.ino

@@ -0,0 +1,170 @@
+#include <I2Cdev.h>
+#include <MPU6050_6Axis_MotionApps20.h>
+#include <Wire.h>
+#include <Servo.h>
+#include <GyverPID.h>
+
+// Пины сервоприводов
+#define AILERON_LEFT_PIN 9
+#define AILERON_RIGHT_PIN 10
+#define ELEVATOR_PIN 11
+#define RUDDER_PIN 12
+
+// Объекты сервоприводов
+Servo aileron_left, aileron_right, elevator, rudder;
+
+// ПИД-регуляторы (Kp, Ki, Kd, dt)
+GyverPID pid_roll(6.0, 0.2, 20.0, 10);   // Крен
+GyverPID pid_pitch(5.0, 0.1, 15.0, 10); // Тангаж
+GyverPID pid_yaw(3.0, 0.05, 8.0, 10);   // Рыскание
+
+// MPU6050 и DMP
+MPU6050 mpu;
+bool dmpReady = false;
+uint8_t mpuIntStatus;
+uint8_t devStatus;
+uint16_t packetSize;
+uint16_t fifoCount;
+uint8_t fifoBuffer[64];
+Quaternion q;
+VectorFloat gravity;
+float ypr[3]; // [yaw, pitch, roll]
+
+// Эмулированные параметры
+float target_altitude = 100.0;
+volatile bool mpuInterrupt = false;
+
+// ================================================================
+// Прерывание для DMP
+// ================================================================
+void dmpDataReady() {
+  mpuInterrupt = true;
+}
+
+// ================================================================
+// Настройка
+// ================================================================
+void setup() {
+  Wire.begin();
+  Wire.setClock(400000);
+  Serial.begin(115200);
+
+  // Инициализация MPU6050
+  mpu.initialize();
+  devStatus = mpu.dmpInitialize();
+  
+  // Калибровка гироскопа (значения для вашего модуля!)
+  mpu.setXGyroOffset(220);
+  mpu.setYGyroOffset(76);
+  mpu.setZGyroOffset(-85);
+  mpu.setZAccelOffset(1788);
+
+  if (devStatus == 0) {
+    mpu.setDMPEnabled(true);
+    attachInterrupt(0, dmpDataReady, RISING);
+    mpuIntStatus = mpu.getIntStatus();
+    dmpReady = true;
+    packetSize = mpu.dmpGetFIFOPacketSize();
+  }
+
+  // Инициализация сервоприводов
+  aileron_left.attach(AILERON_LEFT_PIN);
+  aileron_right.attach(AILERON_RIGHT_PIN);
+  elevator.attach(ELEVATOR_PIN);
+  rudder.attach(RUDDER_PIN);
+
+  // Настройка PID
+  pid_roll.setDirection(NORMAL);
+  pid_pitch.setDirection(NORMAL);
+  pid_yaw.setDirection(NORMAL);
+  
+  pid_roll.setLimits(-50, 50);
+  pid_pitch.setLimits(-30, 30);
+  pid_yaw.setLimits(-25, 25);
+}
+
+// ================================================================
+// Основной цикл
+// ================================================================
+void loop() {
+  if (!dmpReady) return;
+
+  // Получение данных с DMP
+  if (mpuInterrupt && fifoCount < packetSize) {
+    mpuInterrupt = false;
+    mpuIntStatus = mpu.getIntStatus();
+    fifoCount = mpu.getFIFOCount();
+    
+    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
+      mpu.resetFIFO();
+    } else if (mpuIntStatus & 0x02) {
+      while (fifoCount >= packetSize) {
+        mpu.getFIFOBytes(fifoBuffer, packetSize);
+        fifoCount -= packetSize;
+      }
+
+      // Получение углов Эйлера
+      mpu.dmpGetQuaternion(&q, fifoBuffer);
+      mpu.dmpGetGravity(&gravity, &q);
+      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+      
+      // Преобразование радианы -> градусы
+      float roll = ypr[2] * 180/M_PI;
+      float pitch = ypr[1] * 180/M_PI;
+      float yaw = ypr[0] * 180/M_PI;
+
+      // Управление высотой через тангаж
+      pid_pitch.setpoint = calculate_pitch_setpoint(target_altitude);
+      
+      // Обновление PID
+      pid_roll.input = roll;
+      pid_pitch.input = pitch;
+      pid_yaw.input = yaw;
+
+      pid_roll.getResult();
+      pid_pitch.getResult();
+      pid_yaw.getResult();
+
+      // Управление сервами
+      control_ailerons(pid_roll.output);
+      control_elevator(pid_pitch.output);
+      control_rudder(pid_yaw.output);
+    }
+  }
+}
+
+// ================================================================
+// Управление элеронами (противофазное)
+// ================================================================
+void control_ailerons(float pid_output) {
+  int left_angle = 90 + pid_output;   // Нейтрал 90°
+  int right_angle = 90 - pid_output;
+  
+  aileron_left.write(constrain(left_angle, 0, 180));
+  aileron_right.write(constrain(right_angle, 0, 180));
+}
+
+// ================================================================
+// Управление рулём высоты
+// ================================================================
+void control_elevator(float pid_output) {
+  int angle = 90 + pid_output;
+  elevator.write(constrain(angle, 0, 180));
+}
+
+// ================================================================
+// Управление рулём направления
+// ================================================================
+void control_rudder(float pid_output) {
+  int angle = 90 + pid_output;
+  rudder.write(constrain(angle, 0, 180));
+}
+
+// ================================================================
+// Эмуляция высоты и расчёт тангажа
+// ================================================================
+float calculate_pitch_setpoint(float target_alt) {
+  static float current_alt = 100.0;  // Эмуляция текущей высоты
+  float alt_error = target_alt - current_alt;
+  return constrain(alt_error * 0.5, -15, 15);
+}