Работа с инфракрасным дальномером Sharp

Добрый день, хочу поделиться своим методом работы с инфракрасными дальномерами Sharp на примере модели Sharp GP2D120 (4 — 40 см).

Я подробно опишу составление обрабатывающей функции и дам пример применения. Преимущество этой функции в том, что она позволяет использовать весь рабочий диапазон любого дальномера.

Ваш робот больше не будет терять препятствия.

1 — Читаем документацию

У каждого датчика уже есть замеренная вольт-амперная характеристика, которая находится по названию модели. Учитывая специфику датчиков, производитель поставил в соответствие выходному напряжению расстояние до препятствия:

Определяем по графику напряжение, соответствующее каждому расстоянию и записываем в два массива:

float voltage[] = {2.7, 2.34, 2.00, 1.78, 1.56, 1.4, 1.265, 1.06, 0.92, 0.8, 0.74, 0.66, 0.52, 0.42, 0.36, 0.32};
float distanse[] = {4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 12.0, 14.0, 16.0, 18.0, 20.0, 25.0, 30.0, 35.0, 40.0};
int len = 16;  // количество набранных в массив точек  

2 — Реализуем интерполяцию

Вначале несколько раз (10) опросим датчик и возьмем среднее значение, чтобы избежать случайного шума:

int senPin = 0; // "Analog In", куда подключен датчик
int out = 0;
for (int i = 0; i < 10; i++){
   out += analogRead(senPin);
   delay(8);   // задержка получена опытным путём
}
out = out/10;

Зная, что в Ардуине analogRead() выдает значения от 0 до 1023, а датчики Sharp — от 0 до 5 Вольт, преобразуем out в вольты и найдем положение этого значения в массиве voltage:

int pos = 0;
float volt_read = out * 0.00488758553;  //  5 / 1023 = 0.00488758553
if((voltage[0] > volt_read) && (voltage[N-1] < volt_read)){
    // убедились, что показания принадлежат рабочей области  
    for (int i = 0; i < N-1; i++) { 
        if(volt_read >= voltage[i+1]){
            pos = i;
            break; 
        }
    }
}

Предпоследний шаг.

Интерполировав график, рассчитываем расстояние соответствующее выходному вольтажу.

float y1 = voltage[pos];
float x1 = distance[pos];
float y2 = voltage[pos + 1];
float x2 = distance[pos + 1];
float distance_out = (x2 - x1)*(y1 - volt_read)/(y1 - y2) + x1;  // следствие из подобия треугольников 123 и 145 

3 — Собираем функцию

float getDistance(int senPin){
    float voltage[] = {2.7, 2.34, 2.00, 1.78, 1.56, 1.4, 1.265, 1.06, 0.92, 0.8, 0.74, 0.66, 0.52, 0.42, 0.36, 0.32};
    float distanse[] = {4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 12.0, 14.0, 16.0, 18.0, 20.0, 25.0, 30.0, 35.0, 40.0};
    int len = 16; int out = 0; int pos = 0;

    for (int i = 0; i < 10; i++){
       out += analogRead(senPin);
       delay(8);   
    }
    out = out/10;
    
    float volt_read = out * 0.00488758553;  //  5 / 1023 = 0.00488758553
    if((voltage[0] > volt_read) && (voltage[N-1] < volt_read)){
        for (int i = 0; i < N-1; i++) { 
            if(volt_read >= voltage[i+1]){
                pos = i;
                break; 
            }
        }
        float y1 = voltage[pos];
        float x1 = distance[pos];
        float y2 = voltage[pos + 1];
        float x2 = distance[pos + 1];
        float distance_out = (x2 - x1)*(y1 - volt_read)/(y1 - y2) + x1;  
        return distance_out
    }
    else return -1.0;
}

4 — Правильно используем

Опытным путем я заметил, что функция лучше всего работает, если запускать ее с промежутком в 15 миллисекунд для любого количества дальномеров:

void updateDistance{
    float D1 = getDistance(0);
    delay(15);
    // ...
    float D6 = getDistance(5);
    delay(15);
}

Для ценителей DIY отмечаю, что код тестировался вот на такой установке:

Спасибо за внимание.

Автор: deQU

Источник