Инфракрасный датчик расстояния от SmartBricks

Не секрет, что юные робототехники страны (и их педагоги) готовятся к традиционным весенним соревнованиям. Зная это, компания SmartBricks желает им помочь.

Первое, что они сделали - заключили соглашение с всероссийским комитетом WRO и добились разрешения на использование световых датчиков собственного производства в отборочных этапах МСР. Второе - это выпуск нового датчика, который все желающие могут использоваться для более точного определения расстояний.

Встречайте, инфракрасный датчик расстояния:


Датчик и по виду, и по принципу своей работы очень похож на датчики освещенности: светодиод испускает пучок волн - невидимый человеческому глазу свет в инфракрасном диапазоне, фотоэлемент пытается определить, какая часть волн отразилась от препятствия. Кстати, желающие могут все-таки попытаться увидеть свет, генерируемый любым инфракрасным светодиодом, - просто наведите на него объектив цифрового аппарата или мобильного телефона.

Датчик способен довольно точно определять расстояние в диапазоне от 5 мм до 10 см, что дает ему преимущество, как перед ультразвуковым датчиком расстояния, так и перед световыми датчиками, если пытаться их использовать для определения расстояния (это было описано здесь и здесь):

• Ультразвуковой датчик не сможет дать результат при маленьких расстояниях (меньше 6-7 см.)
• Световой датчик будет бесполезен при расстояниях больших 3-4 см.

Данное устройство робототехники могут использовать, например, в Международных Состязаниях Роботов в номинации Лабиринт: робот с помощью него сможет ехать вдоль стенки все время на одинаковом расстоянии или определять, есть сбоку от робота проход или нет.


Другой вариант, считывать просто показания окружающего инфракрасного излучения, чтобы найти наиболее горячий предмет. Тогда можно например создать робота для творческой категории WRO 2013 - охраняющего от пожара леса природных парков или деревянные церкви Спасо-Преображенского погоста на острове Кижи.


Еще один вид состязаний, где робот с таким датчиком сможет принимать участие - состязания роботов-пожарных, проходящее в Волгодонске:


Данный датчик - цифровой - работает по I2C шине. Традиционно, компания заботиться о своих клиентах и выкладывает готовый блок для среды NXT-G, с помощью которого можно опрашивать этот датчик, и пример программы с его использованием.

NiNoXT блог в свою очередь выкладывает в свободный доступ модули для языка NXC и для языка Python, которые реализуют возможность работы с инфракрасным датчиком расстояния SmartBricks из этих языков. Эти модули создавались по той же схеме, как были описаны в статье про способы программирования I2C датчиков.

Желающие могут ознакомиться с кодом ниже, чтобы закрепить принципы работы с I2C шиной.

Функции и константы для работы с инфракрасным датчиком расстояния в NXC:

#define I2CAddr_SBIRdist        0x4c
#define regIRdistData           0x42
#define IRdistDataLength        4
#define regIRdistAmbient        0x42
#define regIRdistReflected      0x44

sub SetSensorSBIRdist(const byte port) {
    SetSensorLowspeed(port);
}

unsigned byte SensorSBIRdistValue(const byte port) {
    byte WriteBuf[] = {I2CAddr_SBIRdist, regIRdistData};
    byte ReadBuf[];

    bool fSuccess;
    byte cLen = IRdistDataLength;
    
    unsigned byte value = 255;

    while (I2CCheckStatus(port) ==  STAT_COMM_PENDING);
    fSuccess = I2CBytes(port, WriteBuf, cLen, ReadBuf);
    if (fSuccess) {
        value = 5500/(ReadBuf[2]+ReadBuf[3]*256-ReadBuf[0]+ReadBuf[1]*256);
    }

    return value;
}

unsigned int SensorSBIRdistAmbientRawValue(const byte port) {
    byte WriteBuf[] = {I2CAddr_SBIRdist, regIRdistAmbient};
    byte ReadBuf[];

    bool fSuccess;
    byte cLen = 2;

    unsigned int value = 0;

    while (I2CCheckStatus(port) ==  STAT_COMM_PENDING);
    fSuccess = I2CBytes(port, WriteBuf, cLen, ReadBuf);
    if (fSuccess) {
        value = ReadBuf[0]+ReadBuf[1]*256;
    }

    return value;
}

unsigned int SensorSBIRdistReflectedRawValue(const byte port) {
    byte WriteBuf[] = {I2CAddr_SBIRdist, regIRdistReflected};
    byte ReadBuf[];

    bool fSuccess;
    byte cLen = 2;

    unsigned int value = 0;

    while (I2CCheckStatus(port) ==  STAT_COMM_PENDING);
    fSuccess = I2CBytes(port, WriteBuf, cLen, ReadBuf);
    if (fSuccess) {
        value = ReadBuf[0]+ReadBuf[1]*256;
    }

    return value;
}

task main() {
    SetSensorSBIRdist(S1);
    
    unsigned byte val;
  
    while(! ButtonPressed(BTNCENTER)) {
        ClearScreen();
        val = SensorSBIRdistValue(S1);
        NumOut(0, LCD_LINE1, SensorSBIRdistValue(S1));
        Wait(500);
    }

    Wait(1000);

    while(! ButtonPressed(BTNCENTER)) {
        ClearScreen();
        val = SensorSBIRdistValue(S1);
        NumOut(0, LCD_LINE2, SensorSBIRdistAmbientRawValue(S1));
        Wait(500);
    }
}

Для Python класс, предоставляющий методы аналогичные функциям выше, будет выглядеть так:

from .common import *
from .digital import BaseDigitalSensor
 
class IRDist(BaseDigitalSensor):
    """SMARTBRICKS IR distance sensor."""
    I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
    I2C_ADDRESS.update({
        'AmbientLight': (0x42, '2B'),
        'ReflectedLight': (0x44, '2B'),
        'all_data': (0x42, '4B'),
    })
    I2C_DEV = 0x4C
 
    # To be divided by data from sensor
    _SCALE_CONSTANT = 5500
 
    def __init__(self, brick, port, check_compatible=True):
        super(IRDist, self).__init__(brick, port, check_compatible)
 
    def get_distance(self):
        """This method returns a distance to an obstacle in millimeter.
        The sensor specific allows to rely on distance in range
        of 0.5 cm. to 10 cm. 
        """
        AL_l, AL_h, LIR_l, LIR_h = self.read_value('all_data')
 
        try:
            result = self._SCALE_CONSTANT / (LIR_l + LIR_h*256 - (AL_l + AL_h*256))
            return result
 
        except ZeroDivisionError:
            return 255
 
    def get_reflected(self):
        """This method returns raw value (0..1023) of amount of
        reflected IR light. 
        """
        low_byte, high_byte = self.read_value('ReflectedLight')
 
        return low_byte + high_byte*256
 
    def get_ambient(self):
        """This method returns raw value (0..1023) of amount of
        ambient IR light. 
        """
        low_byte, high_byte = self.read_value('AmbientLight')
 
        return low_byte + high_byte*256
 
    get_sample = get_distance
 
IRDist.add_compatible_sensor(None, 'SMBRCKS', 'IRDIST')

Если теперь его объединить с классом для работы с датчиком линии из прошлой статьи и прописать в Python27\Lib\site-packages\nxt\sensor\__init__.py строки:

import smartbricks
SBLine = smartbricks.Line
SBIRDist = smartbricks.IRDist

То работать с этими датчиками можно как со стандартными модулями из поставки Python. Например, методы нового класса можно использовать вот так:

from nxt.locator import find_one_brick
from nxt.sensor import *
from time import sleep
 
br=find_one_brick(debug=True)
 
irdist = SBIRDist(br, PORT_1)
 
i = 0
while(i<5):
    print irdist.get_distance()
    i = i+1
    sleep(1)
 
i = 0
while(i<5):
    print irdist.get_ambient()
    i = i+1
    sleep(1)