Arduino:Примеры/Genuino101CurieIMUOrientationVisualiser

Материал из Онлайн справочника
Перейти к навигации Перейти к поиску

Перевод: Максим Кузьмин (Cubewriter) Перевел 364226 статей для сайта.</br>Контакты:</br>* Skype: cubewriter</br>* E-mail: cubewriter@gmail.com</br>* Максим Кузьмин на freelance.ru
Проверка/Оформление/Редактирование: Мякишев Е.А.


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

Этот пример демонстрирует, как считывать значения для 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.

Цепь

Genuino101fzz.jpg

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

Фильтр Мэджвика – это ПО с открытым кодом, и то, как он работает, можно посмотреть, например, здесь. Он был разработан Себастьяном Мэджвиком во время работы над получением докторской степени и создавался с прицелом на низкие требования к вычислительным ресурсам и эффективную работу даже на малых скоростях передачи данных. Фильтр Мэджвика принимает необработанные данные от гироскопа и акселерометра, а затем использует их для вычисления четырех кватернионов. Кватернионы – это 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:

1 filter.updateIMU(gx / factor, gy / factor, gz / factor, ax, ay, az);
2 
3 yaw = filter.getYaw();
4 roll = filter.getRoll();
5 pitch = filter.getPitch();

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

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

 1 if (Serial.available() > 0) {
 2     int val = Serial.read();
 3     if(val == 's')
 4     {
 5       Serial.print(yaw);
 6       Serial.print(",");
 7       Serial.print(pitch);
 8       Serial.print(",");
 9       Serial.println(roll);
10     }
11    }

Обратите внимание, что в скетче 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(), с помощью которой принимаем и обрабатываем данные:

 1 void serialEvent()
 2 {
 3   message = myPort.readStringUntil(13);
 4   if (message != null) {
 5     ypr = split(message, ",");
 6     yaw = float(ypr[0]);
 7     pitch = float(ypr[1]);
 8     roll = float(ypr[2]);
 9   }
10 }

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

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

Код

Скетч для Arduino

  1 /*
  2   ===============================================
  3   Скетч-пример для библиотеки Curie IMU, предназначенной для 
  4   устройств Intel Curie. Все права защищены и принадлежат
  5   Intel Corporation (2015). 
  6 
  7   Основан на демонстрационном Arduino-скетче Джеффа Роуберга 
  8   (Jeff Rowberg) для библиотеки I2Cdev и устройства MPU6050: 
  9   https://github.com/jrowberg/i2cdevlib
 10 
 11   ===============================================
 12   Код библиотеки I2Cdev размещен в соответствии с лицензией MIT. 
 13   Права принадлежат Джеффу Роубергу. 
 14 
 15   Настоящим разрешается любому лицу, владеющему копией этой программы
 16   и сопутствующих документационных файлов (далее – «ПО»), а также тем,
 17   кому это ПО поставляется, обращаться с этим ПО без ограничений,
 18   включая права на использование, копирование, модификацию, слияние, 
 19   публикацию, распространение, сублицензирование и/или продажу копий 
 20   ПО, но при соблюдении следующих условий:
 21   
 22   Запись об авторском праве, написанная выше, а также запись о
 23   разрешениях, написанная ниже, должна быть включена во все копии
 24   или существенные части этого ПО.
 25 
 26   ДАННОЕ ПО ПРЕДОСТАВЛЯЕТСЯ «КАК ЕСТЬ», БЕЗО ВСЯКИХ ГАРАНТИЙ, ЯВНЫХ 
 27   ИЛИ ПОДРАЗУМЕВАЕМЫХ, ВКЛЮЧАЯ ГАРАНТИИ НА КОММЕРЧЕСКОЕ КАЧЕСТВО,
 28   ПРИГОДНОСТЬ ПРИМЕНЕНИЯ ДЛЯ КОНКРЕТНЫХ ЦЕЛЕЙ И НЕНАРУШЕНИЕ ЧЬИХ-ЛИБО
 29   ПРАВ. НИ ПРИ КАКИХ ОБСТОЯТЕЛЬСТВАХ АВТОРЫ ИЛИ ПРАВООБЛАДАТЕЛИ НЕ
 30   НЕСУТ ОТВЕТСВЕННОСТИ ЗА ЛЮБЫЕ ЖАЛОБЫ, УЩЕРБ И ПРОЧИЕ ПРЕТЕНЗИИ,
 31   БУДЬ ТО ИСК ПО КОНТРАКТУ, ПРАВОНАРУШЕНИЮ И Т.Д., ВОЗНИКШИЙ ИЗ ИЛИ 
 32   В СВЯЗИ С ПО, ИСПОЛЬЗОВАНИЯ ПО ИЛИ ПРОЧИХ ДЕЙСТВИЙ С ПО. 
 33 
 34   ===============================================
 35 
 36   3D-визуализатор положение платы в пространстве для Arduino 101
 37 
 38   Необходимое оборудование:
 39   * Arduino/Genuino 101
 40 
 41   Модифицирован в ноябре 2015 года 
 42   Хеленой Бисби (Helena Bisby) <support@arduino.cc>
 43   Этот скетч-пример находится в публичном доступе. 
 44   http://arduino.cc/en/Tutorial/Genuino101CurieIMUOrientationVisualiser
 45 */
 46 
 47 #include <CurieIMU.h>
 48 #include <MadgwickAHRS.h>
 49 
 50 Madgwick filter; // инициализируем объект Madgwick
 51 int ax, ay, az;
 52 int gx, gy, gz;
 53 float yaw;
 54 float pitch;
 55 float roll;
 56 int factor = 800; // на это значение будут поделены данные от
 57                   // гироскопа (нужно для контроля чувствительности);
 58                   // обратите внимание, что если вы повышаете скорость
 59                   // передачи данных, то значение в этой переменной
 60                   // тоже нужно увеличить
 61 
 62 int calibrateOffsets = 1; // здесь устанавливается, будет ли 
 63                           // запущена калибровка или нет 
 64 
 65 
 66 void setup() {
 67   // инициализируем последовательную коммуникацию:
 68   Serial.begin(9600);
 69 
 70   // инициализируем IMU:
 71   CurieIMU.begin();
 72   
 73   if (calibrateOffsets == 1) {
 74     // код для калибровки смещения у акселерометра/гироскопа:
 75     Serial.println("Internal sensor offsets BEFORE calibration...");  //  "Смещение ПЕРЕД калибровкой..."   
 76     Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
 77     Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
 78     Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
 79     Serial.print(CurieIMU.getGyroOffset(X_AXIS)); Serial.print("\t");
 80     Serial.print(CurieIMU.getGyroOffset(Y_AXIS)); Serial.print("\t");
 81     Serial.print(CurieIMU.getGyroOffset(Z_AXIS)); Serial.print("\t");
 82     Serial.println("");
 83 
 84     // используйте эти функции, если хотите задать компенсационные
 85     // значения вручную; если хотите, чтобы эти значения выставились
 86     // автоматически, используйте функцию autoCalibrate...()
 87     //    CurieIMU.setGyroOffset(X_AXIS, 220);
 88     //    CurieIMU.setGyroOffset(Y_AXIS, 76);
 89     //    CurieIMU.setGyroOffset(Z_AXIS, -85);
 90     //    CurieIMU.setAccelerometerOffset(X_AXIS, -76);
 91     //    CurieIMU.setAccelerometerOffset(Y_AXIS, -235);
 92     //    CurieIMU.setAccelerometerOffset(Z_AXIS, 168);
 93 
 94     // чтобы калибровка прошла правильно, IMU-устройство
 95     // должно неподвижно лежать в горизонтальном положении: 
 96 
 97     Serial.print("Starting Gyroscope calibration...");  //  "Запускаем калибровку гироскопа..."   
 98     CurieIMU.autoCalibrateGyroOffset();
 99     Serial.println(" Done");  //  " Готово"   
100     Serial.print("Starting Acceleration calibration...");  //  "Запускаем калибровку акселерометра..."   
101     CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
102     CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
103     CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
104     Serial.println(" Done");  //  " Готово"   
105 
106     Serial.println("Internal sensor offsets AFTER calibration..."); //  "Смещение ПОСЛЕ калибровки..."   
107     Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
108     Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
109     Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
110     Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
111     Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
112     Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
113     Serial.println("");
114   }
115 }
116 
117 void loop() {
118   // считываем «сырые» данные с гироскопа и акселерометра: 
119   CurieIMU.readMotionSensor(ax, ay, az, gx, gy, gz); 
120 
121   // используем функцию из MagdwickAHRS.h, чтобы рассчитать кватернионы:
122   filter.updateIMU(gx / factor, gy / factor, gz / factor, ax, ay, az);
123 
124   // функции для расчета крена, тангажа и рыскания из кватернионов: 
125   yaw = filter.getYaw();
126   roll = filter.getRoll();
127   pitch = filter.getPitch();
128   
129   // задача этого фрагмента – вывод на монитор порта значений от 
130   // гироскопа и акселерометра; он нужен только для отладки, поэтому
131   // при работе со скетчем Processing его нужно отключить.
132   /* 
133   Serial.print(ax); Serial.print("\t");
134   Serial.print(ay); Serial.print("\t");
135   Serial.print(az); Serial.print("\t");
136   Serial.print(gx); Serial.print("\t");
137   Serial.print(gy); Serial.print("\t");
138   Serial.print(gz); Serial.print("\t");
139   Serial.println("");
140   */
141 
142   if (Serial.available() > 0) {
143     int val = Serial.read();
144     if (val == 's') { // если получили символ "s"
145       Serial.print(yaw);
146       Serial.print(","); // пишем запятую, чтобы скетчу Processing было легче обрабатывать данные
147       Serial.print(pitch);
148       Serial.print(","); // пишем запятую, чтобы скетчу Processing было легче обрабатывать данные
149       Serial.println(roll);
150     }
151   }
152 }

Скетч для Processing

 1 import processing.serial.*;
 2 Serial myPort;
 3 
 4 int newLine = 13; // символ новой стоки в ASCII-формате
 5 float yaw;
 6 float pitch;
 7 float roll;
 8 String message;
 9 String [] ypr = new String [3];
10 
11 void setup()
12 {
13   size(600, 500, P3D);
14   
15   /* порт делаем таким же, как у Arduino, скорость – 9600 бод */
16   myPort = new Serial(this, Serial.list()[0], 9600); // если у вас активен только один COM-порт 
17   //myPort = new Serial(this, "COM5", 9600);  // если вы знаете, какой именно порт использует Arduino 101
18 
19   textSize(16); // задаем размер текста 
20   textMode(SHAPE); // режимом текста делаем SHAPE 
21 }
22 
23 void draw()
24 {
25   serialEvent();  // считываем и обрабатываем сообщение, пришедшее по последовательному порту
26   background(255); // делаем фон белым
27 
28   translate(width/2, height/2); // задаем позицию в центре
29   
30   pushMatrix(); // начинаем объект
31   
32   rotateX(pitch); // вращение по оси крена
33   rotateY(-yaw); // вращение по оси тангажа 
34   rotateZ(-roll); // вращение по оси рыскания
35   
36   drawArduino(); // функция, рисующая виртуальную версию Arduino
37   
38   popMatrix(); // заканчиваем объект
39 
40   // выводим данные на консоль:
41   print(pitch);
42   print("\t");
43   print(roll); 
44   print("\t");
45   print(-yaw);   
46   println("\t");
47 
48   myPort.write("s"); // передаем символ «s», чтобы получить новые данные от Arduino
49 } 
50 
51 void serialEvent()
52 {
53   message = myPort.readStringUntil(newLine); // считываем данные, пока не получим символ новой строки (13-ый символ в ASCII-формате)
54   if (message != null) {
55     ypr = split(message, ","); // дробим сообщение по запятым и сохраняем их в массив строк 
56     yaw = float(ypr[0]); // полученное число с плавающей точкой конвертируем в переменную yaw
57     pitch = float(ypr[1]); // полученное число с плавающей точкой конвертируем в переменную pitch
58     roll = float(ypr[2]); // полученное число с плавающей точкой конвертируем в переменную roll 
59   }
60 }
61 void drawArduino() {
62   /* функция, рисующая фигуры, вращаемые IMU */
63   stroke(0, 90, 90); // делаем цвет контура темно-бирюзовым 
64   fill(0, 130, 130); // делаем цвет заливки светло-бирюзовым 
65   box(300, 10, 200); // рисуем основание платы Arduino 
66 
67   stroke(0); // делаем цвет контура черным 
68   fill(80); // делаем цвет заливки темно-серым
69 
70   translate(60, -10, 90); // задаем позицию для первого штырьевого разъема 
71   box(170, 20, 10); // рисуем первый штырьевой разъем 
72 
73   translate(-20, 0, -180); // задаем позицию для второго штырьевого разъема
74   box(210, 20, 10); // рисуем второй штырьевой разъем
75 }

См.также

  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

Внешние ссылки