Ruby и C. Часть 4. Дружим акселерометр, гироскоп и дальномер с Raphael.js
В предыдущих частях от iv_s (раздва три) были описаны различные техники использования C и Ruby вместе. Я бы хотел рассказать о еще одной возможной связке — использовании уже существующих системных C-функций.Я потихоньку улучшаю своего робота-рисовача. Он написан на Ruby, поэтому при подключении к нему акселерометра с гироскопом, мне, само собой, захотелось продолжить использовать эту технологию.
Как оказалось, достучаться до функций работы с шиной I2C в Ruby предельно просто — он позволяет использовать уже написанные и установленные библиотеки на C.
[embedded content]
Схема работы такая: на RaspberryPi запущен Sinatra сервер, который при обращении отдает данные о повороте платы по осям X и Y, а также расстояние до ближайшего препятствия в сантиметрах.На клиенте для визуализации и отладки написан простой скрипт с использованием Raphael3d.js, который каждые 100 мс опрашивает устройство и поворачивает схематическую плату в соответствии с положением платы физической.
Аппаратная часть Подключаем плату акселерометра/гироскопа. В моем случае это трехдолларовый MPU6050.Чтобы получить доступ к функциям этой платы, таким как чтение/запись в регистры, инициализацию и прочее, нужно установить wiringPi. Если кто-то из читающих не в курсе, wiringPi дает простой доступ к выводам (GPIO) и устройствам RaspberryPi. Так что весь описанный далее механизм справедлив для любой из задач, от мигания светодиодом, до работы с PWM.
Следующий шаг — найти скомпилированную библиотеку wiringPi и подключить её к ruby-проекту.
require 'fiddle' wiringpi = Fiddle.dlopen ('/home/pi/wiringPi/wiringPi/libwiringPi.so.2.25') Теперь можно напрямую вызывать все функции из этой библиотеки в том виде, как их задумывал разработчик.Fiddle — это стандартный модуль Ruby, который использует стандартный же *nix механизм libffi (Foreign Function Interface Library).Поскольку мне нужны не все функции библиотеки, то я выбираю нужные и регистрирую только их: Выбираем то, что надо в файле wiringPiI2C.h
extern int wiringPiI2CSetup (const int devId) ; extern int wiringPiI2CWriteReg8 (int fd, int reg, int data) ; И подключаем в программе:
int = Fiddle: TYPE_INT @i2c_setup = Fiddle: Function.new (wiringpi['wiringPiI2CSetup'], [int], int) @i2c_write_reg8 = Fiddle: Function.new (wiringpi['wiringPiI2CWriteReg8'], [int, int, int], int) Параметры это — имя функции, массив принимаемых параметров и возвращаемое значение. Если передаются указатели, то, вне зависимости от их типа, они принимаются равными Fiddle: TYPE_VOIDPВот так происходит вызов подключенной функции:
@fd = @i2c_setup.call 0×68 #адрес устройства на шине I2C. Берется в мануале или с помощью утилиты i2cdetect. @i2c_write_reg8.call @fd, 0×6B, 0×00 # пишем в устройство, в регистр 0×6B значение 0. В данном случае — это вывод из спящего режима. Вот собственно и всё, я сделал класс MPU6050, в конструкторе которого я объявляю все необходимые мне функции, и функцией measure, которая возвращает данные о повороте платы, используя немного магии Калмана.Полный код класса для работы с акселерометром require 'fiddle'
class MPU6050 attr_reader: last_x, : last_y, : k def initialize (path_to_wiring_pi_so) wiringpi = Fiddle.dlopen (path_to_wiring_pi_so)
int = Fiddle: TYPE_INT char_p = Fiddle: TYPE_VOIDP
# int wiringPiI2CSetup (int devId) ; @i2c_setup = Fiddle: Function.new (wiringpi['wiringPiI2CSetup'], [int], int)
# int wiringPiI2CSetupInterface (const char *device, int devId) ; @i2c_setup_interface = Fiddle: Function.new (wiringpi['wiringPiI2CSetupInterface'], [char_p, int], int)
# int wiringPiI2CRead (int fd) ; @i2c_read = Fiddle: Function.new (wiringpi['wiringPiI2CRead'], [int], int)
# int wiringPiI2CWrite (int fd, int data) ; @i2c_write = Fiddle: Function.new (wiringpi['wiringPiI2CWrite'], [int, int], int)
# int wiringPiI2CWriteReg8 (int fd, int reg, int data) ; @i2c_write_reg8 = Fiddle: Function.new (wiringpi['wiringPiI2CWriteReg8'], [int, int, int], int)
# int wiringPiI2CWriteReg16 (int fd, int reg, int data) ; @i2c_write_reg8 = Fiddle: Function.new (wiringpi['wiringPiI2CWriteReg16'], [int, int, int], int)
# int wiringPiI2CReadReg8 (int fd, int reg) ; @i2c_read_reg8 = Fiddle: Function.new (wiringpi['wiringPiI2CReadReg8'], [int, int], int)
# int wiringPiI2CReadReg16 (int fd, int reg) ; @i2c_read_reg16 = Fiddle: Function.new (wiringpi['wiringPiI2CReadReg16'], [int, int], int)
@fd = @i2c_setup.call 0×68 @i2c_write_reg8.call @fd, 0×6B, 0×00
@last_x = 0 @last_y = 0 @k = 0.30
end
def read_word_2c (fd, addr) val = @i2c_read_reg8.call (fd, addr) val = val << 8 val += @i2c_read_reg8.call(fd, addr+1) val -= 65536 if val >= 0×8000 val end
def measure gyro_x = (read_word_2c (@fd, 0×43) / 131.0).round (1) gyro_y = (read_word_2c (@fd, 0×45) / 131.0).round (1) gyro_z = (read_word_2c (@fd, 0×47) / 131.0).round (1)
acceleration_x = read_word_2c (@fd, 0×3b) / 16384.0 acceleration_y = read_word_2c (@fd, 0×3d) / 16384.0 acceleration_z = read_word_2c (@fd, 0×3f) / 16384.0
rotation_x = k * get_x_rotation (acceleration_x, acceleration_y, acceleration_z) + (1-k) * @last_x rotation_y = k * get_y_rotation (acceleration_x, acceleration_y, acceleration_z) + (1-k) * @last_y
@last_x = rotation_x @last_y = rotation_y
# {gyro_x: gyro_x, gyro_y: gyro_y, gyro_z: gyro_z, rotation_x: rotation_x, rotation_y: rotation_y} »#{rotation_x.round (1)} #{rotation_y.round (1)}» end
private def to_degrees (radians) radians / Math: PI * 180 end
def dist (a, b) Math.sqrt ((a*a)+(b*b)) end
def get_x_rotation (x, y, z) to_degrees Math.atan (x / dist (y, z)) end
def get_y_rotation (x, y, z) to_degrees Math.atan (y / dist (x, z)) end
end Этот подход вполне оправдывает себя, когда нет жестких ограничений по времени. То есть, когда речь идет о миллисекундах. А вот когда дело доходит до микросекунд, то приходится использовать вставки C-кода в программу. Иначе просто не успевает.Так получилось с дальномером, его принцип работы — послать сигнал начала измерений в 10 микросекунд, измерить длину обратного импульса, поделить на коэффициент, чтобы получить расстояние в сантиметрах.
Класс для измерения расстояния require 'fiddle' require 'inline'
class HCSRO4 IN = 0 OUT = 1
TRIG = 17 ECHO = 27
def initialize (path_to_wiring_pi_so) wiringpi = Fiddle.dlopen (path_to_wiring_pi_so)
int = Fiddle: TYPE_INT void = Fiddle: TYPE_VOID
# extern int wiringPiSetup (void) ; @setup = Fiddle: Function.new (wiringpi['wiringPiSetup'], [void], int)
# extern int wiringPiSetupGpio (void) ; @setup_gpio = Fiddle: Function.new (wiringpi['wiringPiSetupGpio'], [void], int)
# extern void pinMode (int pin, int mode) ; @pin_mode = Fiddle: Function.new (wiringpi['pinMode'], [int, int], void)
@setup_gpio.call nil @pin_mode.call TRIG, OUT @pin_mode.call ECHO, IN end
inline do |builder|
#sudo cp WiringPi/wiringPi/*.h /usr/include/
builder.include '
//Wait for echo start while (digitalRead (echo) == LOW);
//Wait for echo end long startTime = micros (); while (digitalRead (echo) == HIGH);
long travelTime = micros () — startTime; double distance = travelTime / 58.0;
return distance; } ' end end Минимальный сервер: require 'sinatra' require_relative 'mpu6050' require_relative 'hcsro4'
configure do set: mpu, MPU6050.new ('/home/pi/wiringPi/wiringPi/libwiringPi.so.2.25') set: hc, HCSRO4.new ('/home/pi/wiringPi/wiringPi/libwiringPi.so.2.25') end
get '/' do response['Access-Control-Allow-Origin'] = '*' settings.mpu.measure.to_s + ' ' + settings.hc.measure (17, 27).to_s # пины, к которым подключен дальномер end Что люди не сделают, чтобы не писать на питоне…Альтарнативных вариантов решения задачи много, но мне интереснее мой собственный.В теории, есть библиотека, которая как раз и нужна для работы с wiringPi в Ruby, но на момент публикации она не поддерживает работы RaspberryPi второй модели.Есть также удобная Ruby обертка для механизма libffi с понятным DSL и обработкой всех исключений.Визуализация Ajax запрос каждые 100 мс и отображение с помощью Raphael. Строго говоря, это не сам Raphael, а его расширение для работы с трехмерными объектами. var scene, viewer; var rotationX = 0, rotationY = 0; var divX = document.getElementById ('rotation_x'); var divY = document.getElementById ('rotation_y');
function rotate (x, y, z){ scene.camera.rotateX (x).rotateZ (y).rotateY (z); viewer.update (); }
function getAngles (){ var r = new XMLHttpRequest (); r.open ('get','http://192.168.0.102:4567', true); r.send (); r.onreadystatechange = function (){ if (r.readyState!= 4 || r.status!= 200) return; var angles = r.responseText.split (' ');
divX.textContent = angles[0]; divY.textContent = angles[1];
x_deg = Math.PI * (parseFloat (angles[0]) — rotationX)/ 180; y_deg = Math.PI * (parseFloat (angles[1]) — rotationY)/ 180;
rotate (x_deg, y_deg, 0); rotationX = parseFloat (angles[0]); rotationY = parseFloat (angles[1]); } }
window.onload = function () { var paper = Raphael ('canvas', 1000, 800); var mat = new Raphael3d.Material ('#363', '#030'); var cube = Raphael3d.Surface.Box (0, 0, 0, 5, 4, 0.15, paper, {}); scene = new Raphael3d.Scene (paper); scene.setMaterial (mat).addSurfaces (cube); scene.projection = Raphael3d.Matrix4×4.PerspectiveMatrixZ (900); viewer = paper.Viewer (45, 45, 998, 798, {opacity: 0}); viewer.setScene (scene).fit (); rotate (-1.5,0.2, 0);
var timer = setInterval (getAngles, 100); document.getElementById ('canvas').onclick = function (){ clearInterval (timer); } } В заключение могу сказать, что меня восхищают современные возможности. Работа с шиной I2C и Javascript находятся на разных полюсах технологий. Пропасть между hardware разработкой, 3D-графикой и Javascript’ом оказывается не такой уж и пропастью, даже если этим занимается совсем не программист, а как раз наоборот, менеджер, как я. Курение мануалов, помноженное на обилие документации, дает о себе знать.P.S. Все железки я брал в Минском хакерспейсе, полный код проекта можно посмотреть здесь.
