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

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

Нашёл на сайте код для управления светодиодами при помощи 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;
}

Помогите, пожалуйста.
Заранее спасибо!

 

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

 

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

 

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

 

С радостью бы это сделал, если писал код с нуля или был бы в состоянии полностью в нём разобраться.
А так могу только сказать, что этот кусок:

Код (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)

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

 

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

 

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

 

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

 

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

 

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

 

Аналагична

 

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

 

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

 

Вот так?

Код (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-порт?

 

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

 

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

 

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