手機拍照穩定器項目_代碼解析

這是一個非常簡單的代碼，用於學習可以，但是實際應用並不是很順暢。

具體解析如下：

/*
  DIY Gimbal - MPU6050 Arduino Tutorial
  by Dejan, www.HowToMechatronics.com
  Code based on the MPU6050_DMP6 example from the i2cdevlib library by Jeff Rowberg:
  https://github.com/jrowberg/i2cdevlib
*/
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
 
#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file 如果使用MotionApps包含文件，則沒有必要
 
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#include <Servo.h>
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here 特定的I2C地址可以作爲參數在這裏傳遞
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
 
// Define the 3 servo motors 定義3個伺服電機
Servo servo0;
Servo servo1;
Servo servo2;
float correct;
int j = 0;
 
#define OUTPUT_READABLE_YAWPITCHROLL
 
#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards  定義中斷引腳，UNO和大多數板子是pin 2
 
bool blinkState = false;
 
// MPU control/status vars 微處理機控制/狀態變量
bool dmpReady = false;  // set true if DMP init was successful 如果DMP 初始化成功，則設置爲true
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU 保持從MPU讀取實際的中斷狀態
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error) 返回每個設備操作後狀態
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes) 預期的DMP包大小(默認爲42字節)
uint16_t fifoCount;     // count of all bytes currently in FIFO 當前FIFO中所有字節的計數
uint8_t fifoBuffer[64]; // FIFO storage buffer FIFO存儲緩衝區
 
// orientation/motion vars  方向/運動變量
Quaternion q;           // [w, x, y, z]         quaternion container 四元數的容器
VectorInt16 aa;         // [x, y, z]            accel sensor measurements 加速度傳感器測量
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements 無重力加速度傳感器測量
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements 世界框架加速度傳感器測量
VectorFloat gravity;    // [x, y, z]            gravity vector 重力向量
float euler[3];         // [psi, theta, phi]    Euler angle container 歐拉角容器
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector 偏航/俯仰/滾動容器和重力矢量
 
// packet structure for InvenSense teapot demo 用於InvenSense茶壺演示包結構
uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };
 
 
 
// ================================================================
// ===               INTERRUPT DETECTION ROUTINE  中斷檢測程序              ===
// ================================================================
 
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high 指示MPU中斷引腳是否高
void dmpDataReady() {
  mpuInterrupt = true;
}
 
// ================================================================
// ===                      INITIAL SETUP    初始化設置                   ===
// ================================================================
 
void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically) 加入I2C總線(I2Cdev庫不會自動這樣做)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties 400千赫I2C時鐘。如果有編譯困難，請註釋這一行
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
 
  // initialize serial communication 初始化串行通信
  // (115200 chosen because it is required for Teapot Demo output, but it's  選擇115200是因爲茶壺演示輸出需要它，但它是
  // really up to you depending on your project) 取決於你的項目)
  Serial.begin(38400);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately 等待列奧納多枚舉，其他立即繼續
 
  // initialize device 初始化設備
  //Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  devStatus = mpu.dmpInitialize();
  // supply your own gyro offsets here, scaled for min sensitivity 在這裏提供你自己的陀螺儀偏移量，測量最小靈敏度
  mpu.setXGyroOffset(17);
  mpu.setYGyroOffset(-69);
  mpu.setZGyroOffset(27);
  mpu.setZAccelOffset(1551); // 1688 factory default for my test chip 我的測試芯片1688工廠默認值
 
  // make sure it worked (returns 0 if so) 確保它工作正常(如果工作正常返回0)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready  打開DMP，現在它已經準備好了
    // Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);
 
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
 
    // set our DMP Ready flag so the main loop() function knows it's okay to use it 設置我們的DMP就緒標誌，以便主循環()函數知道可以使用它
    //Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;
 
    // get expected DMP packet size for later comparison 獲取預期的DMP包大小，以便以後進行比較
    packetSize = mpu.dmpGetFIFOPacketSize();
  }

else {
    // ERROR! 錯誤
    // 1 = initial memory load failed 初始內存加載失敗
    // 2 = DMP configuration updates failed DMP配置更新失敗
    // (if it's going to break, usually the code will be 1) (如果它要崩潰，通常代碼是1)
    // Serial.print(F("DMP Initialization failed (code ")); 串口輸出(F(“DMP初始化失敗(代碼)”);
    //Serial.print(devStatus);
    //Serial.println(F(")"));
  }
 
  // Define the pins to which the 3 servo motors are connected 定義連接3個伺服電機的引腳
  servo0.attach(10);
  servo1.attach(9);
  servo2.attach(8);
}
// ================================================================
// ===                    MAIN PROGRAM LOOP     主程序循環                 ===
// ================================================================
 
void loop() {
  // if programming failed, don't try to do anything 如果編程失敗了，不要嘗試做任何事情
  if (!dmpReady) return;
 
  // wait for MPU interrupt or extra packet(s) available 等待MPU中斷或額外的包可用
  while (!mpuInterrupt && fifoCount < packetSize) {
    if (mpuInterrupt && fifoCount < packetSize) {
      // try to get out of the infinite loop 試着跳出無限循環
      fifoCount = mpu.getFIFOCount();
    }
  }
 
  // reset interrupt flag and get INT_STATUS byte 重置中斷標誌並獲取INT_STATUS字節
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
 
  // get current FIFO count 獲取當前FIFO計數
  fifoCount = mpu.getFIFOCount();
 
  // check for overflow (this should never happen unless our code is too inefficient) 檢查溢出(除非我們的代碼效率太低，否則不應該發生這種情況)
  if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
    // reset so we can continue cleanly 重置，我們可以乾淨的繼續
    mpu.resetFIFO();
    fifoCount = mpu.getFIFOCount();
    Serial.println(F("FIFO overflow!"));
 
    // otherwise, check for DMP data ready interrupt (this should happen frequently) 否則，檢查DMP數據準備中斷(這應該經常發生)
  } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
    // wait for correct available data length, should be a VERY short wait 等待正確的可用數據長度，應該是很短的等待時間
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
 
    // read a packet from FIFO 從FIFO中讀取數據包
    mpu.getFIFOBytes(fifoBuffer, packetSize);
 
    // track FIFO count here in case there is > 1 packet available 跟蹤先進先出計數在這裏，如果有> 1包可用
    // (this lets us immediately read more without waiting for an interrupt)(這讓我們可以立即讀取更多內容，而不需要等待中斷)
    fifoCount -= packetSize;
 
    // Get Yaw, Pitch and Roll values 得到偏航，俯仰和橫滾值
#ifdef OUTPUT_READABLE_YAWPITCHROLL
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
 
    // Yaw, Pitch, Roll values - Radians to degrees 偏航，俯仰，橫搖值-弧度到角度
    ypr[0] = ypr[0] * 180 / M_PI;
    ypr[1] = ypr[1] * 180 / M_PI;
    ypr[2] = ypr[2] * 180 / M_PI;
    
    // Skip 300 readings (self-calibration process) 跳過300個讀數(自校準過程)
    if (j <= 300) {
      correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings 偏航從隨機值開始，所以我們捕獲300個讀數後的最後一個值
      j++;
    }
    // After 300 readings 在300個讀數後
    else {
      ypr[0] = ypr[0] - correct; // Set the Yaw to 0 deg - subtract  the last random Yaw value from the currrent value to make the Yaw 0 degrees 設置偏航爲0度-從當前值中減去最後一個隨機偏航值，使偏航爲0度
      // Map the values of the MPU6050 sensor from -90 to 90 to values suatable for the servo control from 0 to 180 將MPU6050傳感器的值從-90映射到可用於0到180的伺服控制的值
      int servo0Value = map(ypr[0], -90, 90, 0, 180);
      int servo1Value = map(ypr[1], -90, 90, 0, 180);
      int servo2Value = map(ypr[2], -90, 90, 180, 0);
      
      // Control the servos according to the MPU6050 orientation 根據MPU6050方向控制伺服系統
      servo0.write(servo0Value);
      servo1.write(servo1Value);
      servo2.write(servo2Value);
    }
#endif
  }
}

 

/*
  DIY Gimbal - MPU6050 Arduino Tutorial
  by Dejan, www.HowToMechatronics.com
  Code based on the MPU6050_DMP6 example from the i2cdevlib library by Jeff Rowberg:
  https://github.com/jrowberg/i2cdevlib
*/
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
 
#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file
 
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#include <Servo.h>
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
 
// Define the 3 servo motors
Servo servo0;
Servo servo1;
Servo servo2;
float correct;
int j = 0;
 
#define OUTPUT_READABLE_YAWPITCHROLL
 
#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
 
bool blinkState = false;
 
// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
 
// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
 
// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };
 
 
 
// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================
 
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}
 
// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================
 
void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
 
  // initialize serial communication
  // (115200 chosen because it is required for Teapot Demo output, but it's
  // really up to you depending on your project)
  Serial.begin(38400);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately
 
  // initialize device
  //Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  devStatus = mpu.dmpInitialize();
  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(17);
  mpu.setYGyroOffset(-69);
  mpu.setZGyroOffset(27);
  mpu.setZAccelOffset(1551); // 1688 factory default for my test chip
 
  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    // Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);
 
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
 
    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    //Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;
 
    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    // Serial.print(F("DMP Initialization failed (code "));
    //Serial.print(devStatus);
    //Serial.println(F(")"));
  }
 
  // Define the pins to which the 3 servo motors are connected
  servo0.attach(10);
  servo1.attach(9);
  servo2.attach(8);
}
// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================
 
void loop() {
  // if programming failed, don't try to do anything
  if (!dmpReady) return;
 
  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize) {
    if (mpuInterrupt && fifoCount < packetSize) {
      // try to get out of the infinite loop
      fifoCount = mpu.getFIFOCount();
    }
  }
 
  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
 
  // get current FIFO count
  fifoCount = mpu.getFIFOCount();
 
  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    fifoCount = mpu.getFIFOCount();
    Serial.println(F("FIFO overflow!"));
 
    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
 
    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);
 
    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount -= packetSize;
 
    // Get Yaw, Pitch and Roll values
#ifdef OUTPUT_READABLE_YAWPITCHROLL
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
 
    // Yaw, Pitch, Roll values - Radians to degrees
    ypr[0] = ypr[0] * 180 / M_PI;
    ypr[1] = ypr[1] * 180 / M_PI;
    ypr[2] = ypr[2] * 180 / M_PI;
    
    // Skip 300 readings (self-calibration process)
    if (j <= 300) {
      correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
      j++;
    }
    // After 300 readings
    else {
      ypr[0] = ypr[0] - correct; // Set the Yaw to 0 deg - subtract  the last random Yaw value from the currrent value to make the Yaw 0 degrees
      // Map the values of the MPU6050 sensor from -90 to 90 to values suatable for the servo control from 0 to 180
      int servo0Value = map(ypr[0], -90, 90, 0, 180);
      int servo1Value = map(ypr[1], -90, 90, 0, 180);
      int servo2Value = map(ypr[2], -90, 90, 180, 0);
      
      // Control the servos according to the MPU6050 orientation
      servo0.write(servo0Value);
      servo1.write(servo1Value);
      servo2.write(servo2Value);
    }
#endif
  }
}