DIY FPV Head Tracking Devresi. Kodlar ve Mekanik.

				
					// FPV HEAD TRACKING TRANSMITTER CODE  |  VERİCİ KODU
// BY KendinYap

#include <Wire.h>
#include "MPU6050_6Axis_MotionApps20.h"
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Servo.h>

/* --- NRF24L01 Setup --- */
RF24 radio(8, 10);
const byte address[6] = "00001";

struct Data_Package {
  byte pitch;
  byte yaw;
  bool calibrationDone;
};
Data_Package data;

/* --- MPU6050 Setup --- */
MPU6050 mpu;
#define INTERRUPT_PIN 2
bool dmpReady = false;
uint8_t mpuIntStatus;
uint16_t packetSize;
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
float ypr[3];
volatile bool mpuInterrupt = false;

/* --- Servo Setup --- */
Servo pitchServo;
Servo yawServo;
#define PITCH_SERVO_PIN 3
#define YAW_SERVO_PIN 4

void dmpDataReady() {
  mpuInterrupt = true;
}

void resetData() {
  data.pitch = 127;
  data.yaw = 127;
  data.calibrationDone = false;
}

void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000);

  Serial.println(F("Initializing MPU6050..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  
  if (!mpu.testConnection()) {
    Serial.println(F("MPU6050 connection failed"));
    while (true);
  }

  Serial.println(F("MPU6050 connection successful"));
  uint8_t devStatus = mpu.dmpInitialize();

  if (devStatus == 0) {
    Serial.println(F("Calibrating MPU6050..."));
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.setDMPEnabled(true);
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();

    data.calibrationDone = true;
    for (int i = 0; i < 5; i++) {
      if (radio.write(&data, sizeof(Data_Package))) {
        Serial.println(F("Calibration done signal sent!"));
        break;
      }
      delay(200);
    }
    delay(1000);
    data.calibrationDone = false;

    pitchServo.attach(PITCH_SERVO_PIN);
    yawServo.attach(YAW_SERVO_PIN);
    pitchServo.write(90 + 60);
    delay(500);
    yawServo.write(90 + 60);
    delay(500);
    pitchServo.write(90);
    delay(500);
    yawServo.write(90);
  } else {
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

  radio.begin();
  radio.openWritingPipe(address);
  radio.setChannel(120);
  radio.setAutoAck(false);
  radio.setDataRate(RF24_250KBPS);
  radio.setPALevel(RF24_PA_MAX);
  radio.stopListening();
  resetData();
}

void loop() {
  if (!dmpReady) return;

  if (mpuInterrupt || mpu.getFIFOCount() >= packetSize) {
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    if ((mpuIntStatus & 0x10) || mpu.getFIFOCount() == 1024) {
      mpu.resetFIFO();
      Serial.println("FIFO overflow!");
      return;
    }

    if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

      data.yaw = constrain(map(ypr[0] * 180 / M_PI, 90, -90, 0, 255), 0, 255);
      data.pitch = constrain(map(ypr[2] * 180 / M_PI, 90, -90, 0, 255), 0, 255);

      Serial.print("Yaw: ");
      Serial.print(data.yaw);
      Serial.print(" Pitch: ");
      Serial.println(data.pitch);

      for (int i = 0; i < 3; i++) {
        if (radio.write(&data, sizeof(Data_Package))) {
          Serial.println("Data sent successfully");
          break;
        }
        delay(100);
      }
    }
  }
}