Arduino/MPU6050/AdafruitMotorShieldV2: скрипт зависает/зависает при включении двигателей

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

Я пытаюсь создать тележку, которая в основном будет бегать сама по себе (простые процедуры ИИ, чтобы избегать препятствий, переходить из точки A в точку B по углам, следовать линиям и т. д.). Я собираю Adafruit Arduino Uno R3 с Adafruit Motor Shield v2 и MPU-6050. Для схемотехники использую "макет" на Моторшилде, все туда припаиваю.

Я могу заставить все части работать независимо с их собственными сценариями: Motor Shield управляет 4 двигателями, как и ожидалось, с помощью библиотеки Adafruit; Я использую библиотеку «JRowberg» для MPU-6050 и начал с примера MPU6050_DMP6.ino, который отлично работает, пока двигатели тележки выключены. Мои единственные изменения в приведенном ниже примере сценария — это запуск двигателя и некоторые простые команды двигателя.

Пока я оставляю переключатель, который отключает двигатели, все выглядит нормально: он непрерывно выводит в окно Serial данные Эйлера, которые, как я полагаю, верны. Однако через несколько секунд после того, как я подаю питание на моторы (и колеса начинают крутиться), он просто зависает/зависает: вывод в окно Serial прекращается (иногда на средней линии), а колеса продолжают крутиться на скорость их последнего изменения. Иногда я вижу ошибки "FIFO overflow", но не всегда. Иногда я вижу «нан» для некоторых значений с плавающей запятой, прежде чем оно зависнет, но не всегда.

Некоторые вещи, которые я пробовал, все изменилось, отметив: * Я заменил плату MPU-6050 на другую от того же производителя. * Я попытался отодвинуть MPU-6050 от двигателей с помощью ленточного кабеля. * Я изменил часы I2C по совету JRowber (изменение в файле .h и изменение значения переменной TWBR), но я не думаю, что перепробовал все возможные значения. * Я изменил скорость MotorShield в команде AFMS.begin(), хотя, опять же, вероятно, есть другие значения, которые я не пробовал, и я не уверен, насколько синхронизированы это и значение TWBR. быть.

И некоторые другие вещи, все безрезультатно.

Ниже приведен пример сценария, который не работает для меня:

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif
#include "Adafruit_MotorShield.h"
#include "utility/Adafruit_PWMServoDriver.h"

#define DEBUG 1

MPU6050 mpu;
#define OUTPUT_READABLE_EULER

#define LED_PIN 13
bool blinkState = false;

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

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

uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
#define NUM_MOTORS 4
#define MOTOR_FL 0
#define MOTOR_FR 1
#define MOTOR_RR 2
#define MOTOR_RL 3
Adafruit_DCMotor *myMotors[NUM_MOTORS] =  {
  AFMS.getMotor(1),
  AFMS.getMotor(2),
  AFMS.getMotor(3),
  AFMS.getMotor(4),
};

#define CHANGE_SPEED_TIME 500
long changeSpeedMillis = 0;
int curSpeed = 30;

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}

void setup() {
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    Serial.begin(115200);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // start the motor shield.
      AFMS.begin();  // create with the default frequency 1.6KHz
//    AFMS.begin(4000);  // OR with a different frequency, say 4KHz
    // kill all the motors.
    myMotors[MOTOR_FL]->run(BRAKE);
    myMotors[MOTOR_FL]->setSpeed(0);  
    myMotors[MOTOR_FR]->run(BRAKE);
    myMotors[MOTOR_FR]->setSpeed(0);  
    myMotors[MOTOR_RR]->run(BRAKE);
    myMotors[MOTOR_RR]->setSpeed(0);  
    myMotors[MOTOR_RL]->run(BRAKE);
    myMotors[MOTOR_RL]->setSpeed(0);
    Serial.println("Motor Shield ready!");

    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") :     F("MPU6050 connection failed"));

    // wait for ready
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}

void loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
      // as per Vulpo's post.
      delay(10);
      if (millis() > changeSpeedMillis) {
            curSpeed += 20;
            if (curSpeed > 256) {
              curSpeed = 30;
            }
            Serial.print("changing speed to: ");
            Serial.println(curSpeed);
            myMotors[MOTOR_FL]->run(FORWARD);
            myMotors[MOTOR_FL]->setSpeed(curSpeed);
            myMotors[MOTOR_FR]->run(FORWARD);
            myMotors[MOTOR_FR]->setSpeed(curSpeed);
            myMotors[MOTOR_RR]->run(FORWARD);
            myMotors[MOTOR_RR]->setSpeed(curSpeed);
            myMotors[MOTOR_RL]->run(FORWARD);
            myMotors[MOTOR_RL]->setSpeed(curSpeed);

            changeSpeedMillis = millis() + CHANGE_SPEED_TIME;
      }
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // 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;

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        // blink LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
    }
}