Ruby and C. Part 4. Friends of the accelerometer, gyroscope and rangefinder with Raphael.js

    In previous parts of iv_s ( two or three times ), various techniques have been described for using C and Ruby together. I would like to talk about another possible bundle - the use of existing system C-functions. I am slowly improving my drawing robot . It is written in Ruby, so when I connected an accelerometer with a gyroscope to it, I, of course, wanted to continue using this technology. As it turned out, getting to the I2C bus functions in Ruby is extremely simple - it allows you to use already written and installed libraries in C.







    The working scheme is as follows:
    on RaspberryPi, a Sinatra server is launched, which, when accessed, provides data on the board's rotation along the X and Y axes, as well as the distance to the nearest obstacle in centimeters.
    A simple script was written on the client for visualization and debugging using Raphael3d.js , which every 100ms polls the device and rotates the circuit board in accordance with the position of the physical board.

    Hardware


    We connect the board of the accelerometer / gyroscope. In my case, this is a three- dollar MPU6050 .

    To access the functions of this board, such as reading / writing to registers, initialization, etc., wiringPi must be installed . If one of the readers is not up to date, wiringPi gives easy access to pins (GPIO) and RaspberryPi devices. So the whole mechanism described below is valid for any of the tasks, from blinking LEDs to working with PWM.

    The next step is to find the compiled wiringPi library and connect it to the ruby ​​project.
    require 'fiddle'
    wiringpi = Fiddle.dlopen('/home/pi/wiringPi/wiringPi/libwiringPi.so.2.25')
    

    Now you can directly call all the functions from this library in the form as designed by the developer.
    Fiddle is a standard Ruby module that uses the standard * nix mechanism of libffi (Foreign Function Interface Library).
    Since I do not need all the library functions, I select the necessary ones and register only them:

    Select what is needed in the wiringPiI2C.h file
    extern int wiringPiI2CSetup          (const int devId) ;
    extern int wiringPiI2CWriteReg8 (int fd, int reg, int data) ;
    


    And we connect in the program:
    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)
    

    Parameters are the name of the function, an array of accepted parameters, and the return value. If pointers are passed, then, regardless of their type, they are taken equal to Fiddle :: TYPE_VOIDP

    This is how the connected function is called:
    @fd = @i2c_setup.call 0x68 #адрес устройства на шине I2C. Берется в мануале или с помощью утилиты i2cdetect.
    @i2c_write_reg8.call @fd, 0x6B, 0x00 # пишем в устройство, в регистр 0x6B значение 0. В данном случае – это вывод из спящего режима.
    

    That's all, I made the MPU6050 class, in the constructor of which I declare all the functions I need, and the measure function, which returns data on the board rotation using a little Kalman magic.
    Full class code for working with the accelerometer
    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
    


    This approach is fully justified when there are no strict time limits. That is, when it comes to milliseconds. But when it comes to microseconds, you have to use C-code inserts in the program. Otherwise, it just does not have time.

    It happened with a range finder , its principle of operation is to send a signal of the beginning of measurements in 10 microseconds, measure the length of the reverse pulse, divide by a factor to get the distance in centimeters.
    Class for measuring distance
    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 ''
        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
    


    Minimum server:
    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
    

    What people will not do in order not to write in python ...
    There are many alternative options for solving the problem, but I’m more interested in my own.
    In theory, there is a library that is just needed for working with wiringPi in Ruby, but at the time of publication it does not support the work of RaspberryPi of the second model.
    There is also a handy Ruby wrapper for the libffi engine with clear DSL and handling of all exceptions.

    Visualization


    Ajax request every 100ms and mapping using Raphael. Strictly speaking, this is not Raphael itself, but its extension for working with three-dimensional objects.
        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);
            }
        }
    

    In conclusion, I can say that I am fascinated by modern opportunities. Work with the I2C bus and Javascript are at different poles of technology. The gulf between hardware development, 3D-graphics and Javascript is not such an abyss, even if this is not done by a programmer at all, but just the opposite, a manager like me. Smoking manuals, multiplied by an abundance of documentation, makes itself felt.

    PS
    I took all the glands in Minsk hackerspace , the full project code can be found here .

    Also popular now: