Stabilizator pentru Cameră (Gimbal)

Introducere

Dispozitivul are rolul de a menține nemișcată camera (sau orice obiect care încape pe platformă), intr-o poziție prestabilită de catre utilizator, indiferent de miscarea utilizatorului. Dispozitivul este destinat oamenilor care filmează videoclip-uri in miscare, întrucat mânerul este structurat astfel încat să fie ținut în mână, dar manerul poate fi adaptat pentru a fi montat pe un autovehicul, pe o drona, etc.

Descriere generală

Funcționalitatea dispozitivului este oferită de 3 servo-motoare cu un unghi de rotație de 180° (Grade), un joystick si un modul MPU-6050 care inglobează un accelerometru pe 3 axe și un giroscop pe 3 axe. Acestea sunt conectate la o placă de dezvoltare Arduino Nano, cu microcontroller ATmega328p. Placa primeste un semnal de la modulul MPU-6050 si ajusteaza servo-motoarele astfel încât platforma/camera să rămână intr-o pozitie presetată. Fiecare dintre cele 3 servo-motoare asigura stabilizarea platformei prin rotirea in jurul celor trei axe, X (Pitch), Y(Roll), Z(Yaw). Pentru a schimba orientarea camerei/platformei, se poate folosi joystick-ul, prin rotatia in jurul axelor Z, respectiv axelor X si Y. Pentru a asigura alimentarea (wireless) a dispozitivului, folosesc 2 acumulatori Li-Ion de 3.7V fiecare, legați în serie. Ansamblul ofera ~7.4V, care intra într-un convertor step-down din care ies 5V. Din convertorul step-down, se alimenteaza direct servomotoarele, Placa Arduino si modulul MPU-6050.

Hardware Design

Listă componente:
  • Arduino NANO

  • Comutator ON-OFF

  • Joystick

  • Accelerometru/Giroscop MPU-6050

  • 3 x Servomotor MG-996R

  • Suport Acumulatori 18650

  • 2 x Acumulator Li-Ion 2200mAh 18650

  • Modul coborâre tensiune LM317

  • Placă prototipare

  • Barete pini mamă/tată
  • Componente imprimate 3D
  • Suruburi/ Piulițe M3
  • Fire

Schema electrică

Software Design

- Pentru realizarea software-ului necesar funcționării proiectului, am utilizat Arduino IDE. - Printre bibliotecile standard, Arduino, folosite se numără `Wire.h` si `Servo.h`. Pentru a realiza citirea și prelucrarea datelor de la accelerometrul/giroscopul MPU-6050, am folosit bibliotecile open-source `MPU6050_6Axis_MotionApps20.h` si `I2Cdev.h'. - Valorile pentru Yaw, Pitch și Roll sunt primite de la functia `dmpGetYawPitchRoll`, din biblioteca menționată anterior, sub forma unui vector, sub forma de radiani. Ulterior, valorile sunt convertite in Grade si sunt mapate în intervalul corespunzător fiecarui servomotor. - Folosind pinii analogici, programul citeste valorile primite de la Joystick. Un pin este pentru axa x, iar celalalt pentru axa y. Daca valoarea inregistrată pe unul dintre pini se modifică, variabila specifica fiecarei axe se incrementează cu 0.5 sau se decrementează cu 0.5. Această variabilă se aduna cu variabila primită de la giroscop, iar rezultatul se da ca si argument funcției write(), care, în cazul de față, setează poziția servomotorului la un anumit număr de grade, față de poziția inițială.

Cod sursă

Cod sursă

Final.ino
#include "I2Cdev.h"
 
#include "MPU6050_6Axis_MotionApps20.h"
 
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#include <Servo.h>
MPU6050 mpu;
 
Servo servo0;
Servo servo1;
Servo servo2;
float correct;
int j = 0;
 
int joyX = 0;
int joyY = 1;
float joyVal1 = 0;
float joyVal2 = 0;
float finalVal1 = 0;
float finalVal2 = 0;
 
#define OUTPUT_READABLE_YAWPITCHROLL
 
#define INTERRUPT_PIN 2
 
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
 
  Serial.begin(38400);
  while (!Serial);
 
  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);
 
  if (devStatus == 0) {
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.PrintActiveOffsets();
    mpu.setDMPEnabled(true);
 
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
 
    dmpReady = true;
 
    packetSize = mpu.dmpGetFIFOPacketSize();
  }
 
  servo0.attach(10);
  servo1.attach(9);
  servo2.attach(8);
}
// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================
 
void loop() {
  if (!dmpReady) return;
 
  while (!mpuInterrupt && fifoCount < packetSize) {
    if (mpuInterrupt && fifoCount < packetSize) {
      fifoCount = mpu.getFIFOCount();
    }
  }
 
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();
 
  fifoCount = mpu.getFIFOCount();
 
  if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 2048) {
    // 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;
      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);
 
      if (analogRead(joyY) >= 550 && finalVal1 < 180)
      {
        joyVal1 += 0.5;
      }
      if (analogRead(joyY) <= 470 && finalVal1 > 32)
      {
        joyVal1 -= 0.5;
      }
      //Serial.println(servoVal1);
 
      if (analogRead(joyX) >= 550 && finalVal2 > 0)
      {
        joyVal2 -= 0.5;
      }
      if (analogRead(joyX) <= 470 && finalVal2 < 180)
      {
        joyVal2 += 0.5;
      }
 
      finalVal1 = servo0Value + joyVal1;
      finalVal2 = servo1Value + joyVal2;
 
      servo0.write(finalVal1);
      servo1.write(finalVal2);
      servo2.write(servo2Value);
      delay(5);
    }
#endif
  }
}

Rezultate Obţinute

Cele 3 servomotoare, giroscopul și joystick-ul lucrează, intr-un final, impreună, pentru a menține platforma dreaptă. Cu toate acestea, nu am putut scăpa de fenomenul de “YAW Drift”, care consta in schimbarea unghiului de rotatie in jurul axei OZ, puțin câte puțin, atunci când suportul stă nemișcat.

Demo funcționare

Concluzii

A fost un proiect interesant care m-a ajutat sa aprofundez noțiunile învățate la laborator. În final, proiectul nu a funcționat cum am planificat inițial, a fost mult mai costisitor (din punct de vedere financiar) decât am planificat inițial datorită unor componente necorespunzătoare pentru acest proiect, dar m-a ajutat să învăț din greșeli și să fiu mai atent la detaliile unei componente precum datasheet-uri, specificații, precum și la anumite funcții in cod.

Download

Jurnal

  • 18.04.2022: Alegerea temei
  • 27.04.2022: Comandarea pieselor
  • 09.05.2022: Prima testare a componentelor pe breadboard
  • 12.05.2022: Imprimarea carcasei si a componentelor de legatura la imprimanta 3D

  • 12.05.2022: prima montare a componentelor electrice in carcasa si ajustarea componentelor printate pentru o potrivire cât mai bună
  • 16.05.2022: Design-ul si lipirea headerelor de pini pe placa de prototipare

  • 17.05.2022: Crearea codului pornind de la template-ul oferit in biblioteca open source
  • 18.05.2022: Prima testare în suportul imprimat 3D

  • 18.05.2022: Încurcarea pinilor de VCC si GND, fapt care a dus la arderea senzorului MPU-6050
  • 18.05.2022: Realizrea ca servomotoarele cumparate anterior nu sunt potrivite pentru aceasta aplicatie si comandarea altora adecvate
  • 20.05.2022: Montarea servomotoarelor bune în carcasă și testarea proiectului cu acestea.
  • 20.05-22.05.2022: Debugging cod
  • 23.05.2022: Realizarea ca senzorul MPU-6050 este ars si comandarea unui nou senzor
  • 26.05.2022: Testare proiect cu senzorul MPU-6050 nou
  • 26.05.2022: Integrarea parțială, în cod, a funcționalității joystick-ului
  • 27.05.2022: Realizarea acestei documentații

Bibliografie/Resurse

Resurse Software:
Resurse Hardware:
pm/prj2022/rstanescu/theodor.tudose.txt · Last modified: 2022/06/02 10:49 by theodor.tudose
CC Attribution-Share Alike 3.0 Unported
www.chimeric.de Valid CSS Driven by DokuWiki do yourself a favour and use a real browser - get firefox!! Recent changes RSS feed Valid XHTML 1.0