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 'скрипка некваліфікований робітник'
wiringpi = скрипка некваліфікований робітник.dlopen('/home/pi/wiringPi/wiringPi/libwiringPi.so.2.25')

Тепер можна безпосередньо викликати всі функції з цієї бібліотеки в тому вигляді, як їх задумував розробник.
Скрипка некваліфікований робітник – це стандартний модуль 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 = скрипка некваліфікований робітник::TYPE_INT
@i2c_setup = скрипка некваліфікований робітник::Function.new(wiringpi['wiringPiI2CSetup'], [int], int)
@i2c_write_reg8 = скрипка некваліфікований робітник::Function.new(wiringpi['wiringPiI2CWriteReg8'], [int, int, int], int)

Параметри це – ім'я функції, масив прийнятих параметрів і значення, що повертається. Якщо передаються покажчики, то, незалежно від їх типу, вони приймаються рівними скрипка некваліфікований робітник::TYPE_VOIDP

Ось так відбувається виклик підключеної функції:
@fd = @i2c_setup.call 0x68 #адреса пристрою на шині I2C. Береться в мануалі або за допомогою утиліти i2cdetect.
@i2c_write_reg8.call @fd, 0x6B, 0x00 # пишемо в пристрій, у регістр 0x6B значення 0. В даному випадку – це висновок із сплячого режиму.

Ось власне і все, я зробив клас MPU6050, в конструкторі якого я оголошую всі потрібні мені функції, і функцією measure, яка повертає дані про поворот плати, використовуючи трохи магії Калмана.
Повний код класу для роботи з акселерометром
require 'скрипка некваліфікований робітник'

class MPU6050
attr_reader :last_x, :last_y, :k
def initialize(path_to_wiring_pi_so)
wiringpi = скрипка некваліфікований робітник.dlopen(path_to_wiring_pi_so)

int = скрипка некваліфікований робітник::TYPE_INT
char_p = скрипка некваліфікований робітник::TYPE_VOIDP

# int wiringPiI2CSetup (int devId) ;
@i2c_setup = скрипка некваліфікований робітник::Function.new(wiringpi['wiringPiI2CSetup'], [int], int)

# int wiringPiI2CSetupInterface (const char *device, int devId) ;
@i2c_setup_interface = скрипка некваліфікований робітник::Function.new(wiringpi['wiringPiI2CSetupInterface'], [char_p, int], int)

# int wiringPiI2CRead (int fd) ;
@i2c_read = скрипка некваліфікований робітник::Function.new(wiringpi['wiringPiI2CRead'], [int], int)

# int wiringPiI2CWrite (int fd, int data) ;
@i2c_write = скрипка некваліфікований робітник::Function.new(wiringpi['wiringPiI2CWrite'], [int, int], int)

# int wiringPiI2CWriteReg8 (int fd, int reg, int data) ;
@i2c_write_reg8 = скрипка некваліфікований робітник::Function.new(wiringpi['wiringPiI2CWriteReg8'], [int, int, int], int)

# int wiringPiI2CWriteReg16 (int fd, int reg, int data) ;
@i2c_write_reg8 = скрипка некваліфікований робітник::Function.new(wiringpi['wiringPiI2CWriteReg16'], [int, int, int], int)

# int wiringPiI2CReadReg8 (int fd, int reg) ;
@i2c_read_reg8 = скрипка некваліфікований робітник::Function.new(wiringpi['wiringPiI2CReadReg8'], [int, int], int)

# int wiringPiI2CReadReg16 (int fd, int reg) ;
@i2c_read_reg16 = скрипка некваліфікований робітник::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 'скрипка некваліфікований робітник'
require 'inline'

class HCSRO4
IN = 0
OUT = 1

TRIG = 17
ECHO = 27

def initialize(path_to_wiring_pi_so)
wiringpi = скрипка некваліфікований робітник.dlopen(path_to_wiring_pi_so)

int = скрипка некваліфікований робітник::TYPE_INT
void = скрипка некваліфікований робітник::TYPE_VOID

# extern int wiringPiSetup (void) ;
@setup = скрипка некваліфікований робітник::Function.new(wiringpi['wiringPiSetup'], [void], int)

# extern int wiringPiSetupGpio (void) ;
@setup_gpio = скрипка некваліфікований робітник::Function.new(wiringpi['wiringPiSetupGpio'], [void], int)

# extern void pinMode (int pin, int mode) ;
@pin_mode = скрипка некваліфікований робітник::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, перегляду;
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.
Всі залізяки я брав в Мінському хакерспейсе, повний код проекту можна подивитися тут.

Джерело: Хабрахабр

0 коментарів

Тільки зареєстровані та авторизовані користувачі можуть залишати коментарі.