Программа и тест

2D-гироподвес на основе Ардуино, MPU6050 и SG90

Олег Евсегнеев

Как уже было написано выше, алгоритм работы гироподвеса должен выглядеть следующим образом:

  1. получаем данные с датчиков положения
  2. вычисляем на какой угол нужно повернуть сервомоторы
  3. поворачиваем сервомоторы
  4. переходим к пункту 1

Исходный код программы:

include "I2Cdev.h"

include "MPU6050.h"

include "MadgwickAHRS.h"

include <Servo.h>



#define TO_RAD 0.01745329252f

#define PITCH_DIR -1 // 1 - направление сервомотора и датчика совпадают, -1 - нет

#define PITCH_DEFAULT -90 // начальный угол датчика

#define ROLL_DIR -1

#define ROLL_DEFAULT 0

#define PITCH 0

#define ROLL 1

#define LPF 0.7



MPU6050 accelgyro;



Servo servos[2]; // тангаж (pitch), вращение (roll)

int angle[2] = {0,0}; // pitch, roll



unsigned long tm, imu_t, stab_t;

const unsigned int imu_to = 20; // каждые 20 мс опрашиваем датчик

const unsigned int stab_to = 50; // каждые 50 мс управляем сервомоторами

const unsigned int calib_to = 1000; // 1 секунду после старта калибруемся



float tdelta;

int16_t ax, ay, az;

int16_t gx_raw, gy_raw, gz_raw;

float imu[3];

float imu_f[3];

float quat[4];

bool calibrated = 0;



void setup() {

  Serial.begin(9600);

  Serial.println("Initializing I2C devices…");

  accelgyro.initialize(); // инициализация датчиков

  Serial.println("Testing device connections…");

  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful!" : "MPU6050 connection failed");

  servos[0].attach(2);

  servos[1].attach(3);

}



int clamp (int a, int mn, int mx){

  if( a>mx ) return mx;

  if( a<mn ) return mn;

  return a;

}



void loop() {

  // каждые 20мс вычисляем углы наклона

  tm = millis();

  if (imu_t + imu_to < tm) {

    tdelta = tm - imu_t; // вычисляем дельту времени в миллисекундах

    imu_t = tm;

    accelgyro.getMotion6(&ax, &ay, &az, &gx_raw, &gy_raw, &gz_raw);



    // преобразуем сырые данные гироскопа в рад/с

    float gx = gx_raw * TO_RAD / 131.0;

    float gy = gy_raw * TO_RAD / 131.0;

    float gz = gz_raw * TO_RAD / 131.0;



    // вызываем алгоритм фильтра Мажвика

    MadgwickAHRSupdateIMU(tdelta/1000.0, gx, gy, gz, (float)ax, (float)ay, (float)az);

    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;

    // преобразуем кватернион в углы Эйлера

    quat2Euler(&quat[0], &imu[0]);

    // применяем фильтр низких частот

    imu_f[0] = imu_f[0]*LPF + imu[0]*(1-LPF);

    imu_f[1] = imu_f[1]*LPF + imu[1]*(1-LPF);

    imu_f[2] = imu_f[2]*LPF + imu[2]*(1-LPF);

  }



  // первые 1000мс тратим на калибровку

  tm = millis();

  if (!calibrated && (calib_to < tm)) {

    calibrated = 1;

    imu_f[0] = imu[0];

    imu_f[1] = imu[1];

    imu_f[2] = imu[2];

  }



  // каждые 50мс управляем сервомоторами

  tm = millis();

  if (calibrated && (stab_t + stab_to < tm)) {

    stab_t = tm;

    float a = 0;

    // PITCH

    a = imu_f[0]/TO_RAD + PITCH_DEFAULT;

    if( abs(a)>2 ){

      angle[PITCH] = clamp(angle[PITCH] + a*PITCH_DIR, 0, 180);

      servos[PITCH].write(angle[PITCH]);

    }

    // ROLL

    a = imu_f[1]/TO_RAD + ROLL_DEFAULT;

    if( abs(a)>2 ){

      angle[ROLL] = clamp(angle[ROLL] + a*ROLL_DIR, 0, 180);

      servos[ROLL].write(angle[ROLL]);

    } 

  }

}

Загружаем программу на Ардуино и пробуем шевелить гироподвес за основное крепление.

Надо заметить, что текущая программа — лишь набросок, который следует серьезно доработать, прежде чем применить в настоящем стэдикаме. Более того, моторы SG90, да и вообще любые популярные в среде DIY сервомоторы, не годятся для подобных устройств. Более правильным будет применение бесколлекторных моторов, которые вы можете найти в любом заводском гироподвесе.


Изменено: