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

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 сервомоторы, не годятся для подобных устройств. Более правильным будет применение бесколлекторных моторов, которые вы можете найти в любом заводском гироподвесе.


Изменено:

2D-гироподвес на основе Ардуино, MPU6050 и SG90: 19 комментариев

  1. Здравствуйте, постоянно выбивает ошибку expected ‘;’ before numeric constant с выделением строчки define LPF 0.7.
    ЧТо может быть?

    • В коде была опечатка: перед define нужно ставить символ #. Исправлено!

        • Ошибка осталась, предполагаю что может быть ошибка из-за разных библиотек под модуль mpu6050, если можно — напишите мне пожалуйста на почту — oxxyser @ gmail.com для нормальной связи, ибо тут сообщения очень долго проверяются

  2. Добрый день, с Вами как-то связаться можно? ПРоблема чувствую из-за разных библиотек под модуль MPU 6050

    • Похоже на скриншоте у вас не вся ошибка целиком. Скопируйте всё, что выводится в нижнем окошке.

  3. Не подскажете, как решили эту ошибку expected ‘;’ before numeric constant с выделением строчки define LPF 0.7.?

    • в этих строках кода я дописал умножения
      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);
      проблема решилось

    • 1) Обязательно пишите текст ошибки. Непонятно же, на что у вас ругается компилятор.
      2) В программе пропали символы произведения, как раз в этом месте. Исправил, попробуете снова скопировать код!

  4. /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino: In function ‘void loop()’:
    /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino:63:5: error: ‘MadgwickAHRSupdateIMU’ was not declared in this scope
    MadgwickAHRSupdateIMU(tdelta/1000.0, gx, gy, gz, (float)ax, (float)ay, (float)az);
    ^~~~~~~~~~~~~~~~~~~~~
    /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino:63:5: note: suggested alternative: ‘MadgwickAHRS_h’
    MadgwickAHRSupdateIMU(tdelta/1000.0, gx, gy, gz, (float)ax, (float)ay, (float)az);
    ^~~~~~~~~~~~~~~~~~~~~
    MadgwickAHRS_h
    /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino:64:15: error: ‘q0’ was not declared in this scope
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    ^~
    /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino:64:15: note: suggested alternative: ‘A0’
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    ^~
    A0
    /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino:64:29: error: ‘q1’ was not declared in this scope
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    ^~
    /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino:64:29: note: suggested alternative: ‘A1’
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    ^~
    A1
    /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino:64:43: error: ‘q2’ was not declared in this scope
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    ^~
    /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino:64:43: note: suggested alternative: ‘A2’
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    ^~
    A2
    /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino:64:57: error: ‘q3’ was not declared in this scope
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    ^~
    /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino:64:57: note: suggested alternative: ‘A3’
    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    ^~
    A3
    /Users/elenaandreeva/Downloads/SERVO_w_GYRO_2/SERVO_w_GYRO_2.ino:66:5: error: ‘quat2Euler’ was not declared in this scope
    quat2Euler(&quat[0], &imu[0]);
    ^~~~~~~~~~
    Несколько библиотек найдено для «Servo.h»
    Используется: /Users/elenaandreeva/Documents/Arduino/libraries/Servo
    Не используется: /Users/elenaandreeva/Library/Arduino15/libraries/Servo
    exit status 1

    Compilation error: ‘MadgwickAHRSupdateIMU’ was not declared in this scope
    кое что я исправил сам а это осталось

  5. Да была проблема в библиотеке в самой программе ардуино скачал через интернет прошивка заработала. Но почему-то пишет ошибку по гироскопу в мониторинге порта(невидит гироскоп) Все подключено верно менял по разному и A5 A4 skl sda.

    • Лучше загрузить на контроллера программу — сканер I2C (она есть в штатных примерах в Arduino IDE) и добиться чтобы сканер находил гироскоп в принципе. А потом уже двигаться дальше.
      Может гироскоп неисправный, или у вас подтяжки нет на линии I2C.

  6. Подтяжка по I2c есть и гироскоп рабочий я проверял просто это программа и схема будет использоваться не в стабилизаторе. Я просто еще не работал с гироскопом и для него программу не писал ,в интернете не по тем осям :(. (нужно по осям Z X)
    Можете написать на почту а то здесь долгая проверка amakss378@gmail.com

Добавить комментарий

Ваш адрес email не будет опубликован. Обязательные поля помечены *

Этот сайт использует Akismet для борьбы со спамом. Узнайте, как обрабатываются ваши данные комментариев.