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.4k
    • 2
    Share post
    AdBlock has stolen the banner, but banners are not teeth — they will be back

    More
    Ads

    Comments 2

      0
      Выглядит впечатляюще!

      Понимаю, что маловероятно, но было бы здорово, если бы вы попробовали заменить Ruby на Crystal или mruby ради спортивного интереса и поделиться охапкой впечатлений и бенчмарков :)
        +1
        Думаю, многим проще будет просто построить биндинг к C-библиотеке, разнеся различный функционал по разным проектам.

        Only users with full accounts can post comments. Log in, please.