Шаговый двигатель работает медленно при чтении с датчика MPU 6050

Question

Шаговый двигатель работает медленно при чтении с датчика MPU 6050

Я пытаюсь заставить работать проект перевернутого маятника (следуя инструкциям здесь), и в то время как мой шаговый двигатель прекрасно двигает тележку, когда это единственный код в скетче Arduino, как только я добавляю датчик MPU 6050 для определения угла маятника, тележка движется намного медленнее, слишком медленно быть полезным для удержания маятника в вертикальном положении.

Я предполагаю, что синхронизация команд двигателя искажается показаниями датчика, но я не знаю, как избежать этой проблемы. Кроме того, автор примера опубликовал практически тот же код, что и его рабочее решение для пропорционального управления, так что может быть что-то еще, что я упустил, что портит производительность моего двигателя.

Есть ли у кого-нибудь какие-либо предложения о том, как заставить шаговый двигатель вращаться достаточно быстро, одновременно считывая данные с MPU 6050?

Мой код ниже. Я использую шаговый двигатель Nema 17 с драйвером A4988, блок питания 12 В 10 А и этот MPU Aukru. -6050. Чтобы быть уверенным, что обновления скорости и серийной печати не являются проблемой, я использую интервал 500 мс между обновлениями/печатью.

#include "I2Cdev.h"
#include <AccelStepper.h>
#include "MPU6050_6Axis_MotionApps20.h"

// Библиотека Arduino Wire требуется, если реализация I2Cdev I2CDEV_ARDUINO_WIRE
// используется в I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu;

#define INTERRUPT_PIN 2  // использовать контакт 2 на Arduino Uno & большинство плат

// переменные управления/состояния MPU
bool dmpReady = false;  // устанавливаем true, если инициализация DMP прошла успешно
uint8_t mpuIntStatus;   // содержит фактический байт состояния прерывания от MPU
uint8_t devStatus;      // возвращаем статус после каждой операции с устройством (0 = успех, !0 = ошибка)
uint16_t packetSize;    // ожидаемый размер пакета DMP (по умолчанию 42 байта)
uint16_t fifoCount;     // подсчет всех байтов в настоящее время в FIFO
uint8_t fifoBuffer[64]; // Буфер хранения FIFO

// переменные ориентации/движения
Quaternion q;           // [w, x, y, z] контейнер кватернионов
VectorInt16 aa;         // [x, y, z] измерения датчика ускорения
VectorInt16 aaReal;     // [x, y, z] измерения датчика ускорения без гравитации
VectorInt16 aaWorld;    // [x, y, z] измерения датчика ускорения мирового кадра
VectorFloat gravity;    // [x, y, z] вектор гравитации
float euler[3];         // [пси, тета, фи] Контейнер угла Эйлера
float ypr[3];           // [рыскание, тангаж, крен] контейнер рыскания/тангажа/крена и вектор гравитации

// =============================================== ================
// === ПРОЦЕДУРА ОБНАРУЖЕНИЯ ПРЕРЫВАНИЯ ===
// =============================================== ================

volatile bool mpuInterrupt = false;     // указывает, перешел ли вывод прерывания MPU в высокий уровень
void dmpDataReady() {
    mpuInterrupt = true;
}

// Определяем номера контактов
const int stepPin = 3;
const int dirPin = 4;
AccelStepper stepper(1,stepPin,dirPin);

int motorSpeed, currentPos;
unsigned long t_start, t_elapsed;
int interval = 500; // мс между скоростью печати

int speedMax = 4000;
float k_proportional = 4;

float angleCurrent, speedSet;
 
void setup() {
  // подключаемся к шине I2C (библиотека I2Cdev не делает этого автоматически)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000); // Часы I2C 400 кГц. Прокомментируйте эту строку, если у вас возникли трудности с компиляцией
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif
    
  Serial.begin(115200);
  
  // инициализируем устройство
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  // проверяем соединение
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // загрузить и настроить DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // укажите здесь собственные смещения гироскопа, масштабированные для минимальной чувствительности
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 по умолчанию для моего тестового чипа

  // убедиться, что это сработало (возвращает 0, если это так)
  if (devStatus == 0) {
      // включаем DMP, теперь, когда он готов
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // включить обнаружение прерываний Arduino
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
      mpuIntStatus = mpu.getIntStatus();

      // устанавливаем наш флаг готовности DMP, чтобы основная функция loop() знала, что ее можно использовать
      Serial.println(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // получаем ожидаемый размер пакета DMP для последующего сравнения
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      // ОШИБКА!
      // 1 = первоначальная загрузка памяти не удалась
      // 2 = не удалось обновить конфигурацию DMP
      // (если он сломается, обычно код будет 1)
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }  
  
  stepper.setMaxSpeed(speedMax);
  stepper.setAcceleration(10000);
  stepper.setSpeed(2000);
}

void loop() {
    // если программирование не удалось, не пытайтесь ничего сделать
    if (!dmpReady) return;

    // ждем прерывания MPU или доступных дополнительных пакетов
    while (!mpuInterrupt && fifoCount < packetSize) {
        if (mpuInterrupt && fifoCount < packetSize) {
          // пытаемся выйти из бесконечного цикла
          fifoCount = mpu.getFIFOCount();
        }
    }
  
    // сброс флага прерывания и получение байта INT_STATUS
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // получаем текущий счетчик FIFO
    fifoCount = mpu.getFIFOCount();

    // проверка на переполнение (это никогда не должно происходить, если только наш код не слишком неэффективен)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // сбрасываем, чтобы мы могли продолжить работу без ошибок
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // в противном случае проверьте прерывание готовности данных DMP (это должно происходить часто)
    } else if (mpuIntStatus & 0x02) {
      // ожидание правильной доступной длины данных, должно быть ОЧЕНЬ короткое ожидание
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

      // читаем пакет из FIFO
      mpu.getFIFOBytes(fifoBuffer, packetSize);
      
      // отслеживаем количество FIFO здесь, если есть > 1 пакет в наличии
      // (это позволяет нам сразу читать больше, не дожидаясь прерывания)
      fifoCount -= packetSize;

      // отображаем углы Эйлера в градусах
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    }
    
  t_elapsed = millis() - t_start;
  
  if (t_elapsed >= interval) {
    t_start += interval;
    motorSpeed=setMotorSpeed();
    stepper.setSpeed(motorSpeed);
    Serial.print("Roll ");
    Serial.print(ypr[2] * 180/M_PI);
    Serial.print("Motor speed: ");
    Serial.println(motorSpeed);
  }  
 
  stepper.runSpeed();
}

int setMotorSpeed() {
  // Пропорциональное управление
  angleCurrent = ypr[2];
  speedSet = constrain(-angleCurrent*k_proportional*speedMax, -speedMax, speedMax);
  return speedSet;
}

Редактировать: добавлен мой код, который ограничивает логику датчика тем же интервалом, что и обновление скорости, на основе ответа chrisl. Тележка движется немного быстрее, но все же недостаточно быстро, чтобы удерживать маятник в вертикальном положении.

#include "I2Cdev.h"
#include <AccelStepper.h>
#include "MPU6050_6Axis_MotionApps20.h"

// Библиотека Arduino Wire требуется, если реализация I2Cdev I2CDEV_ARDUINO_WIRE
// используется в I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu;

#define INTERRUPT_PIN 2  // использовать контакт 2 на Arduino Uno & большинство плат

// переменные управления/состояния MPU
bool dmpReady = false;  // устанавливаем true, если инициализация DMP прошла успешно
uint8_t mpuIntStatus;   // содержит фактический байт состояния прерывания от MPU
uint8_t devStatus;      // возвращаем статус после каждой операции с устройством (0 = успех, !0 = ошибка)
uint16_t packetSize;    // ожидаемый размер пакета DMP (по умолчанию 42 байта)
uint16_t fifoCount;     // подсчет всех байтов в настоящее время в FIFO
uint8_t fifoBuffer[64]; // Буфер хранения FIFO

// переменные ориентации/движения
Quaternion q;           // [w, x, y, z] контейнер кватернионов
VectorInt16 aa;         // [x, y, z] измерения датчика ускорения
VectorInt16 aaReal;     // [x, y, z] измерения датчика ускорения без гравитации
VectorInt16 aaWorld;    // [x, y, z] измерения датчика ускорения мирового кадра
VectorFloat gravity;    // [x, y, z] вектор гравитации
float euler[3];         // [пси, тета, фи] Контейнер угла Эйлера
float ypr[3];           // [рыскание, тангаж, крен] контейнер рыскания/тангажа/крена и вектор гравитации

// =============================================== ================
// === ПРОЦЕДУРА ОБНАРУЖЕНИЯ ПРЕРЫВАНИЯ ===
// =============================================== ================

volatile bool mpuInterrupt = false;     // указывает, перешел ли вывод прерывания MPU в высокий уровень
void dmpDataReady() {
    mpuInterrupt = true;
}

// Определяем номера контактов
const int stepPin = 3;
const int dirPin = 4;
AccelStepper stepper(1,stepPin,dirPin);

int motorSpeed, currentPos;
unsigned long t_start, t_elapsed, t_mpu, t_start_mpu;
int interval = 50; // мс между скоростью печати
unsigned long count = 0;

int speedMax = 800;
float k_proportional = 3;

float angleCurrent, speedSet;
 
void setup() {
  // подключаемся к шине I2C (библиотека I2Cdev не делает этого автоматически)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
      Wire.setClock(400000); // Часы I2C 400 кГц. Прокомментируйте эту строку, если у вас возникли трудности с компиляцией
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif
    
  Serial.begin(115200);
  
  // инициализируем устройство
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  // проверяем соединение
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // загрузить и настроить DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // укажите здесь собственные смещения гироскопа, масштабированные для минимальной чувствительности
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 по умолчанию для моего тестового чипа

  // убедиться, что это сработало (возвращает 0, если это так)
  if (devStatus == 0) {
      // включаем DMP, теперь, когда он готов
      Serial.println(F("Enabling DMP..."));
      mpu.setDMPEnabled(true);

      // включить обнаружение прерываний Arduino
      Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
      attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
      mpuIntStatus = mpu.getIntStatus();

      // устанавливаем наш флаг готовности DMP, чтобы основная функция loop() знала, что ее можно использовать
      Serial.println(F("DMP ready! Waiting for first interrupt..."));
      dmpReady = true;

      // получаем ожидаемый размер пакета DMP для последующего сравнения
      packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      // ОШИБКА!
      // 1 = первоначальная загрузка памяти не удалась
      // 2 = не удалось обновить конфигурацию DMP
      // (если он сломается, обычно код будет 1)
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }  
  
  stepper.setMaxSpeed(speedMax);
  stepper.setAcceleration(10000);
  stepper.setSpeed(100);
}

void loop() {
    
  t_elapsed = millis() - t_start;
  count += 1;
  
  if (t_elapsed >= interval) {
    t_start += interval;
  
    // ждем прерывания MPU или доступных дополнительных пакетов
    // Сильно замедляет работу корзины, но без этого код зависает
    while (!mpuInterrupt && fifoCount < packetSize) {
        if (mpuInterrupt && fifoCount < packetSize) {
          // пытаемся выйти из бесконечного цикла
          fifoCount = mpu.getFIFOCount();
        }  
    }    
  
    // сброс флага прерывания и получение байта INT_STATUS
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // получаем текущий счетчик FIFO
    fifoCount = mpu.getFIFOCount();

    // проверка на переполнение (это никогда не должно происходить, если только наш код не слишком неэффективен)
    
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // сбрасываем, чтобы мы могли продолжить работу без ошибок
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // в противном случае проверьте прерывание готовности данных DMP (это должно происходить часто)
    } else if (mpuIntStatus & 0x02) {
      // ожидание правильной доступной длины данных, должно быть ОЧЕНЬ короткое ожидание
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
      
      // читаем пакет из FIFO
      mpu.getFIFOBytes(fifoBuffer, packetSize);
      
      // отслеживаем количество FIFO здесь, если есть > 1 пакет в наличии
      // (это позволяет нам сразу читать больше, не дожидаясь прерывания)
      fifoCount -= packetSize;

      // отображаем углы Эйлера в градусах
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);      
    }  
  
    motorSpeed=setMotorSpeed();
    // Используйте один потенциометр для включения или выключения системы (A0)
    int A0val = analogRead(A0);
    // Используйте другой потенциометр для настройки времени отклика (или максимальной скорости)
    int A1val = analogRead(A1);
    //максимальная скорость = 2*A1val;
    interval = A1val/2;
    Serial.print("Roll: ");
    Serial.print(ypr[2] * 180/M_PI);
    Serial.print(", motor speed: ");
    Serial.print(motorSpeed);
    Serial.print(", A0: ");
    Serial.print(A0val);
    Serial.print(", interval: ");
    Serial.print(interval);
    //Serial.print(", speedMax: ");
    //Серийный.принт(скоростьМакс);
    Serial.print(", iterations: ");
    Serial.println(count);
    count=0;
    if (A0val > 512)
      stepper.setSpeed(motorSpeed);
    else
      stepper.setSpeed(0);
  }
 
  stepper.runSpeed();
}

int setMotorSpeed() {
  // Пропорциональное управление
  angleCurrent = ypr[2];
  speedSet = constrain(-angleCurrent*k_proportional*speedMax, -speedMax, speedMax);
  return speedSet;
}

@Frecka, 👍1

Обсуждение

как насчет того, тебе удалось завершить проект?, @Angel Hidalgo

1 ответ

Лучший ответ:

@*chrisl* · Accepted Answer · 2020-10-12 10:28-00:00

Я не могу сказать, почему этот принцип работает у автора этого туториала. Хотя я могу догадаться, почему ваши моторы работают так медленно.

Функция, которая фактически приводит в движение двигатель, называется stepper.runSpeed(). Вы вызываете его ровно один раз в конце функции loop(). Хотя эта функция предназначена для очень частого вызова, так как она только проверяет, не пора ли сделать шаг, и затем делает это. Это просто сделать максимум 1 шаг за выполнение. Таким образом, вы получаете 1 шаг за итерацию loop().

И в начале loop() вы ожидаете готовности данных MPU с помощью этой строки:

while (!mpuInterrupt && fifoCount < packetSize)

Таким образом, вы выполняете шаги на максимальной скорости с той же скоростью, с которой вы читаете MPU. Кажется, этого недостаточно для вашей установки.

Вы можете попробовать чаще вызывать функцию runSpeed() (и тем самым снять ограничение на количество шагов). Либо перепишите свой код так, чтобы он выполнял только связь с MPU, если данные MPU готовы, и в противном случае переходите к другому коду. Или вы можете вставить функцию runSpeed() в циклы while, ожидающие данных MPU. Первый вариант был бы чище и облегчил бы расширение кода.