arduino鸡头云台舵机

### 使用Arduino控制鸡头云台舵机

#### 初始化设置
为了实现鸡头云台的功能，首先需要初始化Arduino Nano、MPU6050陀螺仪以及舵机之间的连接。这一步骤确保所有组件能够正常通信并协同工作。

#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]

MPU6050 mpu;

#define LED_PIN 13
bool blinkState = false;

Servo servoPitch;
Servo servoYaw;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  pinMode(LED_PIN, OUTPUT);

  // Initialize MPU6050 and configure it for use with the DMP.
  mpu.initialize();

  // Load and configure the DMP.
  devStatus = mpu.dmpInitialize();
  mpu.setDMPEnabled(true);

  // Attach interrupt to handle data ready notifications from MPU6050.
  attachInterrupt(digitalPinToInterrupt(MPU_INT), dmpDataReady, RISING);
  mpuIntStatus = mpu.getIntStatus();

  // Set up servos on specific pins.
  servoPitch.attach(9); // Pitch axis (up/down).
  servoYaw.attach(10);  // Yaw axis (left/right).

  // Ensure servos are centered at start-up.
  servoPitch.writeMicroseconds(1500);
  servoYaw.writeMicroseconds(1500);
}

#### 姿态解算与PID控制
接下来，在主程序循环中读取来自MPU6050的数据，并通过PID算法处理这些数据来驱动舵机动作。此过程涉及到姿态角的实时计算和误差补偿机制的设计[^1]。

double Kp = 2.0, Ki = 0.0, Kd = 1.0; // PID coefficients
double prevErrorPitch = 0, integralPitch = 0;
double prevErrorYaw = 0, integralYaw = 0;

void loop() {
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get new sensor data.

    // Convert raw FIFO packet contents to Euler angles.
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    double targetAnglePitch = 0; // Desired angle for pitch.
    double currentAnglePitch = RAD_TO_DEG * ypr[PITCH];
    
    double errorPitch = targetAnglePitch - currentAnglePitch;
    integralPitch += errorPitch * dt;
    double derivativePitch = (errorPitch - prevErrorPitch) / dt;
    int outputPitch = constrain(Kp*errorPitch + Ki*integralPitch + Kd*derivativePitch, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
    
    servoPitch.writeMicroseconds(outputPitch);
    prevErrorPitch = errorPitch;

    double targetAngleYaw = 0; // Desired angle for yaw.
    double currentAngleYaw = RAD_TO_DEG * ypr[YAW];
    
    double errorYaw = targetAngleYaw - currentAngleYaw;
    integralYaw += errorYaw * dt;
    double derivativeYaw = (errorYaw - prevErrorYaw) / dt;
    int outputYaw = constrain(Kp*errorYaw + Ki*integralYaw + Kd*derivativeYaw, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
    
    servoYaw.writeMicroseconds(outputYaw);
    prevErrorYaw = errorYaw;
  }
}

void dmpDataReady() {
  mpuInterrupt = true;
}

上述代码实现了基本的PID控制系统框架，其中包含了对俯仰(Pitch)和偏航(Yaw)方向上的角度测量及相应的伺服电机响应逻辑[^2]。