MPU6050 и Arduino, управление светодиодами

Тема в разделе "Arduino & Shields", создана пользователем WinterBreeze, 25 авг 2019.

  1. WinterBreeze

    WinterBreeze Нерд

    Всем привет!
    Прошу помощи с кодом.

    Нашёл на сайте код для управления светодиодами при помощи GY-521.
    Вычленил из полного кода кусок для управления только четырьмя диодами (у автора реализована возможность переключения на управление большей группой диодов).
    Получилось так:
    Код (C++):

    const int frontLed = 3;
    const int bottomLed = 5;
    const int rightLed = 6;
    const int leftLed = 9;
    long int lastPrintTime;

    #include "I2Cdev.h"
    #include "MPU6050_6Axis_MotionApps20.h"
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
    #endif

    MPU6050 mpu;

    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
    volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

    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(9600);
        while (!Serial); // wait for Leonardo enumeration, others continue immediately
        Serial.println(F("Initializing I2C devices..."));
        mpu.initialize();
        Serial.println(F("Testing device connections..."));
        Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
        Serial.println(F("Initializing DMP..."));
        devStatus = mpu.dmpInitialize();
        mpu.setXGyroOffset(220);
        mpu.setYGyroOffset(76);
        mpu.setZGyroOffset(-85);
        mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
        if (devStatus == 0) {
            // turn on the DMP, now that it's ready
            Serial.println(F("Enabling DMP..."));
            mpu.setDMPEnabled(true);
            Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
            attachInterrupt(0, dmpDataReady, RISING);
            mpuIntStatus = mpu.getIntStatus();
            Serial.println(F("DMP ready! Waiting for first interrupt..."));
            dmpReady = true;
            packetSize = mpu.dmpGetFIFOPacketSize();
        } else {
            Serial.print(F("DMP Initialization failed (code "));
            Serial.print(devStatus);
            Serial.println(F(")"));
        }
        initializeLEDsSimple();
        lastPrintTime = millis();  
    }

    void loop()
    {
        if (!dmpReady) return;
        mpuInterrupt = false;
        mpuIntStatus = mpu.getIntStatus();
        fifoCount = mpu.getFIFOCount();
        if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
            mpu.resetFIFO();
            Serial.println(F("FIFO overflow!"));
        } else if (mpuIntStatus & 0x02) {
            while (fifoCount < packetSize) {
              fifoCount = mpu.getFIFOCount();
            }
            mpu.getFIFOBytes(fifoBuffer, packetSize);      
            fifoCount -= packetSize;
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            int x = ypr[0] * 180/M_PI;
            int y = ypr[1] * 180/M_PI;
            int z = ypr[2] * 180/M_PI;
            Serial.print(y);Serial.print("\t");Serial.println(z);  
            flashLEDsSimple(x, y, z);
            }
    }

    void initializeLEDsSimple()
    {
        pinMode(frontLed, OUTPUT);
        pinMode(bottomLed, OUTPUT);  
        pinMode(rightLed, OUTPUT);
        pinMode(leftLed, OUTPUT);
    }

    void flashLEDsSimple(int x, int y, int z)
    {
        if (y > 0) {
            analogWrite(rightLed, y*4);
            analogWrite(leftLed, 0);          
        } else {
            analogWrite(leftLed, y*4*-1);    
            analogWrite(rightLed, 0);    
        }
        if (z > 0) {
            analogWrite(bottomLed, z*4);
            analogWrite(frontLed, 0);        
        } else {
            analogWrite(frontLed, z*4*-1);    
            analogWrite(bottomLed, 0);    
        }    
    }

    void dmpDataReady()
    {
        mpuInterrupt = true;
    }
    Всё работает (но если осталось ещё что-то лишнее, подскажите - вырежу).

    Моя же задача сводится к тому, чтобы датчик управлял диодами относительно положения, в котором он находился при включении, а не относительно горизонта. Ну и направлять на светодиоды не аналоговый сигнал, а цифровой.

    Попробовал собрать такой вариант:
    Код (C++):
    const int frontLed = 3;
    const int bottomLed = 5;
    const int rightLed = 6;
    const int leftLed = 9;
    int x1;
    int y1;
    int z1;
    long int lastPrintTime;

    #include "I2Cdev.h"
    #include "MPU6050_6Axis_MotionApps20.h"
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
    #endif

    MPU6050 mpu;

    bool dmpReady = false;
    uint8_t mpuIntStatus;
    uint8_t devStatus;
    uint16_t packetSize;
    uint16_t fifoCount;
    uint8_t fifoBuffer[64];

    // orientation/motion vars
    Quaternion q;
    VectorInt16 aa;
    VectorInt16 aaReal;
    VectorInt16 aaWorld;
    VectorFloat gravity;
    float euler[3];
    float ypr[3];
    volatile bool mpuInterrupt = false;

    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(9600);
        while (!Serial);
        Serial.println(F("Initializing I2C devices..."));
        mpu.initialize();
        Serial.println(F("Testing device connections..."));
        Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
        Serial.println(F("Initializing DMP..."));
        devStatus = mpu.dmpInitialize();
        mpu.setXGyroOffset(220);
        mpu.setYGyroOffset(76);
        mpu.setZGyroOffset(-85);
        mpu.setZAccelOffset(1788);
        if (devStatus == 0) {
            // turn on the DMP, now that it's ready
            Serial.println(F("Enabling DMP..."));
            mpu.setDMPEnabled(true);
            Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
            attachInterrupt(0, dmpDataReady, RISING);
            mpuIntStatus = mpu.getIntStatus();
            Serial.println(F("DMP ready! Waiting for first interrupt..."));
            dmpReady = true;
            packetSize = mpu.dmpGetFIFOPacketSize();
        } else {
            Serial.print(F("DMP Initialization failed (code "));
            Serial.print(devStatus);
            Serial.println(F(")"));
        }
        initializeLEDsSimple();
        lastPrintTime = millis();


        if (!dmpReady) return;
        mpuInterrupt = false;
        mpuIntStatus = mpu.getIntStatus();
        fifoCount = mpu.getFIFOCount();
        if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
            mpu.resetFIFO();
            Serial.println(F("FIFO overflow!"));
        } else if (mpuIntStatus & 0x02) {
            while (fifoCount < packetSize) {
              fifoCount = mpu.getFIFOCount();
            }
            mpu.getFIFOBytes(fifoBuffer, packetSize);      
            fifoCount -= packetSize;
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            x1 = ypr[0] * 180/M_PI;
            y1 = ypr[1] * 180/M_PI;
            z1 = ypr[2] * 180/M_PI;
            Serial.print(y1);Serial.print("\t");Serial.println(z1);
            }
    }

    void loop()
    {
        if (!dmpReady) return;
        mpuInterrupt = false;
        mpuIntStatus = mpu.getIntStatus();
        fifoCount = mpu.getFIFOCount();
        if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
            mpu.resetFIFO();
            Serial.println(F("FIFO overflow!"));
        } else if (mpuIntStatus & 0x02) {
            while (fifoCount < packetSize) {
              fifoCount = mpu.getFIFOCount();
            }
            mpu.getFIFOBytes(fifoBuffer, packetSize);      
            fifoCount -= packetSize;
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            int x = ypr[0] * 180/M_PI;
            int y = ypr[1] * 180/M_PI;
            int z = ypr[2] * 180/M_PI;
            Serial.print(y);Serial.print("\t");Serial.println(z);  
            flashLEDsSimple(x, y, z, x1, y1, z1);
            }
    }

    void initializeLEDsSimple()
    {
        pinMode(frontLed, OUTPUT);
        pinMode(bottomLed, OUTPUT);  
        pinMode(rightLed, OUTPUT);
        pinMode(leftLed, OUTPUT);
    }

    void flashLEDsSimple(int x, int y, int z, int x1, int y1, int z1)
    {
        if (y1 > y) {
            digitalWrite(rightLed, 1);
            digitalWrite(leftLed, 0);          
        } else {
            digitalWrite(leftLed, 1);    
            digitalWrite(rightLed, 0);    
        }
        if (z1 > z) {
            digitalWrite(bottomLed, 1);
            digitalWrite(frontLed, 0);        
        } else {
            digitalWrite(frontLed, 1);    
            digitalWrite(bottomLed, 0);    
        }    
    }

    void dmpDataReady()
    {
        mpuInterrupt = true;
    }
    Помогите, пожалуйста.
    Заранее спасибо!
     
  2. NikitOS

    NikitOS Король шутов Администратор

    Записывайте положение при старте, а потом сравнивайте с текущим.
    X больше начального - зажечь светодиод 1, меньше - 2
    С Y то же самое
     
    Asper Daffy нравится это.
  3. WinterBreeze

    WinterBreeze Нерд

    Спасибо за отзывчивость!
    Дело в том, что я и работаю в этом направлении.
    Внёс переменные x1, y1 и z1. Сравниваю уже с ними, но только результат никакой.
    Посмотрите, пожалуйста, может где-то явный косяк?
     
  4. NikitOS

    NikitOS Король шутов Администратор

    У вас слишком сложный код, я его не понимаю XD
    Проще библиотеки не нашли?
     
  5. WinterBreeze

    WinterBreeze Нерд

    Да проблема в том, что с нуля я могу написать код не многим сложнее мигалки.
    Поэтому переписываю уже готовые коды, которые выполняют наиболее близкий к моим требованиям алгоритм.
    В исходном коде y и z сопоставляются с нулём.
    Я скопировал кусок кода из loop в setup для записи стартовых значений в переменные y1 и z1. И в loop уже сопоставляю получаемые переменные с ними.
    Вроде как всё должно работать, но нет)
     
  6. parovoZZ

    parovoZZ Гуру

    Для этого надо снабдить код комментариями, чтобы была понятна логика/алгоритм. Уважайте собеседника.
     
    NikitOS нравится это.
  7. WinterBreeze

    WinterBreeze Нерд

    С радостью бы это сделал, если писал код с нуля или был бы в состоянии полностью в нём разобраться.
    А так могу только сказать, что этот кусок:
    Код (C++):
    if (!dmpReady) return;
        mpuInterrupt = false;
        mpuIntStatus = mpu.getIntStatus();
        fifoCount = mpu.getFIFOCount();
        if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
            mpu.resetFIFO();
            Serial.println(F("FIFO overflow!"));
        } else if (mpuIntStatus & 0x02) {
            while (fifoCount < packetSize) {
              fifoCount = mpu.getFIFOCount();
            }
            mpu.getFIFOBytes(fifoBuffer, packetSize);    
            fifoCount -= packetSize;
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            int x = ypr[0] * 180/M_PI;
            int y = ypr[1] * 180/M_PI;
            int z = ypr[2] * 180/M_PI;
            Serial.print(y);Serial.print("\t");Serial.println(z);
    Скопировал из loop в setup.
    Поменял в нём x, y, z на x1, y1 и z1 соответственно.
    В начале кода добавил строчки:
    int x1;
    int y1;
    int z1;

    Строки
    flashLEDsSimple(x, y, z);
    void flashLEDsSimple(int x, int y, int z)
    Дополнил новыми значениями
    flashLEDsSimple(x, y, z, x1, y1, z1);
    void flashLEDsSimple(int x, int y, int z, int x1, int y1, int z1)

    Сопоставления
    if (y > 0)
    if (z > 0)
    заменил на
    if (y1 > y)
    if (z1 > z)

    Это, наверное, максимум как я могу прокомментировать.
     
  8. parovoZZ

    parovoZZ Гуру

    Значит, надо начинать с простого и дальше, как по ступенькам, наращивать сложность. Хочешь мигать светодиодами? Научись мигать светодиодами. Хочешь считывать информацию с датчика? Научись его инициализировать и работать с ним. Потом, когда вся информация будет разложена в голове по полочкам, можно приступить к совмещению разных сущностей. Поверь мне - за тебя этим никто заниматься не будет. Разве что за деньги.
     
    NikitOS нравится это.
  9. Daniil

    Daniil Гуру

    А что не работает, какая отладочная инфа, нам гадать?
    А зачем передавать глобальную переменную как аргумент ф-ии? Не проверял как это работает, но любопытно - глобальная переменная и аргумент ф-ии имеют одинаковое имя - кто победит?
     
  10. WinterBreeze

    WinterBreeze Нерд

    Ну, не каждый день вопросы на форум пишу, уж простите.
    Затрудняюсь точно сказать, что именно не работает, но диоды включаются точно не относительно стартового положения. Похоже, что они по-прежнему работают относительно нулевых координат.
    Про глобальную функцию поясните пожалуйста.
     
  11. WinterBreeze

    WinterBreeze Нерд

    Прекрасно понимаю, только времени у меня на изучение совсем нет, постоянно работаю, и в другой сфере. А устройство собрать хочется.
    Именно так и начинал. Изучал всё с простого, потом после длительного перерыва уже вспомнить ничего не мог.
    Есть желание помочь - очень рад. А пространственное "учи с нуля" мне известно)
     
  12. parovoZZ

    parovoZZ Гуру

    внутри функции - аргумент функции, за ее пределами - глобальная переменная. При этом аргумент функции никак не влияет на глобальную переменную.
     
    Daniil нравится это.
  13. parovoZZ

    parovoZZ Гуру

    Аналагична
     
  14. SergeiL

    SergeiL Оракул Модератор

    Тут все просто. ;)
    При вызове функции на стек (в регистры) закидываются значения переменных, в том числе и глобальных.
    Все, связь с переменной потеряна, используется только значение.
    А если внутри функции упоминается глобальная переменная, в этот момент используется значение, которое хранится в переменной.
     
    Daniil нравится это.
  15. Daniil

    Daniil Гуру

    Выведите в компорт значения у, у1, z и z1 в процедуре flashLEDsSimple.
     
    WinterBreeze нравится это.
  16. WinterBreeze

    WinterBreeze Нерд

    Вот так?
    Код (C++):
    void flashLEDsSimple(int x, int y, int z, int x1, int y1, int z1)
    {
      Serial.print(y);Serial.print("\t");Serial.println(z);
      Serial.print(y1);Serial.print("\t");Serial.println(z1);
        if (y1 > y) {
            digitalWrite(rightLed, 1);
            digitalWrite(leftLed, 0);          
        } else {
            digitalWrite(leftLed, 1);    
            digitalWrite(rightLed, 0);    
        }
        if (z1 > z) {
            digitalWrite(bottomLed, 1);
            digitalWrite(frontLed, 0);        
        } else {
            digitalWrite(frontLed, 1);    
            digitalWrite(bottomLed, 0);    
        }    
    }
    О чудо! Оно работает!
    Спасибо огромное!!
    То есть в этой библиотеке обрабатываются только те данные, которые попадают в COM-порт?
     
  17. NikitOS

    NikitOS Король шутов Администратор

    Нет
     
    Daniil нравится это.
  18. Daniil

    Daniil Гуру

    Может быть когда вы увидели числа, вы начали крутить правильно?
    Вы больше ничего не меняли в коде?
     
  19. WinterBreeze

    WinterBreeze Нерд

    Угу)) Начал смотреть под нужным углом)
    Нет, ничего не менял.
    Код работает. Возможно не идеально с точки зрения стабильности, но работает.
    Нестабильность попробую исправить калибровкой модуля. Не калибровал ещё.
    А не должен был работать? Вы предлагали просто посмотреть на данные, направляемые в COM-порт?
     
  20. WinterBreeze

    WinterBreeze Нерд

    Эх! А иллюзия понимания процесса была так реальна.
    А почему тогда код начал работать по другому алгоритму?
    Знания по работе с COM-портом у меня чуть ниже 0.
    Если есть ссылка на доступный материал, по которому можно понять, как именно COM-порт участвует в данном коде, кроме вывода данных на дисплей, был бы рад её получить. Просто даже не знаю, что Гуглу вписать, чтобы он выдал ссылки не на хардкор или же Hello Word.