Pull to refresh

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

DIY
Sandbox
Добрый день, хочу поделиться своим методом работы с инфракрасными дальномерами 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 отмечаю, что код тестировался вот на такой установке:



Спасибо за внимание.
Tags:ардуинодальномерробототехникаdiy или сделай самdiy
Hubs: DIY
Total votes 22: ↑18 and ↓4+14
Views21K

Popular right now

Top of the last 24 hours