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:
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ă.
- 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
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: