Abnormal programming
Ruby
Programming
9 June 2015

Ruby и C. Часть 4. Дружим акселерометр, гироскоп и дальномер с Raphael.js

В предыдущих частях от iv_s (раз два три) были описаны различные техники использования C и Ruby вместе. Я бы хотел рассказать о еще одной возможной связке – использовании уже существующих системных C-функций.

Я потихоньку улучшаю своего робота-рисовача. Он написан на Ruby, поэтому при подключении к нему акселерометра с гироскопом, мне, само собой, захотелось продолжить использовать эту технологию.

Как оказалось, достучаться до функций работы с шиной I2C в Ruby предельно просто – он позволяет использовать уже написанные и установленные библиотеки на C.



Схема работы такая:
на 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 0x68 #адрес устройства на шине I2C. Берется в мануале или с помощью утилиты i2cdetect.
@i2c_write_reg8.call @fd, 0x6B, 0x00 # пишем в устройство, в регистр 0x6B значение 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 0x68
    @i2c_write_reg8.call @fd, 0x6B, 0x00

    @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 >= 0x8000
    val
  end

  def measure
    gyro_x = (read_word_2c(@fd, 0x43) / 131.0).round(1)
    gyro_y = (read_word_2c(@fd, 0x45) / 131.0).round(1)
    gyro_z = (read_word_2c(@fd, 0x47) / 131.0).round(1)


    acceleration_x = read_word_2c(@fd, 0x3b) / 16384.0
    acceleration_y = read_word_2c(@fd, 0x3d) / 16384.0
    acceleration_z = read_word_2c(@fd, 0x3f) / 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 '<wiringPi.h>'
    builder.c '
    double measure(int trig, int echo){
        //initial pulse
        digitalWrite(trig, HIGH);
        delayMicroseconds(20);
        digitalWrite(trig, LOW);

        //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.Matrix4x4.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.
Все железки я брал в Минском хакерспейсе, полный код проекта можно посмотреть здесь.

+12
8.3k 89
Comments 2