Arduino:Примеры/Genuino101CurieIMUOrientationVisualiser

Содержание

Знакомство с Arduino

Продукты

Основы

Справочник языка Arduino

Примеры

Библиотеки

Хакинг

Изменения

Сравнение языков Arduino и Processing

Перевод: Максим Кузьмин

Проверка/Оформление/Редактирование: Мякишев Е.А.

3D-визуализатор положения платы в пространстве

Этот пример демонстрирует, как считывать значения для X, Y и Z с 6-осевого IMU, встроенного в плату Arduino/Genuino 101. IMU (от «inertial measurement unit») – это инерционное измерительное устройство, задачей которого является определение положения платы в пространстве. Оно состоит из акселерометра, определяющего ускорение свободного падения, и гироскопа, определяющего угловую скорость. В этом примере используется фильтр Мэджвика – с его помощью рассчитываются четыре кватерниона для шести осей IMU, после чего эти кватернионы используются для расчета углов Эйлера (крена, тангажа и рыскания), а они, в свою очередь, передаются в Processing. Там они используются для 3D-визуализации положения платы в пространстве – все действия (наклоны, вращение и т.д.), совершаемые с платой, будут повторяться 3D-объектом, нарисованным в окне Processing.

Необходимое оборудование

• Плата Arduino/Genuino

Библиотека Curie IMU использует IMU (акселерометр + гироскоп), встроенный в Arduino/Genuino 101.

Инструкции

1. Установите IDE Arduino в соответствии с инструкциями в этой статье.
2. Подключите плату Arduino/Genuino 101 с компьютеру.
3. Запустите IDE Arduino, а затем кликните на Инструменты > Плата (Tools > Board) и выберите Arduino/Genuino 101.
4. Зайдите в «Менеджер библиотек» и установите там библиотеку Madgwick. Чтобы сделать это, откройте IDE Arduino и кликните на Скетч > Подключить библиотеку > Управлять библиотеками (Sketch > Include Library > Manage Libraries). В открывшемся окне нужно найти библиотеку Madgwick и там же ее установить. Более подробно об установке и импорте библиотек читайте в этой статье.
5. Загрузите и установите IDE Processing, а затем создайте файл с Processing-скетчем, который выложен ниже, под Arduino-скетчем.
6. Поменяйте последовательный порт на тот, который использует ваша Arduino 101 (см. раздел «Код для Processing» ниже)
7. Загрузите скетч в Arduino 101 и убедитесь, что плата лежит ровно и неподвижно – это нужно для правильной калибровки.
8. Через несколько секунд запустите Processing-скетч, а затем как-нибудь поменяйте положение платы – соответствующим образом должно поменяться и положение 3D-объекта в окне Processing.

Цепь

Как это работает

Фильтр Мэджвика – это ПО с открытым кодом, и то, как он работает, можно посмотреть, например, здесь. Он был разработан Себастьяном Мэджвиком во время работы над получением докторской степени и создавался с прицелом на низкие требования к вычислительным ресурсам и эффективную работу даже на малых скоростях передачи данных. Фильтр Мэджвика принимает необработанные данные от гироскопа и акселерометра, а затем использует их для вычисления четырех кватернионов. Кватернионы – это 4-мерные числа, содержащие значения для X, Y, и Z (они обозначают ось, вокруг которой происходит вращение), а также значение ω (оно обозначает вращение, которое происходит вокруг этой оси). Эти кватернионы можно использовать для расчета углов Эйлера – трех углов, используемых для описания положения твердого тела в пространстве касаемо осей X, Y и Z (этот концепт был представлен Леонардом Эйлером в 1700 году). В статье Мэджвика в формулах 7, 8 и 9 с помощью углов Эйлера рассчитываются крен, тангаж и рыскание (соответствующие функции включены в библиотеку Мэджвика).

Поскольку в плату Arduino/Genuino 101 встроен IMU, мы можем определить ее положение в пространстве, а затем повторить эти действия через 3D-визуализацию в IDE Processing. Это выполняется как раз при помощи крена, тангажа и рыскания, рассчитанных при помощи фильтра Мэджвика. Эти данные через последовательный порт передаются в скетч Processing, где становятся аргументами для функций rotateX(), rotateY() и rotateZ() (в радианах), а они, в свою очередь, запускают вращение объекта в соответствии со значениями, указанными в этих аргументах.

Скетч для Arduino

Чтобы получить данные от акселерометра и гироскопа, скетч Arduino использует функции из библиотеки Curie IMU.

Поставленная задача (т.е. 3D-визуализация платы в Processing) требует от скетча Arduino двух вещей. Первая – он должен при помощи данных от IMU и алгоритма Мэджвика рассчитать крен, тангаж и рыскание. Вторая – он должен запустить последовательную коммуникацию, чтобы передать эти значения скетчу Processing.

В блоке setup() имеет место процесс калибровки, но его, если хотите, можно отключить. Для этого в переменной calibrateOffsets нужно вместо «1» поставить «0». Таким образом, если в скетче будет выполнено условие if (calibrateOffsets == 1), это запустит процесс калибровки: система определит начальные значения для каждой оси, а затем рассчитает для них компенсационные значения. Имейте в виду, что во время калибровки плата должна лежать горизонтально и максимально ровно, как показано в пункте 5.2 даташита BMI160.

Сначала давайте создадим экземпляр класса Madgwick, чтобы получить доступ к его функциям. Даем ему название filter:

Madgwick filter;

Теперь считываем данные от гироскопа и акселерометра при помощи функции библиотеки Curie IMU:

CurieIMU.readMotionSensor(ax, ay, az, gx, gy, gz);

Теперь используем функцию updateIMU() из библиотеки Madgwick:

filter.updateIMU(gx / factor, gy / factor, gz / factor, ax, ay, az);

yaw = filter.getYaw();
roll = filter.getRoll();
pitch = filter.getPitch();

Как видите, значения от гироскопа уменьшаются при помощи делителя factor, чтобы подогнать их под числовой диапазон, более подходящий для фильтра Мэджвика. Если подгонки не делать, значения останутся очень большими, вследствие чего визуализатор станет гиперчувствительным – виртуальный 3D-объект начнет сильно вращаться даже при малейших изменениях в положении платы. В скетче значением для делителя factor указано «800», но оно лишь приблизительное, поэтому с ним лучше поэкспериментировать. В частности, если вы увеличиваете скорость передачи данных, то и значение в factor тоже нужно будет увеличить.

Теперь о том, как будет происходить последовательная коммуникация. Сначала проверяем, не прислал ли скетч Processing значение «s» – он отправляет его в конце каждого цикла. Это нужно для того, чтобы скетч Arduino не отправлял данные слишком часто, иначе скетч Processing просто не успеет их обработать, вследствие чего визуализация будет сильно запаздывать. Если значение «s» получено, скетч Arduino отправляет данные через последовательный порт, разделяя их запятой, а в конце ставя символ новой строки – чтобы скетчу Processing было проще их обрабатывать. Весь этот код полностью смотрите ниже:

if (Serial.available() > 0) {
    int val = Serial.read();
    if(val == 's')
    {
      Serial.print(yaw);
      Serial.print(",");
      Serial.print(pitch);
      Serial.print(",");
      Serial.println(roll);
    }
   }

Обратите внимание, что в скетче Arduino есть также вывод значений ax, ay, az, gx, gy и gz на монитор порта. Но этот блок закомментирован, потому что его следует использовать лишь в отладочных целях.

Скетч для Processing

Самое первое – вам нужно, если вы этого еще не сделали, загрузить последнюю версию Processing отсюда. Processing – это язык, позволяющий рисовать динамические изображения, и в нем, как и в языке Arduino, используется структура скетча, состоящая из блоков setup() и loop(). Более подробно о языке Processing читайте на этой странице.

Скетч Processing принимает данные, присланные через последовательный порт, затем обрабатывает их, а получившийся результат заносит в переменные yaw, pitch и roll. Затем значения в этих переменных становятся аргументами в функциях rotateX(), rotateY() и rotateZ(), которые, в свою очередь, используются для отрисовки новой позиции объекта. В завершение скетч Processing отправляет скетчу Arduino значение «s», тем самым сообщая, что он готов к получению новых данных. Все эти действия совершаются при каждом проходе через блок loop().

Теперь надо сделать так, чтобы скетч Processing считывал данные с того же последовательного порта, по которому их передает скетч Arduino. Это указывается во втором аргументе переменной myPort, находящейся в блоке setup():

myPort = new Serial(this, Serial.list()[0], 9600);

Нужный порт можно найти при помощи функции list(), принадлежащей классу Serial. Число в квадратных скобочках – это номер последовательного порта (это может быть «0», «1», «2» и т.д.). По умолчанию скетч будет работать, если на вашей машине активен лишь один COM-порт. Если же их несколько, то порт, к которому подключена плата, можно указать напрямую (в формате «COMx») – это тот же самый порт, который используется скетчем Arduino. Но в таком случае вот эту строчку...

myPort = new Serial(this, Serial.list()[0], 9600);

...нужно будет закомментировать, а вот эту...

myPort = new Serial(this, "COM5", 9600);

...наоборот, раскомментировать. При этом значение COM5 надо будет поменять на порт, который используется в вашем проекте.

Если сомневаетесь, можно создать отдельный скетч и с его помощью вывести список всех доступных последовательных портов.

Далее пишем функцию serialEvent(), с помощью которой принимаем и обрабатываем данные:

void serialEvent()
{
  message = myPort.readStringUntil(13);
  if (message != null) {
    ypr = split(message, ",");
    yaw = float(ypr[0]);
    pitch = float(ypr[1]);
    roll = float(ypr[2]);
  }
}

Она будет считывать данные от скетча Arduino, пока не получит 13-ый ASCII-символ (т.е. символ новой строки), а затем при помощи функции split() отделит друг от друга значения, разделенные запятой. Мы знаем, что эти данные передаются от Arduino в порядке yaw-pitch-roll (т.е. сначала крен, потом тангаж, а потом рыскание), поэтому записываем их в трехсоставной массив строк ypr[], а затем в переменные yaw, pitch и roll, тем самым конвертируя их в числа с плавающей точкой.

Полные версии скетчей для Arduino и Processing смотрите ниже.

Код

Скетч для Arduino

/*
  ===============================================
  Скетч-пример для библиотеки Curie IMU, предназначенной для 
  устройств Intel Curie. Все права защищены и принадлежат
  Intel Corporation (2015). 

  Основан на демонстрационном Arduino-скетче Джеффа Роуберга 
  (Jeff Rowberg) для библиотеки I2Cdev и устройства MPU6050: 
  https://github.com/jrowberg/i2cdevlib

  ===============================================
  Код библиотеки I2Cdev размещен в соответствии с лицензией MIT. 
  Права принадлежат Джеффу Роубергу. 

  Настоящим разрешается любому лицу, владеющему копией этой программы
  и сопутствующих документационных файлов (далее – «ПО»), а также тем,
  кому это ПО поставляется, обращаться с этим ПО без ограничений,
  включая права на использование, копирование, модификацию, слияние, 
  публикацию, распространение, сублицензирование и/или продажу копий 
  ПО, но при соблюдении следующих условий:
  
  Запись об авторском праве, написанная выше, а также запись о
  разрешениях, написанная ниже, должна быть включена во все копии
  или существенные части этого ПО.

  ДАННОЕ ПО ПРЕДОСТАВЛЯЕТСЯ «КАК ЕСТЬ», БЕЗО ВСЯКИХ ГАРАНТИЙ, ЯВНЫХ 
  ИЛИ ПОДРАЗУМЕВАЕМЫХ, ВКЛЮЧАЯ ГАРАНТИИ НА КОММЕРЧЕСКОЕ КАЧЕСТВО,
  ПРИГОДНОСТЬ ПРИМЕНЕНИЯ ДЛЯ КОНКРЕТНЫХ ЦЕЛЕЙ И НЕНАРУШЕНИЕ ЧЬИХ-ЛИБО
  ПРАВ. НИ ПРИ КАКИХ ОБСТОЯТЕЛЬСТВАХ АВТОРЫ ИЛИ ПРАВООБЛАДАТЕЛИ НЕ
  НЕСУТ ОТВЕТСВЕННОСТИ ЗА ЛЮБЫЕ ЖАЛОБЫ, УЩЕРБ И ПРОЧИЕ ПРЕТЕНЗИИ,
  БУДЬ ТО ИСК ПО КОНТРАКТУ, ПРАВОНАРУШЕНИЮ И Т.Д., ВОЗНИКШИЙ ИЗ ИЛИ 
  В СВЯЗИ С ПО, ИСПОЛЬЗОВАНИЯ ПО ИЛИ ПРОЧИХ ДЕЙСТВИЙ С ПО. 

  ===============================================

  3D-визуализатор положение платы в пространстве для Arduino 101

  Необходимое оборудование:
  * Arduino/Genuino 101

  Модифицирован в ноябре 2015 года 
  Хеленой Бисби (Helena Bisby) <support@arduino.cc>
  Этот скетч-пример находится в публичном доступе. 
  http://arduino.cc/en/Tutorial/Genuino101CurieIMUOrientationVisualiser
*/

#include <CurieIMU.h>
#include <MadgwickAHRS.h>

Madgwick filter; // инициализируем объект Madgwick
int ax, ay, az;
int gx, gy, gz;
float yaw;
float pitch;
float roll;
int factor = 800; // на это значение будут поделены данные от
                  // гироскопа (нужно для контроля чувствительности);
                  // обратите внимание, что если вы повышаете скорость
                  // передачи данных, то значение в этой переменной
                  // тоже нужно увеличить

int calibrateOffsets = 1; // здесь устанавливается, будет ли 
                          // запущена калибровка или нет 


void setup() {
  // инициализируем последовательную коммуникацию:
  Serial.begin(9600);

  // инициализируем IMU:
  CurieIMU.begin();
  
  if (calibrateOffsets == 1) {
    // код для калибровки смещения у акселерометра/гироскопа:
    Serial.println("Internal sensor offsets BEFORE calibration...");  //  "Смещение ПЕРЕД калибровкой..."   
    Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getGyroOffset(X_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getGyroOffset(Y_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getGyroOffset(Z_AXIS)); Serial.print("\t");
    Serial.println("");

    // используйте эти функции, если хотите задать компенсационные
    // значения вручную; если хотите, чтобы эти значения выставились
    // автоматически, используйте функцию autoCalibrate...()
    //    CurieIMU.setGyroOffset(X_AXIS, 220);
    //    CurieIMU.setGyroOffset(Y_AXIS, 76);
    //    CurieIMU.setGyroOffset(Z_AXIS, -85);
    //    CurieIMU.setAccelerometerOffset(X_AXIS, -76);
    //    CurieIMU.setAccelerometerOffset(Y_AXIS, -235);
    //    CurieIMU.setAccelerometerOffset(Z_AXIS, 168);

    // чтобы калибровка прошла правильно, IMU-устройство
    // должно неподвижно лежать в горизонтальном положении: 

    Serial.print("Starting Gyroscope calibration...");  //  "Запускаем калибровку гироскопа..."   
    CurieIMU.autoCalibrateGyroOffset();
    Serial.println(" Done");  //  " Готово"   
    Serial.print("Starting Acceleration calibration...");  //  "Запускаем калибровку акселерометра..."   
    CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
    Serial.println(" Done");  //  " Готово"   

    Serial.println("Internal sensor offsets AFTER calibration..."); //  "Смещение ПОСЛЕ калибровки..."   
    Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
    Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
    Serial.println("");
  }
}

void loop() {
  // считываем «сырые» данные с гироскопа и акселерометра: 
  CurieIMU.readMotionSensor(ax, ay, az, gx, gy, gz); 

  // используем функцию из MagdwickAHRS.h, чтобы рассчитать кватернионы:
  filter.updateIMU(gx / factor, gy / factor, gz / factor, ax, ay, az);

  // функции для расчета крена, тангажа и рыскания из кватернионов: 
  yaw = filter.getYaw();
  roll = filter.getRoll();
  pitch = filter.getPitch();
  
  // задача этого фрагмента – вывод на монитор порта значений от 
  // гироскопа и акселерометра; он нужен только для отладки, поэтому
  // при работе со скетчем Processing его нужно отключить.
  /* 
  Serial.print(ax); Serial.print("\t");
  Serial.print(ay); Serial.print("\t");
  Serial.print(az); Serial.print("\t");
  Serial.print(gx); Serial.print("\t");
  Serial.print(gy); Serial.print("\t");
  Serial.print(gz); Serial.print("\t");
  Serial.println("");
  */

  if (Serial.available() > 0) {
    int val = Serial.read();
    if (val == 's') { // если получили символ "s"
      Serial.print(yaw);
      Serial.print(","); // пишем запятую, чтобы скетчу Processing было легче обрабатывать данные
      Serial.print(pitch);
      Serial.print(","); // пишем запятую, чтобы скетчу Processing было легче обрабатывать данные
      Serial.println(roll);
    }
  }
}

Скетч для Processing

import processing.serial.*;
Serial myPort;

int newLine = 13; // символ новой стоки в ASCII-формате
float yaw;
float pitch;
float roll;
String message;
String [] ypr = new String [3];

void setup()
{
  size(600, 500, P3D);
  
  /* порт делаем таким же, как у Arduino, скорость – 9600 бод */
  myPort = new Serial(this, Serial.list()[0], 9600); // если у вас активен только один COM-порт 
  //myPort = new Serial(this, "COM5", 9600);  // если вы знаете, какой именно порт использует Arduino 101

  textSize(16); // задаем размер текста 
  textMode(SHAPE); // режимом текста делаем SHAPE 
}

void draw()
{
  serialEvent();  // считываем и обрабатываем сообщение, пришедшее по последовательному порту
  background(255); // делаем фон белым

  translate(width/2, height/2); // задаем позицию в центре
  
  pushMatrix(); // начинаем объект
  
  rotateX(pitch); // вращение по оси крена
  rotateY(-yaw); // вращение по оси тангажа 
  rotateZ(-roll); // вращение по оси рыскания
  
  drawArduino(); // функция, рисующая виртуальную версию Arduino
  
  popMatrix(); // заканчиваем объект

  // выводим данные на консоль:
  print(pitch);
  print("\t");
  print(roll); 
  print("\t");
  print(-yaw);   
  println("\t");

  myPort.write("s"); // передаем символ «s», чтобы получить новые данные от Arduino
} 

void serialEvent()
{
  message = myPort.readStringUntil(newLine); // считываем данные, пока не получим символ новой строки (13-ый символ в ASCII-формате)
  if (message != null) {
    ypr = split(message, ","); // дробим сообщение по запятым и сохраняем их в массив строк 
    yaw = float(ypr[0]); // полученное число с плавающей точкой конвертируем в переменную yaw
    pitch = float(ypr[1]); // полученное число с плавающей точкой конвертируем в переменную pitch
    roll = float(ypr[2]); // полученное число с плавающей точкой конвертируем в переменную roll 
  }
}
void drawArduino() {
  /* функция, рисующая фигуры, вращаемые IMU */
  stroke(0, 90, 90); // делаем цвет контура темно-бирюзовым 
  fill(0, 130, 130); // делаем цвет заливки светло-бирюзовым 
  box(300, 10, 200); // рисуем основание платы Arduino 

  stroke(0); // делаем цвет контура черным 
  fill(80); // делаем цвет заливки темно-серым

  translate(60, -10, 90); // задаем позицию для первого штырьевого разъема 
  box(170, 20, 10); // рисуем первый штырьевой разъем 

  translate(-20, 0, -180); // задаем позицию для второго штырьевого разъема
  box(210, 20, 10); // рисуем второй штырьевой разъем
}

См.также

1. Curie IMU Accelerometer
2. Curie IMU Accelerometer Orientation
3. Curie IMU Gyro
4. Curie IMU Raw Imu Data Serial
5. Curie IMU Shock Detect
6. Curie IMU Step Count
7. Curie IMU Tap Detect