Подключаем лидар Delta2B к ESP32

Общие сведения:

Лидар 3irobotix Delta2B является электро-механическим лазерным радаром, замеряет расстояния на 360° вращая оптическую сборку вокруг своей оси посредством ременной передачи. Данные передаются только в одну сторону, лидар посылает данные о расстоянии и градусах, но не принимает никаких данных. Для передачи используется только вывод TX лидара подключенный к выводу RX esp32.

Спецификация:

• Питание двигателя: 3,3 В постоянного тока; 
• Контроль скорости вращения: ШИМ на 5 В или регулировка напряжения;
• Допустимая скорость вращения: 4 - 10 об/с;
• Рабочее расстояние: 13 ... 800 см;
• Питание электроники: 5 В постоянного тока;
• Пусковой ток: 600 мА;
• Рабочий ток: 500 мА;
• Протокол передачи данных: симплексный rs-232, LSB, 8-N-1;
• Скорость передачи данных: 230400 бод.

Подключение:

Разъем на самом лидаре:

Подключение к Wemos D1 R32:

Подключаем лидар к разновидности ESP32 под названием Wemos D1 R32 при помощи комплектного шнура, проводов «папа-папа» и trema-модуля силовой ключ (для увеличения изображения кликните на него).

Описание пакета данных:

Расчеты:

Градус пробы n = стартовый угол + 22,5°*(n-1)/N

N = (длинна полезных данных - 5) / 3

Пример пакета данных:

AA 00 4F 00 61 AD 00 47 79 00 40 72 42 3C 05 6D 37 05 8A 3A 05 93 34 05 9C 35 05 AD 35 05 B8

35 05 C6 3505 D5 34 05 E5 36 05 F2 31 06 07 2D 06 16 2E 06 2B 2E 06 40 36 06 52 35 06 67 32

06 61 2D 06 45 2B 06 222B 06 03 31 05 DF 30 05 C6 DC 27

• AA: заголовок пакета 
• 004F: длинна пакета
• 00: версия протокола
• 61: тип пакета
• AD: заголовок данных
• 0047: длинна данных
• 79: скорость мотора = 121(0х79)*0,05 = 6,05 об/с
• 0040: сдвиг нулевого градуса: 64(0х40)*0,01°=0.64°
• 7242: стартовый угол данного пакета 29250*0,01°=292,5°
• 3C: уровень сигнала 1-й пробы
• 056D: расстояние 1-й пробы, 1389*0,25мм=347,25мм
• 37: уровень сигнала 2-й пробы
• 058A: расстояние второй пробы, 1418*0,25мм=354,5мм
• .............
• 30: уровень сигнала N-ой пробы
• 05C6: расстояние N-ой пробы, 1478*0,25мм=369,5мм
• DC27: контрольная сумма

Пример:

Запись расстояний от 0° до 360° в массив lidar_data[360]

#define MOTOR_MOSFET_PIN      19    // определяем вывод для управления ключом мотора
// из datasheet:
#define FRAME_HEADER          0XAA  // определяем заголовок пакета
#define DATA_HEADER           0xAD  // определяем значение байта начала данных
#define SIZE_HI_BYTE          1     // определяем место в пакете старшего байта размера данных без checksum
#define SIZE_LO_BYTE          2     // определяем место в пакете младшего байта размера данных без checksum
#define DATA_START_BYTE       5     // определяем место в пакете байта начала полезных данных
#define DISTANCE_DATA_SYZE    7     // определяем место в пакете размера данных расстояния
#define START_ANGLE_HI_BYTE   11    // определяем место старшего байта стартового угла пакета
#define START_ANGLE_LO_BYTE   12    // определяем место младшего байта стартового угла пакета
#define FIRST_DIST_HI_BYTE    14    // определяем место старшего байта первой пробы расстояния в пакете
#define FIRST_DIST_LO_BYTE    15    // определяем место младшего байта первой пробы расстояния в пакете
// конец datasheet

float lidar_data[360];              // массив для хранения расстояний

uint8_t raw_data[256];              // буфер для хранения "сырых" данных
bool data_ready = false;            // флаг готовности данных
long lidar_delay = 1000;            // интервал вывода в серийный порт
long current_millis = 0;            // текущие millis() для сравнения

void setup() {

    Serial.begin(115200);           // инициируем работу с нулевым серийным портом для вывода в серийный монитор
    Serial1.begin(230400);          // инициируем работу с первым серийным портом для чтения данных лидара
    pinMode(
            MOTOR_MOSFET_PIN,
            OUTPUT
            );                      // устанавливаем вывод ключа мотора в режим выход
    digitalWrite(
            MOTOR_MOSFET_PIN,
            HIGH);                  // устанавливаем вывод ключа мотора в режим логической единицы
    memset(raw_data, 0, 256);       // обнуляем буфер
}

void loop() {

    // если прошёл интервал ожидания и данные готовы
    if(millis() - current_millis > lidar_delay && data_ready) {
        for(int i = 0; i < 360; i++) {

            // выводим текст в серийный монитор
            Serial.print("Angle: ");

            // добавляем пробел (для красоты вывода)
            // если угол меньше 10
            if(i < 10){
                Serial.print(" ");
            }
            // выводим индекс массива
            Serial.print(i);

            // выводим данные в серийный монитор
            Serial.print("\tDistance: ");
            Serial.println(lidar_data[i]);

            // записываем текущие millis
            current_millis = millis();

            // обнуляем флаг готовности данных
            data_ready = false;
        }
    }

    // пока есть данные для чтения из серйиного порта №1
    while(Serial1.available()) {

        // читаем стартовый байт пакета
        if (Serial1.read() == (raw_data[0] = FRAME_HEADER)){
            for (int i = 1; i!= 256; i++){

                // заполняем буфер
                raw_data[i] = Serial1.read();
            }
        }
    }

    // если байт указывающий на данные совпадает с заголовком пакета
    if(raw_data[DATA_START_BYTE] == DATA_HEADER) {

        // берем длинну полезных данных (2-й и 3-й байт сырых данных)
        uint16_t raw_data_length = (raw_data[SIZE_HI_BYTE] << 8) + raw_data[SIZE_LO_BYTE];

        // берем контрольную сумму из сырых данных
        uint16_t checksum = (raw_data[raw_data_length] << 8) + raw_data[raw_data_length+1];
        delay(10);

        // вызываем функцию подсчета контрольной суммы
        if (checksum == checksum_cmp(raw_data, raw_data_length)) { // если сумма прошла

            // подсчет стартового угла данного пакета
            float start_angle = ((raw_data[START_ANGLE_HI_BYTE] << 8)
                                    + raw_data[START_ANGLE_LO_BYTE])*0.01;

            // подсчет кол-ва проб расстояния: кол-во проб = (размер данных расстояния - 5) /  3
            uint8_t read_count = (raw_data[DISTANCE_DATA_SYZE] - 5) /  3;
            for (int n = 0; n < read_count; n++){

                // подсчет угла каждой пробы:
                // угол = стартовый угол + 22,5° * (номер пробы - 1)/ кол-во проб
                float angle = start_angle + 22.5 * (n - 1) / read_count;

                // итератор для массива lidar_data
                int i = int(angle);

                // подсчет расстояния
                float distance = ((raw_data[FIRST_DIST_HI_BYTE+n*3] << 8)
                                    + raw_data[FIRST_DIST_LO_BYTE+n*3]) * 0.25;

                // запись расстояния в массив
                lidar_data[i] = distance;
            }
        }
        data_ready = true;
    }
}

// функция вычисления контрольной суммы.
uint16_t checksum_cmp(uint8_t *raw_data, uint16_t raw_data_length) {
    uint16_t _checksum = 0;
    while (raw_data_length--) {
        // складываем все байты пакета
        _checksum += *raw_data++;
    }
    return _checksum;
}

Применение

• роботы
• сигнализация
• интерактивные системы
• планировка помещений

Ссылки

• Лидар 3irobotix DeltaB2
• Провода «папа-папа»
• Trema модуль силовой ключ
• Программа для подключения к ПК