Line Follower помогите адаптировать

Войдите на сайт для отправки комментариев

Чт, 01/03/2018 - 20:25

al072

Offline

Зарегистрирован: 20.04.2017

Добрый день! Уважаемые господа, нашел исходники проекта "Line follower" на данном ресурсе: https://lesson.iarduino.ru/page/urok-33-obuchaem-arduino-robota-ezdit-po-linii/

У меня проект с 2-мя датчиками света вместо 3-х и мой робот 4-х колесный (на каждое колесо по мотору, скорость управляется через ШИМ). Помогите пожалуйста адаптировать данный код для 2-х датчиков света:

//                   БИБЛИОТЕКИ:
#include             <iarduino_HC_SR04_int.h>                       // подключаем библиотеку для работы с датчиком
//                   НОМЕРА ВЫВОДОВ:
const uint8_t        pinSensorL   = A3;                             // Вывод к которому подключен датчик находящийся слева         (по направлению движения)
const uint8_t        pinSensorC   = A4;                             // Вывод к которому подключен датчик находящийся по центру     (по направлению движения)
const uint8_t        pinSensorR   = A5;                             // Вывод к которому подключен датчик находящийся справа        (по направлению движения)
const uint8_t        pinSens_TRIG = 2;                              // Вывод к которому подключен датчик расстояния HC_SR04        (вывод обозначенный на датчике как TRIG)
const uint8_t        pinSens_ECHO = 3;                              // Вывод к которому подключен датчик расстояния HC_SR04        (вывод обозначенный на датчике как ECHO)
const uint8_t        pinShield_LH = 7;                              // Вывод направления к которому подключен левый мотор          (по направлению движения)
const uint8_t        pinShield_LE = 6;                              // Вывод ШИМ         к которому подключен левый мотор          (по направлению движения)
const uint8_t        pinShield_RE = 5;                              // Вывод ШИМ         к которому подключен левый мотор          (по направлению движения)
const uint8_t        pinShield_RH = 4;                              // Вывод направления к которому подключен левый мотор          (по направлению движения)
//                   ОБЪЕКТЫ:
iarduino_HC_SR04_int hcsr(pinSens_TRIG, pinSens_ECHO);              // Объект hcsr для работы с библиотекой iarduino_HC_SR04       (вывод TRIG, вывод ECHO)
//                   УСТАНАВЛИВАЕМЫЕ ЗНАЧЕНИЯ:
const uint16_t       valSensor1   = 930;                            // Показание датчика находящегося на линии                     (указывается для конкретной трассы)
const uint16_t       valSensor0   = 730;                            // Показание датчика находящегося вне линии                    (указывается для конкретной трассы)
const uint8_t        valSpeed     = 255;                            // Максимальная скорость                                       (число от 1 до 255)
const uint32_t       tmrDelay     = 2000;                           // Время в течении которого требуется остановиться             (если в течении этого времени состояние остаётся неопределённым (линия не обнаружена), то требуется остановиться)
const uint8_t        valTurning   = 10;                             // Крутизна поворотов (скорость реакции)                       (число от 1 до 255)
const uint8_t        valDistance  = 20;                             // Минимально допустимое расстояние до объекта в сантиметрах   (если расстояние будет меньше, то требуется остановитьтся)
const bool           arrRoute[2]  = {1,1};                          // Направление движения для каждого мотора                     (зависит от полярности,  нулевой элемент - правый мотор, первый элемент - левый мотор)
//                   РАССЧИТЫВАЕМЫЕ ЗНАЧЕНИЯ:
      uint8_t        arrSpeed[2];                                   // Рассчитываемая скорость для каждого мотора                  (число от 1 до valSpeed, нулевой элемент - правый мотор, первый элемент - левый мотор)
      uint16_t       valSensorM;                                    // Рассчитываемое среднее значение датчика                     (значение между цветом линии и цветом вне линии)
      uint8_t        valSensor;                                     // Биты рассчитываемых логических уровней всех датчиков        (0bxxxxxLCR)
      bool           flgLine;                                       // Флаг указывающий на то, что используется светлая линия      (0 - тёмная линия, 1 - светлая линия)
      int8_t         flgTurning;                                    // Флаг наличия и направления поворота                         (0 - не поворачиваем, -1 - поворачиваем налево, +1 - поворачиваем направо)
      bool           flgPWM;                                        // Флаг указывающий на то, что требуется изменить ШИМ моторов  (0 - тёмная линия, 1 - светлая линия)
      bool           flgStop;                                       // Флаг указывающий на необходимость остановиться              (0 - без остановки, 1 - требуется остановиться)
      bool           flgDistance;                                   // Флаг обнаружения препятствия                                (0 - не обнаружено, 1 - обнаружено)
      uint32_t       tmrMillis;                                     // Время совершения последней операции                         (в миллисекундах)
void setup(){
//  Узнаём цвет линии используемой на трассе, если он светлый, то устанавливаем флаг lineColor тёмный
    flgLine = (valSensor0>valSensor1);                              // Если условие (valSensor0>valSensor1) выполняется значит линия светлая и флаг flgLine установится в 1, иначе он сбросится в 0
//  Вычисляем среднее значение между показаниями датчиков на линии и все линии
    if(flgLine){valSensorM=valSensor1+(valSensor0-valSensor1)/2;}   // Если на трассе используется светлая линия
    else       {valSensorM=valSensor0+(valSensor1-valSensor0)/2;}   // Если на трассе используется тёмная линия
//  Устанавливаем значение скорости обоих моторов
    arrSpeed[1]=valSpeed;                                           // Максимальная скорость на левом моторе
    arrSpeed[0]=valSpeed;                                           // Максимальная скорость на правом моторе
//  Устанавливаем флаг ШИМ, сбрасываем флаг наличия поворота, флаг остановки и флаг обнаружения припятствий
    flgPWM=1; flgTurning=0; flgStop=0; flgDistance=0;
//  Устанавливаем режим работы выводов и направление обоих моторов
    pinMode     (pinSensorL,   INPUT      );                        // Конфигурируем вывод pinSensorL   как вход                   (для получения данных от левого датчика линии)
    pinMode     (pinSensorC,   INPUT      );                        // Конфигурируем вывод pinSensorC   как вход                   (для получения данных от центрального датчика линии)
    pinMode     (pinSensorR,   INPUT      );                        // Конфигурируем вывод pinSensorR   как вход                   (для получения данных от правого датчика линии)
    pinMode     (pinShield_LH, OUTPUT     );                        // Конфигурируем вывод pinShield_LH как выход                  (для управления направлением движения левого мотора)
    pinMode     (pinShield_LE, OUTPUT     );                        // Конфигурируем вывод pinShield_LE как выход                  (для управления скоростью вращения левого мотора, при помощи ШИМ)
    pinMode     (pinShield_RE, OUTPUT     );                        // Конфигурируем вывод pinShield_RE как выход                  (для управления скоростью вращения правого мотора, при помощи ШИМ)
    pinMode     (pinShield_RH, OUTPUT     );                        // Конфигурируем вывод pinShield_RH как выход                  (для управления направлением движения правого мотора)
    digitalWrite(pinShield_LH, arrRoute[1]);                        // Устанавливаем на выходе pinShield_LH уровень arrRoute[1]    (направление движения левого мотора)
    digitalWrite(pinShield_RH, arrRoute[0]);                        // Устанавливаем на выходе pinShield_RH уровень arrRoute[0]    (направление движения правого мотора)
//  Выводим показания центрального датчика линии
    Serial.begin(9600); while(!Serial){}                            // Инициируем передачу данных по последовательному порту       (на скорости 9600 бит/сек)
    Serial.println(analogRead(pinSensorC));                         // Выводим показания центрального датчика линии                (для указания значений константам valSensor0 и valSensor1)
//  Устанавливаем задержку и обновляем время совершения последней операции
    delay(2000);
    tmrMillis = millis();
}
void loop(){
//  Читаем показания датчиков и преобразуем их в логические уровни  // (1 - датчик на линии, 0 - датчик вне линии)
    valSensor  =  0;                                                // сбрасываем все биты переменной valSensor
    valSensor |= ((analogRead(pinSensorL)>valSensorM)^flgLine)<<2;  // Устанавливаем 2 бит переменной valSensor в 1 если левый   датчик находится на линии, иначе оставляем бит сброшенным в 0
    valSensor |= ((analogRead(pinSensorC)>valSensorM)^flgLine)<<1;  // Устанавливаем 1 бит переменной valSensor в 1 если средний датчик находится на линии, иначе оставляем бит сброшенным в 0
    valSensor |= ((analogRead(pinSensorR)>valSensorM)^flgLine)<<0;  // Устанавливаем 0 бит переменной valSensor в 1 если правый  датчик находится на линии, иначе оставляем бит сброшенным в 0
                                                                    // РАССМОТРИМ ТРИ ПРЕДЫДУЩИЕ СТРОКИ: Каждая строка устанавливает или сбрасывает свой бит переменной valSensor в зависимости от того, находится датчик на линии или нет.
                                                                    // Оператор составного побитового ИЛИ "|=" выполнит побитовое ИЛИ между переменной valSensor и результатом всех вычислений, полученное значение запишется в valSensor.
                                                                    // Оператор сравнения ">" вернет "1" если значение analogRead(номер_вывода) больше чем значение valSensorM, а значит датчик находится над объектом, который темнее чем значение valSensorM,
                                                                    // Результат возвращённый оператором сравнения ">" нам подходит если используется тёмная линия, но если используется светлая линия, то результат нужно инвертировать ...
                                                                    // Оператор побитового XOR "^" выполнит эту инверсию, только если установлен флаг flgLine, указывающий о том, что используется светлая линия
                                                                    // Оператор побитового сдвига влево "<<" сдвинет результат на указанное число бит, таким образом каждый результат займет свою позицию.
//  Определяем действия в соответствии с текущим положением датчиков
    switch(valSensor){ //            Сохраняем время:    Меняем флаг ШИМ:   Меняем флаг поворота:  Меняем флаг остановки:
        case 0b000:                                      flgPWM=flgTurning;                        flgStop=!flgTurning; break;  // 000 - Если все датчики находятся вне линии                 (неопределённое состояние - если до этого мы не поворачивали, то резко останавливаемся, а если поворачивали, то продолжаем поворот в ту же сторону)
        case 0b010:                  tmrMillis=millis(); flgPWM=flgTurning; flgTurning=0;          flgStop=0;           break;  // 010 - Если только центральный датчик находится на линии    (продолжаем движение прямо)
        case 0b100:                                                                                                             // 100 - Если только левый датчик находится на линии          (поворачиваем влево)
        case 0b110:                  tmrMillis=millis(); flgPWM=1;          flgTurning=-1;         flgStop=0;           break;  // 110 - Если левый и центральный датчики находятся на линии  (поворачиваем влево)
        case 0b001:                                                                                                             // 001 - Если только правый датчик находится на линии         (поворачиваем вправо)
        case 0b011:                  tmrMillis=millis(); flgPWM=1;          flgTurning=1;          flgStop=0;           break;  // 011 - Если правый и центральный датчики находятся на линии (поворачиваем вправо)
        default:                                         flgPWM=1;                                                      break;  // 1x1 - Если правый и левый датчики находятся на линии       (неопределённое состояние)
    }
    if(tmrMillis>millis())         { tmrMillis=0;}                                                                              // Избавляемся от переполнения millis();
    if(tmrMillis+tmrDelay<millis()){                     flgPWM=1;          flgTurning=0;          flgStop=1;           }       // Останавливаемся если линия потеряна на более чем tmrDelay мс
    if(hcsr.distance()<valDistance){ tmrMillis=millis(); flgPWM=1;          flgDistance=1;                              }       // Останавливаемся если обнаружено препятствие
    else if(flgDistance)           { tmrMillis=millis(); flgPWM=1;          flgDistance=0;                              }       // Продолжаем движение если препятствие исчезло
//  Устанавливаем ШИМ для моторов
    if(flgPWM){flgPWM=0;                                                                                                        // Если установлен флаг flgPWM, то сбрасываем его и устанавливаем ШИМ ...
      switch(flgTurning){ // Скорость левого мотора:           Скорость правого мотора:
        case -1:             if(arrSpeed[1]>0){arrSpeed[1]--;} arrSpeed[0]=valSpeed;                                    break;  // Уменьшаем скорость левого  мотора (поворачиваем налево)
        case  0:             arrSpeed[1]=valSpeed;             arrSpeed[0]=valSpeed;                                    break;  // Устанавливаем одинаковую скорость (едим прямо)
        case  1:             arrSpeed[1]=valSpeed;             if(arrSpeed[0]>0){arrSpeed[0]--;}                        break;  // Уменьшаем скорость правого мотора (поворачиваем направо)
      } if(flgStop){         arrSpeed[1]=0;                    arrSpeed[0]=0;}                                                  // Останавливаемся если установлен флаг flgStop
        if(flgDistance){     arrSpeed[1]=0;                    arrSpeed[0]=0;}                                                  // Останавливаемся если установлен флаг flgDistance
//      Выводим ШИМ
        analogWrite(pinShield_LE, arrSpeed[1]);
        analogWrite(pinShield_RE, arrSpeed[0]);
    }
}

Я понимаю что изменения нужно вносить вот в этой части программы, но мозгов не хватает:

//  Читаем показания датчиков и преобразуем их в логические уровни  // (1 - датчик на линии, 0 - датчик вне линии)
    valSensor  =  0;                                                // сбрасываем все биты переменной valSensor
    valSensor |= ((analogRead(pinSensorL)>valSensorM)^flgLine)<<2;  // Устанавливаем 2 бит переменной valSensor в 1 если левый   датчик находится на линии, иначе оставляем бит сброшенным в 0
    valSensor |= ((analogRead(pinSensorC)>valSensorM)^flgLine)<<1;  // Устанавливаем 1 бит переменной valSensor в 1 если средний датчик находится на линии, иначе оставляем бит сброшенным в 0
    valSensor |= ((analogRead(pinSensorR)>valSensorM)^flgLine)<<0;  // Устанавливаем 0 бит переменной valSensor в 1 если правый  датчик находится на линии, иначе оставляем бит сброшенным в 0
                                                                    // РАССМОТРИМ ТРИ ПРЕДЫДУЩИЕ СТРОКИ: Каждая строка устанавливает или сбрасывает свой бит переменной valSensor в зависимости от того, находится датчик на линии или нет.
                                                                    // Оператор составного побитового ИЛИ "|=" выполнит побитовое ИЛИ между переменной valSensor и результатом всех вычислений, полученное значение запишется в valSensor.
                                                                    // Оператор сравнения ">" вернет "1" если значение analogRead(номер_вывода) больше чем значение valSensorM, а значит датчик находится над объектом, который темнее чем значение valSensorM,
                                                                    // Результат возвращённый оператором сравнения ">" нам подходит если используется тёмная линия, но если используется светлая линия, то результат нужно инвертировать ...
                                                                    // Оператор побитового XOR "^" выполнит эту инверсию, только если установлен флаг flgLine, указывающий о том, что используется светлая линия
                                                                    // Оператор побитового сдвига влево "<<" сдвинет результат на указанное число бит, таким образом каждый результат займет свою позицию.
//  Определяем действия в соответствии с текущим положением датчиков
    switch(valSensor){ //            Сохраняем время:    Меняем флаг ШИМ:   Меняем флаг поворота:  Меняем флаг остановки:
        case 0b000:                                      flgPWM=flgTurning;                        flgStop=!flgTurning; break;  // 000 - Если все датчики находятся вне линии                 (неопределённое состояние - если до этого мы не поворачивали, то резко останавливаемся, а если поворачивали, то продолжаем поворот в ту же сторону)
        case 0b010:                  tmrMillis=millis(); flgPWM=flgTurning; flgTurning=0;          flgStop=0;           break;  // 010 - Если только центральный датчик находится на линии    (продолжаем движение прямо)
        case 0b100:                                                                                                             // 100 - Если только левый датчик находится на линии          (поворачиваем влево)
        case 0b110:                  tmrMillis=millis(); flgPWM=1;          flgTurning=-1;         flgStop=0;           break;  // 110 - Если левый и центральный датчики находятся на линии  (поворачиваем влево)
        case 0b001:                                                                                                             // 001 - Если только правый датчик находится на линии         (поворачиваем вправо)
        case 0b011:                  tmrMillis=millis(); flgPWM=1;          flgTurning=1;          flgStop=0;           break;  // 011 - Если правый и центральный датчики находятся на линии (поворачиваем вправо)
        default:                                         flgPWM=1;                                                      break;  // 1x1 - Если правый и левый датчики находятся на линии       (неопределённое состояние)
    }

Заранее спасибо. Еще может я взял за основу не очень хороший пример, если есть более правильные примеры большая просьба поделитесь ссылочкой.

Чт, 01/03/2018 - 20:42

Arhat109-2

Offline

Зарегистрирован: 24.09.2015

Выбрасываете из программы всё, что относится к среднему датчику и дополнительно принимаете решение как будете определять что пора ехать прямо. Дополнительно - потому что ситуации "оба датчика сбоку от линии" (потеря линии) и "линия промеж датчиков" (посередине) - становятся алгоритмически неразличимы для 2-х датчиков.

Собственно все.

P.S. Вычисление числа для switch() можно и вовсе выкинуть и переделать про обыкновенные if(){}else{}. Алгоритм в вашем образце туп до безобразия: "какой датчик видит линию - тем колесом и тормозим".

Чт, 01/03/2018 - 21:30

kalapanga

Offline

Зарегистрирован: 23.10.2016

А мы удивляемся, откуда переполнение миллис несут. Оказывается из уроков...

Чт, 01/03/2018 - 22:34

ЕвгенийП

Offline

Зарегистрирован: 25.05.2015

kalapanga пишет:

А мы удивляемся, откуда переполнение миллис несут. Оказывается из уроков...

Блин, как Вы внимательно смотрите. Я бы и не заметил.

Чт, 01/03/2018 - 22:49

al072

Offline

Зарегистрирован: 20.04.2017

Arhat109-2 пишет:

Собственно все.

Спасибо за ответ. Убрал средний датчик. Посоветуйте пожалуйста как правильно определить эти две ситуации.

void loop(){

    valSensor |= ((analogRead(pinSensorL)>valSensorM)^flgLine)<<2;    
    valSensor |= ((analogRead(pinSensorR)>valSensorM)^flgLine)<<0; 
                                                                  

    switch(valSensor){ 
        case 0b000: flgPWM=flgTurning; flgStop=!flgTurning; break; 
        case 0b010: tmrMillis=millis(); flgPWM=flgTurning; flgTurning=0; flgStop=0; break; 
        case 0b100:                                                                                                           
        case 0b110: tmrMillis=millis(); flgPWM=1; flgTurning=-1;flgStop=0;break;  
        case 0b001:                                                                                                           
        case 0b011: tmrMillis=millis(); flgPWM=1; flgTurning=1; flgStop=0;break; 
        default: flgPWM=1;break; 
    }
    if(tmrMillis>millis()) { tmrMillis=0;}                                                                          
    if(tmrMillis+tmrDelay<millis()){flgPWM=1;flgTurning=0;flgStop=1; } 
    if(hcsr.distance()<valDistance){ tmrMillis=millis(); flgPWM=1; flgDistance=1; } 
    else if(flgDistance) { tmrMillis=millis(); flgPWM=1;flgDistance=0;}  

    if(flgPWM){flgPWM=0;                                                                                                      
      switch(flgTurning){ 
        case -1:             if(arrSpeed[1]>0){arrSpeed[1]--;} arrSpeed[0]=valSpeed;  break;  
        case  0:             arrSpeed[1]=valSpeed;arrSpeed[0]=valSpeed; break; 
        case  1:             arrSpeed[1]=valSpeed;if(arrSpeed[0]>0){arrSpeed[0]--;} break;  
      } if(flgStop){         arrSpeed[1]=0;  arrSpeed[0]=0;}                                                
        if(flgDistance){   arrSpeed[1]=0;arrSpeed[0]=0;}

Пт, 02/03/2018 - 07:12

Arhat109-2

Offline

Зарегистрирован: 24.09.2015

Правильно? Никак. Я же отписал что они "алгоритмически неразличимы". Нет способа, и тем более "правильно". Только добавление третьего датчика. :)

Пт, 02/03/2018 - 17:12

Arhat109-2

Offline

Зарегистрирован: 24.09.2015

Подумал-подумал и наверное есть способ различать эти ситуации на двух датчиках:

Если датчики поставить достаточно близко к линии, так, чтобы они ловили линию "краем", только слегка снижая показания, то ситуации станут различимы даже на двух датчиках:

1. Линия между датчиками - оба датчика показывают "не совсем белое", а несколько меньше.

2. Линия сбоку - оба датчика показывают "совсем белое" - достаточно много.

То есть, можно отслеживая аналоговые показания обнаружить что у датчика заходящего на линию показания стали уменьшаться, в то время как у датчика выходящего с линии - расти и в тот момент когда они выравниваются промеж себя - можно считать линию "посередине" (если оба датчика одинаковых, чего не бывает в природе, но можно прибегнуть к функции map()! ). В общем, ценой существенного усложнения кода вполне можно обойтись и двумя датчиками..

Ситуация потери линии, в этом случае, будет выглядеть совсем иначе: один датчик УЖЕ и давно кажет "совсем белое", а второй светлеет на глазах и тоже к совсем белому. Точно также можно сразу же и отслеживать в какой стороне справа или слева осталась линия - куда рулить.

Только этот код ни разу не применим в таком случае весь полностью. Впрочем, его и так правильнее выбросить за полной неграмотностью его автора .. пишете что это - "уроки"? Ну-ну.. :)

Пт, 02/03/2018 - 17:27

al072

Offline

Зарегистрирован: 20.04.2017

Arhat109-2 пишет:

Подумал-подумал и наверное есть способ различать эти ситуации на двух датчиках:

1. Линия между датчиками - оба датчика показывают "не совсем белое", а несколько меньше.

2. Линия сбоку - оба датчика показывают "совсем белое" - достаточно много.

Большое вам спасибо за развёрнутый ответ! Теперь стало понятно, что два датчика использовать не вариант. Тогда буду применять три датчика. Потребность именно в двух а не трёх датчиках была связана с целью экономии одного пина для дальномера HC SR-04, больше свободных пинов на моей Nano не осталось.., но похоже чём-то придётся пожертвовать. Посоветуйте пожалуйста хороший пример алгоритма для трёх датчиков с которого можно начать?

Пт, 02/03/2018 - 19:31

Arhat109-2

Offline

Зарегистрирован: 24.09.2015

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

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

На примере в цифрах:

Допустим у вас 3 датчика: один "по оси" и его смещение = 0, два других на некотором расстоянии и их "смещение" примем за +1 и -1. Далее, будем считать что "черное" = 1, а "белое" = 0. Итого получим:

1. Датчики "по центру" левый (смещение -1) видит белое (0), правый (смещение +1) видит белое (0), центральный (смещение 0) видит черное (1), ошибка = (-1)*0 + (0)*1 + (+1)*0 = 0 - едем прямо.

2. Линия ушла вправо. Только правый датчик (+1) видит линию (черное = 1), ошибка: (-1)*0 + (0)*0 + (+1)*1 = 1.

3. Линия ушла влево, аналогично, ошибка = (-1)*1 +(0)*0 + (+1)*0 = -1

4. Линия слегка вправо. черное видят 2 датчика - центральный и правый одновременно, ошибка: (-1)*0 + (0)*1 + (+1)*1 = 1, но линию видят 2 датчика!

То же самое можно распространить на ситуацию когда больше 3-х датчиков, дав смещения кратные основанию 2, +-: 2,4,8.., пример в числах:

4 датчика, смещения: -4, -2, 2, 4. Центральный датчик - отсутствует. Линия немного справа, черное видят оба правых датчика, имеем ошибка = (-4)*0 + (-2)*0 + (2)*1 + (4)*1 = 6. Мы получили удвоенное значение, потому что 2 датчика видят линию. Если поделить это число на количество датчиков, видящих черное, то получим 3, что ровно посередине между 2 и 4. То есть линия "где-то между правыми датчиками".

Те же 4 датчика, линия широкая и посередине. Все видят черное, ошибка = (-4)*1 + (-2)*1 + (+2)*1 + (+4)*1 = 0. :)

Итого, создаем массив структур "датчик", где в структуре для каждого датчика прописываем: "уровень черного", "уровень белого", "порог перехода белое/черное", "текущее значение черное/белое" и "смещение датчика". Далее в цикле по всем датчикам заносим в замер с текущего датчика, сразу же сравнивая с порогом и устанавливая значение. В этом же цикле можно сразу же считать количество датчиков, видящих черное и суммарную ошибку. По завершению цикла проверяем есть ли датчики, видящие черное и делим суммарную ошибку на количество датчиков. А если таковых нет, то линия сбоку от всех датчиков. С какого? С того, где она была "предыдущий раз".

Превратить этот алгоритм в программу, надеюсь сможете самостоятельно. Как изменить алгоритм для белой/черной линии, думаю уже тоже "не проблема". :)

Интересный момент: при смещении линии и определенном растоянии промеж датчиков, возможна ситуевина когда датчики видят линию в такой последовательности: 1 датчик, 2 датчика, 1 датчик, 2 датчика .. если подобрать "правильное" расстояние между датчиками, то эти ситуации будут занимать одинаковое время при равномерном смещении линии от центра и практически получается как бы "удвоение" количества датчиков. Какое расстояние есть "правильное"? Это на дом, подумать.. :)

Можно и вообще не заморачиваться с "битовым" представлением линии как черное/белое, а использовать сразу аналоговое значение с датчиков. Разве что замеренное значение надо "привести" функцией map() к единому стандартному диапазону, скажем 100-0 (чем чернее - тем больше) из индивидуального "мин. черное - макс. белое" - как правило наоборот: чем белее тем больше. К примеру из 4 датчиков, крайний левый видит белое (0), левый видит край линии (50), правый видит линию (100), крайний правый видит почти линию (75), ошибка = (-4)*0 + (-2)*50 + (+2)*100 + (+4)*75 = 133(1/3). Даже так работает, но неплохо преобразовать к варианту в целых числах, ибо Ардуино считает float фантастически медленно. Тоже на самостоятельную практику. :)

Главный алгоритм loop().

Его проще всего сделать "единым" на все случаи жизни, типа такого:

void loop()
{
  getSensors();   // читаем датчики в глобальный массив структур:
  calcError();    // вычисляем ошибку оси датчиков от оси линии в глобал
  getDistance();  // читаем расстояние до препятствия в глобал
  if( glDistance > MIN_DISTANCE )
  {
    // ok. Можно ехать
    calcSpeed();  // вычисляем скорости моторов
    moveMotors(); // переустанавливаем скорости моторов
  }else{
    // упс. Препятствие
    doWall();     // исполняем алгоритм "стенка" .. смотря что надо: остановиться или объехать..
  }
}

Заменяя нужные функции, со временем освоите и "фильтр Кальмана" и прочие компенсационные алгоритмы устранения ошибок датчиков и разные способы управления и можно перенастраивать на разные драйвера моторов и т.д. главный loop() - менять не за чем. Просто как пример проектирования программы "сверху вниз". :)

К примеру, у нас та же calcSpeed() уже разрослась до самостоятельного "проекта": если прямо, то одна процедура calcForward(), если поворот, то иная calcRotate(). Первая учитывает текущую разницу скоростей моторов (выход на прямую с поворота), вторая вычисляет кривизну поворота, опрашивает алгоритм - предсказатель быстрого разворота, умеет различать ещё несколько ситуаций, требующих отдельного управления в повороте и т.д. :)

Наша функция calcError() заодно вычисляет скорость локального и накопленного (общую кривизну) поворота, считает перекрестки на трассе и включает узв. датчик только в критических местах, если "финал" и т.д. В общем, это только "с виду" движение по линии - примитив. Нет предела совершенству. Дерзайте.

Как пройдете трассу Робофест за менее 7сек - пишите обязательно. :)

Пт, 02/03/2018 - 19:57

Arhat109-2

Offline

Зарегистрирован: 24.09.2015

Кстати, а куда у вас делись пины у НАНО?

У нас: 5-7 датчиков линии А7..А1, как тележку собрали (Ардуино как Лего - сборка за 5сек десятка вариантов :) ), замер напряжения на аккумуляторах с целью недопущения переравзряда - А0, (2,3),4,5,6,7 - управление 2-я моторами в зависимости от драйвера на 2(свой) или 3 ноги (L298N) управления. При этом 5,6 - ШИМ на моторы. 8,9 или А1,А2 или 2,3 - узв. датчик, смотря сколько датчиков линии установлено и какой драйвер мотора и надо ли ещё чего. 9,10,11,12,13 - "лампочки" диагностики (светодиоды) по 1 шт на каждый датчик линии. 8 - ещё может использоваться как кнопка - переключатель алгоритма поведения робота - смена алгоритма "квалификация - финал", которая включает в работу узв. датчик. На квалификациях - он нафиг не нужен. Или меняет настроечный массив "быстрая - медленная" покатушка.

При необходимости, можно и 0,1 пины задействовать...

Пт, 02/03/2018 - 20:18

#10

Arhat109-2

Offline

Зарегистрирован: 24.09.2015

Наш setup():

void setup()
{
  setupMotors()   // настраиваем пины моторов и сразу их выключаем нафиг
  delay(4000);    // задержка на заливку программы, установку на трассу и т.п.
  setupLeds();    // настройка пинов "лампочек"
  blinkAll();     // моргаем всеми лампочками - пошла подготовка.
  setupSensors(); // настройка пинов, контроль места и если надо автонастройка или чтение из EEPROM данных датчиков
  setupSpeed();   // настройка/чтение EEPROM настроечных констант движения, контроль переключателя
  blinkAll();     // сигнализация о готовности к движению
  delay(3000);    // задержка для установки препятствия "рука" дабы не поехать раньше старта
  glDistance = 5000; // разрешаем движение по препятствию "далеко" (у нас в мм).
  time = millis();   // фикс. время старта для узв. датчика
}

Время нужно, потому что функция замера у нас ограничена по времени замера только 2мсек. и она запускается не "каждый раз", а только по прошествию не менее 40мсек (blink без delay), для того чтобы не ловить свои же писки, отраженные от дальних предметов, участников, судей и т.д. Дальность срабатывания датчика по его чувствительности - около 5м, или почти 30мсек. Вот, чтобы исключить накладки он замеряет расстояния не слишком часто.

Весь цикл loop() укладывается в 1-2мсек без замера расстояний.

Пт, 02/03/2018 - 21:43

#11

al072

Offline

Зарегистрирован: 20.04.2017

Arhat109-2 пишет:

Наш setup():

void setup()
{
  setupMotors()   // настраиваем пины моторов и сразу их выключаем нафиг
  delay(4000);    // задержка на заливку программы, установку на трассу и т.п.
  setupLeds();    // настройка пинов "лампочек"
  blinkAll();     // моргаем всеми лампочками - пошла подготовка.
  setupSensors(); // настройка пинов, контроль места и если надо автонастройка или чтение из EEPROM данных датчиков
  setupSpeed();   // настройка/чтение EEPROM настроечных констант движения, контроль переключателя
  blinkAll();     // сигнализация о готовности к движению
  delay(3000);    // задержка для установки препятствия "рука" дабы не поехать раньше старта
  glDistance = 5000; // разрешаем движение по препятствию "далеко" (у нас в мм).
  time = millis();   // фикс. время старта для узв. датчика
}

Весь цикл loop() укладывается в 1-2мсек без замера расстояний.

Огого да тут целый научный труд на странице А4 изложен! Большое вам спасибо за столь подробные и познавательные пояснения! Этой информации просто так на поверхности интернета не найти... у вас наверное кружок робототехники свой раз так глубоко разбираетесь в данном вопросе. Значит по тихонько будем переходить от теории к практике...

В своём проекте я использую вот такие датчики: http://s.aliexpress.com/mIFnM3eI

Целый день пытался получить корректные аналоговые показания, но пока без результатно. Подключил 3 датчика на аналоговые пины А0-А2, монитор порта для белого фона даёт следующие показания: 49, 96, 360 (левый, правый, центральный) датчики, не пойму почему такая большая разница.. датчики подключены напрямую через плату расширения к нано., думаю если не получится совладать с аналоговыми показателями попробую использовать плату которая шла вместе с датчиками и снимать с неё дискретные сигналы, вот здесь как раз мне будет проще понять как реализовать алгоритм на основе ваших формул.

Практически все пины ушли на четыре мотора (по два дискрета + ШИМ на каждый), два сервопривода, TTL для Управления по радиоканалу, а также три датчика для следования по линии. Свободные остались еще А5 и А6-А7 но данные контакты отсутствуют на плате расширения (походу китайцы про них забыли, вот плата которую использую:http://s.aliexpress.com/QRBF3YjM), а подпаиваться напрямую к пинам наны крайне не удобно, у меня она располагается не очень удобно.., вот я и хотел выкружить ещё один пинок для HC SR-04. Но теперь пойду другим путём, планирую применить другой дальномер (sharp), а он просит как раз один пин.

Еще раз спасибо вам, думаю что блинов с комками будет ещё очень много, буду писать по мере поступления вопросов..

У вас есть личная почта?

Сб, 03/03/2018 - 05:20

#12

Arhat109-2

Offline

Зарегистрирован: 24.09.2015

A6, A7 - не бывает на платах расширения. Это доп. пины только для Нано и только для аналоговых датчиков. Ни к чему больше они не годны.

У Вас какая-то сложная конструкция, а не гонка по линии, раз 4 мотора и 2 сервы. В этом разе 3 датчиков Вам может оказаться вполне достаточно, а может даже и уйти на всего два, поскольку скоростное решение задачи Вам не так интересно.

Ваши датчики правильно подключать через их плату, а не напрямую. Там стоит вполне нормальный ОУ с регулировкой сигнала. Подключать датчики без ОУ напрямую сильно проблематично. Ардуино просит выходное сопротивление датчика не более 10кОм, а у таких же наших самоделок около 500кОм. При переключении мультиплексора АЦП НАНО просто с датчика без усилителя - получите фигню из-за подзаряженного конденсатора АЦП внутри НАНО. Резисторами выводите датчики на примерно одинаковые показания, но в целом, стремиться подобрать совсем одинаково - незачем. Можно и напрямую, если вес заставляет, но тогда надо в коде чтения каждого датчика вычитать некоторый остаток от предыдущего замера, особенно если делаете их быстро в одном цикле. У нас приходилось вычитать до трети-четверти от предыдущего замера - компенсация взаимовлияния датчиков из-за высокого выходного сопротивления.

Разброс в несколько раз - нормально. При изготовлении линейки датчиков для тележки на соревнования, типа ваших (только ОУ и резистор на каждом датчике + замена сопротивления освещения), из партии в 50 ИК фото-, свето- диодов, удалось откалибровать "более-менее" (5%) только кучками до 12шт, чаще 2-3шт. Отклик с измерительным резистором 620кОм имеет разброс от 0.08 до 4.05в, а светодиоды дают свой разброс не менее 2.5 раз. Вот и подумайте что получится в серийном производстве насыпным способом. :)

Можно посоветовать дискретные пины моторов посадить на какой-нибудь расширитель портов, типа регистр 595 или расширитель по I2C. Последний кажется поудобней в управлении, но все равно несколько проблематично ..

Кстати, тут была тема "Драйвер коллекторного мотора" http://arduino.ru/forum/proekty/arduino-draiver-motora-dlya-robota, в которой делали драйвера, работающие по I2C или com или как сервы. Пошукайте, спишитесь с авторами. Это может оказаться решением и существенным.

Если хотите, пишите сюда: arhat109 собака мейл точка ру

P.S. Да, уже можно сказать что у меня кружок. Даже почти 3шт.. :) В проектах есть тема arduino.ru/forum/proekty/lego-kirpich-iz-mega2560 - там есть практически вся история нашего появления тут. Думаю Вам будет интересно почитать и за моторы и за датчики и за иное тоже. :)

Ещё посмотрите эту тему: "Скоростное движение по линии.." в основном разделе.

P.P.S. И да, согласен: "этой информации в интерентах нет". Сам в свое время - обыскался. А будете выкладываться также - Вас точно также будут поливать все кто "знает но молчит", ибо торгует... :)

Сб, 03/03/2018 - 08:15

#13

al072

Offline

Зарегистрирован: 20.04.2017

Спасибо за ссылочки, обязательно гляну и проникнусь!!! Конструкция вся металлическая, с достаточно мощными мотор редукторами. Две сервы идут на Управление клешней (две степени свободы x,y), вы правильно подметили скоростной проход линии мне совершенно не к чему. Медленно но верно. Также передача видео по радиоканалу имеется с функцией распознавания лиц. В общем и целом все.

Интересное это занятие кружок робототехники! Успехов вам в соревнованиях!

Сб, 03/03/2018 - 11:57

#14

kalapanga

Offline

Зарегистрирован: 23.10.2016

al072 пишет:

Мне случайно как-то попалась страничка, оставил в закладках. Там математические модельки - тренажёры, которые позволяют онлайн визуально поиграться с алгоритмами движения по линии для разного числа датчиков и разных трасс. Робототехника

Сб, 03/03/2018 - 12:45

#15

al072

Offline

Зарегистрирован: 20.04.2017

kalapanga пишет:

al072 пишет:

Супер!!!

Вс, 18/03/2018 - 22:40

#16

al072

Offline

Зарегистрирован: 20.04.2017

Доброго времени суток! Наконец-то получилось научить тележку ездить по линии, но до совершенства ещё далеко, подскажите плиз:

1. Как понять в какую сторону поварачивать если робот сходит с линии?

2. Как отличить кратковременный сход с линии от необходимости остановки? Условия остановки: все три датчика на черном.

3. Как заставить робота двигаться более плавно по линии, а то слишком уж дерганный.

Ниже код который я использую (https://www.drive2.ru/b/2891581/):

//Робот с функцией следования по белой полосе "изоленте".

// *********************** Установка выводов моторов ************************
int MotorLeftSpeed = 5; // Левый (А) мотор СКОРОСТЬ — ENA
int MotorLeftForward = 4; // Левый (А) мотор ВПЕРЕД — IN1
int MotorLeftBack = 2; // Левый (А) мотор НАЗАД — IN2
int MotorRightForward = 8; // Правый (В) мотор ВПЕРЕД — IN3
int MotorRightBack = 7; // Правый (В) мотор НАЗАД — IN4
int MotorRightSpeed = 6; // Правый (В) мотор СКОРОСТЬ — ENB
int duration;

// ********************* Установка выводов датчиков линии *******************
const int LineSensorLeft = 19; // вход левого датчика линии
const int LineSensorMiddle = 18; // вход центрального датчика линии
const int LineSensorRight = 17; // вход правого датчика линии
int SL; // статус левого сенсора
int SM; // статус центрального сенсора
int SR; // статус правого сенсора

// *********************************** SETUP ********************************
void setup ()
{
//*************** Задаем контакты моторов
pinMode (MotorRightBack, OUTPUT); // Правый (В) мотор НАЗАД
pinMode (MotorRightForward, OUTPUT); // Правый (В) мотор ВПЕРЕД
pinMode (MotorLeftBack, OUTPUT); // Левый (А) мотор НАЗАД
pinMode (MotorLeftForward, OUTPUT); // Левый (А) мотор ВПЕРЕД
delay(duration);
//*************** Задаем контакты датчиков полосы
pinMode (LineSensorLeft, INPUT); // определением pin левого датчика линии
pinMode (LineSensorMiddle, INPUT);// определением pin центрального датчика линии
pinMode (LineSensorRight, INPUT); // определением pin правого датчика линии
}
// ****************** Основные команды движения ******************
void forward (int a, int sa) // ВПЕРЕД
{
digitalWrite (MotorRightBack, LOW);
digitalWrite (MotorRightForward, HIGH);
analogWrite (MotorRightSpeed, sa);
digitalWrite (MotorLeftBack, LOW);
digitalWrite (MotorLeftForward, HIGH);
analogWrite (MotorLeftSpeed, sa);
delay (a);
}
void right (int b, int sb) // ПОВОРОТ ВПРАВО (одна сторона)
{
digitalWrite (MotorRightBack, LOW);
digitalWrite (MotorRightForward, LOW);
digitalWrite (MotorLeftBack, LOW);
digitalWrite (MotorLeftForward, HIGH);
analogWrite (MotorLeftSpeed, sb);
delay (b);
}
void left (int k, int sk) // ПОВОРОТ ВЛЕВО (одна сторона)
{
digitalWrite (MotorRightBack, LOW);
digitalWrite (MotorRightForward, HIGH);
analogWrite (MotorRightSpeed, sk);
digitalWrite (MotorLeftBack, LOW);
digitalWrite (MotorLeftForward, LOW);
delay (k);
}
void turnR (int d, int sd) // БЫСТРЫЙ ПОВОРОТ ВПРАВО (обе стороны)
{
digitalWrite (MotorRightBack, HIGH);
digitalWrite (MotorRightForward, LOW);
analogWrite (MotorRightSpeed, sd);
digitalWrite (MotorLeftBack, LOW);
digitalWrite (MotorLeftForward, HIGH);
analogWrite (MotorLeftSpeed, sd);
delay (d);
}
void turnL (int e, int se) // БЫСТРЫЙ ПОВОРОТ ВЛЕВО (обе стороны)
{
digitalWrite (MotorRightBack, LOW);
digitalWrite (MotorRightForward, HIGH);
analogWrite (MotorRightSpeed, se);
digitalWrite (MotorLeftBack, HIGH);
digitalWrite (MotorLeftForward, LOW);
analogWrite (MotorLeftSpeed, se);
delay (e);
}
void back (int g, int sg) // НАЗАД
{
digitalWrite (MotorRightBack, HIGH);
digitalWrite (MotorRightForward, LOW);
analogWrite (MotorRightSpeed, sg);
digitalWrite (MotorLeftBack, HIGH);
digitalWrite (MotorLeftForward, LOW);
analogWrite (MotorLeftSpeed, sg);
delay (g);
}
void stopp (int f) // СТОП
{
digitalWrite (MotorRightBack, LOW);
digitalWrite (MotorRightForward, LOW);
digitalWrite (MotorLeftBack, LOW);
digitalWrite (MotorLeftForward, LOW);
delay (f);
}
// *********************************** LOOP *********************************
// ********************** Режим следования по ЛИНИИ *************************
void loop ()
{
SL = digitalRead (LineSensorLeft); // считываем сигнал с левого датчика полосы
SM = digitalRead (LineSensorMiddle); // считываем сигнал с центрального датчика полосы
SR = digitalRead (LineSensorRight); // считываем сигнал с правого датичка полосы

// ************************* Следование по белой линии ***********************
// РОБОТ на полосе — едем прямо
if (SL == HIGH & SM == LOW & SR == HIGH ) // ЧЕРНЫЙ — БЕЛЫЙ — ЧЕРНЫЙ — едем ПРЯМО
{
forward (10, 140);// ПРЯМО (время, скорость)
}
// РОБОТ начинает смещаться с полосы — подруливаем
else if (SL == LOW & SM == LOW & SR == HIGH) // БЕЛЫЙ — БЕЛЫЙ — ЧЕРНЫЙ — поворот ВЛЕВО
{
left (10, 180);// поворот ВЛЕВО (время, скорость)
}
else if (SL == HIGH & SM == LOW & SR == LOW) // ЧЕРНЫЙ — БЕЛЫЙ — БЕЛЫЙ — поворот ВПРАВО
{
right (10, 180);// поворот ВПРАВО (время, скорость)
}
// РОБОТ начинает съезжать с полосы — доворачиваем
else if (SL == HIGH & SM == HIGH & SR == LOW) // ЧЕРНЫЙ — ЧЕРНЫЙ — БЕЛЫЙ — поворот ВПРАВО БЫСТРО
{
turnR (50, 150);// РАЗВОРОТ ВПРАВО (время, скорость)
}
else if (SL == LOW & SM == HIGH & SR == HIGH) // БЕЛЫЙ — ЧЕРНЫЙ — ЧЕРНЫЙ — поворот ВЛЕВО БЫСТРО
{
turnL (30, 150);// РАЗВОРОТ ВЛЕВО (время, скорость)
}
// РОБОТ съехал с полосы — кружим, ищем полосу
else if (SL == HIGH & SM == HIGH & SR == HIGH) // ЧЕРНЫЙ — ЧЕРНЫЙ — ЧЕРНЫЙ — поворот затяжной ВЛЕВО
{
left (10, 130); // поворот ВЛЕВО (время, скорость)
}
else if (SL == LOW & SM == LOW & SR == LOW) // БЕЛЫЙ — БЕЛЫЙ — БЕЛЫЙ — поворот затяжной ВПРАВО
{
forward (10, 140);// ПРЯМО (время, скорость)
}
}

Arduino.ru

Регистрация новых пользователей и создание новых тем теперь только на новом форуме https://forum.arduino.ru

Line Follower помогите адаптировать