Избегаем столкновений с препятствиями

Я установил на платформу инфракрасные датчики препятствий. Статья об этих модулях. И, разумеется, в статье приводится код.


Робот с ИК датчиками.

Установлено три модуля.

Этот датчик у меня давно. Я уже делал робота с этим модулем http://justforduino.blogspot.ru/2014/02/blog-post_23.html

Недавно приобретенные мной датчики препятствий. Есть разные по конструкции, но внешне похожие реализации. На картинке показана версия на NE555. Я ещё видел на 74HC00D.

Ранее купленные ИК модули лучше. Они обнаруживают препятствия с большего расстояния. Для моей платформы они также лучше подходят по размерам и креплению.

Принципиальных отличий у показанных датчиков препятствий нет. Светодиод периодически мигает на определенной частоте. Отраженный от препятствий ИК свет регистрируется приемником.

У модулей одинаковое число выводов. Назначение выводов также идентично.

В этом проекте модули подключены к ардуино так, как описано ниже.

Левый модуль.

Arduino --- Infrared Sensor
GND --- GND
VCC --- +
A0 --- OUT

Центральный модуль.

Arduino --- Infrared Sensor
GND --- -
VCC --- +
A1 --- S

Правый модуль.

Arduino --- Infrared Sensor
GND --- GND
VCC --- +
A2 --- OUT

Когда препятствий нет, на выходах датчиков установлена логическая единица. При регистрации препятствия, на выходах модулей устанавливается логический ноль.

Скетч

////////////////////////
//
// Arduino UNO
//
////////////////////////
//
// Sketch: Robot
//

#include <util/delay.h>

#define DRIVER_DDR DDRB
#define DRIVER_PORT PORTB

#define ENA 5
#define IN1 4
#define IN2 3
#define IN3 2
#define IN4 1
#define ENB 0

#define BUMPER_DDR DDRC
#define BUMPER_PORT PORTC
#define BUMPER_PIN PINC

#define S1 2
#define S2 1
#define S3 0

volatile unsigned char count = 0;
volatile unsigned char width = 22;

void bumper_init( void );
void driver_init( void );
void tc2_init( void );
void stop( void );
void forward( void );
void backward( void );
void right( void );
void left( void );

void setup()
{
    bumper_init();
    driver_init();
    tc2_init();
    _delay_ms(4000);
}

void loop()
{
    if ( (BUMPER_PIN & (1<<S1)) == 0 )
    {
        stop();
        _delay_ms(1000);

        right();
        while ( (BUMPER_PIN & ((1<<S1)|(1<<S2)|(1<<S3))) !=
                ((1<<S1)|(1<<S2)|(1<<S3)) ) ;
        _delay_ms(500);

        stop();
        _delay_ms(1000);
    }

    if ( (BUMPER_PIN & (1<<S3)) == 0 )
    {
        stop();
        _delay_ms(1000);

        left();
        while ( (BUMPER_PIN & ((1<<S1)|(1<<S2)|(1<<S3))) !=
                ((1<<S1)|(1<<S2)|(1<<S3)) ) ;
        _delay_ms(500);

        stop();
        _delay_ms(1000);
    }

    if ( (BUMPER_PIN & (1<<S2)) == 0 )
    {
        stop();
        _delay_ms(1000);

        if ( (TCNT2 & (1<<0)) == 0 )
        {
            right();
        }
        else
        {
            left();
        }

        while ( (BUMPER_PIN & ((1<<S1)|(1<<S2)|(1<<S3))) !=
                ((1<<S1)|(1<<S2)|(1<<S3)) ) ;
        _delay_ms(500);

        stop();
        _delay_ms(1000);
    }

    forward();

    while ( (BUMPER_PIN & ((1<<S1)|(1<<S2)|(1<<S3))) ==
            ((1<<S1)|(1<<S2)|(1<<S3)) ) ;
}

void bumper_init()
{
    BUMPER_DDR &= ~((1<<S1)|(1<<S2)|(1<<S3));
    BUMPER_PORT |= ((1<<S1)|(1<<S2)|(1<<S3));
}

void driver_init()
{
    DRIVER_DDR |= ((1<<ENA)|(1<<IN1)|(1<<IN2)|(1<<IN3)|(1<<IN4)|(1<<ENB));
    DRIVER_PORT &= ~((1<<ENA)|(1<<IN1)|(1<<IN2)|(1<<IN3)|(1<<IN4)|(1<<ENB));
}

void tc2_init()
{
     // TC2 Counter Value Register
    TCNT2 = 0;
    // TC2 Control Register A
    // COM2A1 COM2A0 COM2B1 COM2B0 - - WGM21 WGM20
    TCCR2A = 0;
    // TC2 Control Register B
    // FOC2A FOC2B - - WGM22 CS22 CS21 CS20
    TCCR2B = (1<<CS20);
    // TC2 Interrupt Mask Register
    // - - - - - OCIE2B OCIE2A TOIE2
    TIMSK2 = (1<<TOIE2);
    // TC2 Interrupt Flag Register
    // - - - - - OCF2B OCF2A TOV2
    TIFR2 = (1<<TOV2);
}

void stop()
{
    DRIVER_PORT &= ~((1<<IN1)|(1<<IN2)|(1<<IN3)|(1<<IN4));
}

void forward()
{
    DRIVER_PORT &= ~((1<<IN1)|(1<<IN2)|(1<<IN3)|(1<<IN4));
    DRIVER_PORT |= ((0<<IN1)|(1<<IN2)|(0<<IN3)|(1<<IN4));
}

void backward()
{
    DRIVER_PORT &= ~((1<<IN1)|(1<<IN2)|(1<<IN3)|(1<<IN4));
    DRIVER_PORT |= ((1<<IN1)|(0<<IN2)|(1<<IN3)|(0<<IN4));
}

void right()
{
    DRIVER_PORT &= ~((1<<IN1)|(1<<IN2)|(1<<IN3)|(1<<IN4));
    DRIVER_PORT |= ((1<<IN1)|(0<<IN2)|(0<<IN3)|(1<<IN4));
}

void left()
{
    DRIVER_PORT &= ~((1<<IN1)|(1<<IN2)|(1<<IN3)|(1<<IN4));
    DRIVER_PORT |= ((0<<IN1)|(1<<IN2)|(1<<IN3)|(0<<IN4));
}

ISR(TIMER2_OVF_vect)
{
    if ( count == 0 ) DRIVER_PORT |= ((1<<ENA)|(1<<ENB));
    if ( count == width ) DRIVER_PORT &= ~((1<<ENA)|(1<<ENB));

    count += 1;
    if ( count == 40 ) count = 0;
}

//
// End
//
////////////////////////

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

Повороты выполняются до тех пор, пока датчики не перестают видеть препятствие плюс ещё 500 миллисекунд. Все изменения направления движения выполняются после остановки и неподвижного стояния в течении секунды.

Пока препятствий нет, робот едет вперед.

Итог. Не зря снижал скорость робота. Если бы робот двигался быстрее, то просто не успевал бы остановится. А так всё работает не плохо.