В этом уроке:
- Устанавливаем знак «Железнодорожный переезд со шлагбаумом» на трассу
- Учим машинку реагировать на шлагбаум
- Остановка перед препятствием
Установка знака со шлагбаумом на трассу
Представим, что нам на пути встречается железнодорожный переезд со шлагбаумом. Шлагбаум установлен на соответствующем знаке, который, как и любой другой знак, может передавать информацию ИК-сигналом.
Однако, давайте на этот раз не будем считывать ИК-сигнал от знака, а реализуем остановку автомобиля при опущенном шлагбауме с помощью датчика расстояния. Это также поможет нам не врезаться в другие объекты на трассе, если вдруг они появятся. Мы выбрали такой способ распознавания шлагбаума ещё и из-за технической особенности знаков. Вы же помните, что обычно они устанавливаются под углом 45° к трассе для того, чтобы ИК-сигнал попадал на приёмник машинки?
Для того, чтобы шлагбаум был перпендикулярен дороге, мы устанавливаем знак так, что его сигнал не будет воспринят машинкой. Подробнее об установке шлагбаума можете почитать в соответствующей статье.
Итак, соберите и настройте шлагбаум, если еще этого не сделали, после чего установите его на таким образом, чтобы круглая табличка «STOP» располагалась точно над линией трассы.
Поиск препятствий на пути
Для обнаружения препятствий используем ультразвуковой датчик расстояния, поэтому не забываем подключить библиотеку (её мы установили в первой главе) и создать объект датчика:
#include <iarduino_HC_SR04_tmr.h> // Подключаем библиотеку для работы с датчиком расстояния HC-SR04
iarduino_HC_SR04_tmr len (12,11); // Объявляем объект len для работы с датчиком расстояния, указав номера выводов arduino, подключённых к выводам TRIG (12) и ECHO (11) датчика
Также не забудьте выполнить инициализацию:
len.begin(); // Инициируем работу с датчиком расстояния HC-SR04
Запуск функции будем производить следующим образом, передавая в неё значение минимальной дистанции для остановки.
const uint16_t min_Distance = 15; // Минимальное расстояние до препятствия в см, при котором требуется остановиться
// ОСТАНОВКА ПЕРЕД ПРЕПЯТСТВИЕМ:
fnc_StopObstacle(min_Distance); // Остановка при наличии препятствия ближе min_Distance см
Функция, выполняющая остановку:
// ФУНКЦИЯ ОСТАНОВКИ ПЕРЕД ПРЕПЯТСТВИЕМ:
void fnc_StopObstacle(uint16_t distance){ // Функция получает дистанцию в см, при которой требуется остановиться
// Проверяем наличие препятствия:
if( len.distance() > distance ){ return; } // Выходим из функции, если расстояние до препятствия больше «distance» см.
// Определяем время обнаружения препятствия:
uint32_t currentTime = millis(); // Получаем текущее время
// Выполняем остановку:
mot_R.setStop(); // Останавливаем правый мотор
mot_L.setStop(); // Останавливаем левый мотор
disp.print("STOP"); // Загружаем текст «STOP» для бегущей строки
disp.scrollPos(0); // Переходим к началу бегущей строки
disp.autoScroll(245, 300); // Выводим загруженный текст со скоростью 245 и паузой между прокрутками строки в 300 мс
// Ждём освобождения дороги от препятствия:
while( currentTime > millis()-tim_Wait ){ // Пока не пройдёт «tim_Wait» мс после времени «currentTime»
if( len.distance() <= distance+3 ){ // Если до препятствия меньше «distance»+3 см ...
currentTime = millis(); // Обновляем текущее время
}
}
// Информируем о готовности продолжить движение:
disp.clrScr(); // Чистим экран светодиодной матрицы
delay(500);
disp.print("GO"); // Загружаем текст «GO» для бегущей строки
disp.scrollPos(0); // Переходим к началу бегущей строки
disp.autoScroll(245); // Выводим загруженный текст однократно со скоростью 245
delay(1000);
disp.clrScr(); // Чистим экран светодиодной матрицы
}
Не забываем ввести переменную времени ожидания перед началом движения:
const uint32_t tim_Wait = 1000; // Определяем время ожидания в мс до начала движения после остановки
Алгоритм работы следующий:
В функцию передаётся значение минимального расстояния (49 строка). Происходит вызов функции (строка 206), после чего проверяется условие (209 строка): если расстояние до препятствия больше минимального, то происходит немедленный выход из функции (return). Если нет, то продолжаем: нужно остановить машинку.
Определяем время обнаружения препятствия (строка 212), останавливаем моторы (строки 215, 216), далее как обычно работаем с дисплеем, выводя «STOP».
В строке 222 попадаем в цикл, длящийся до тех пор, пока не пройдёт время ожидания tim_Wait, указанное в начале скетча. После этого снова измеряется расстояние, и, если препятствие еще перед машинкой, отсчёт времени начинается заново (строка 224), и выхода из цикла не происходит.
Иначе машинка продолжает движение (строки 229-235). Все команды здесь Вы уже отлично знаете.
Готовый скетч урока:
#include <Wire.h> // Подключаем библиотеку для работы с аппаратной шиной I2C
#include <iarduino_I2C_Motor.h> // Подключаем библиотеку для работы с мотором I2C-flash
#include <iarduino_I2C_Bumper.h> // Подключаем библиотеку для работы с бампером I2C-flash
#include <iarduino_I2C_IR.h> // Подключаем библиотеку для работы с Trema модулями: ИК-приёмник/передатчик I2C-flash
#include <iarduino_I2C_Matrix_8x8.h> // Подключаем библиотеку для работы с LED матрицей 8x8
#include <iarduino_HC_SR04_tmr.h> // Подключаем библиотеку для работы с датчиком расстояния HC-SR04
iarduino_I2C_Motor mot_R (0x0A); // Объявляем объект mot_R для правого мотора, указав адрес модуля на шине I2C
iarduino_I2C_Motor mot_L (0x0B); // Объявляем объект mot_L для правого мотора, указав адрес модуля на шине I2C
iarduino_I2C_Bumper bum (0x0C); // Объявляем объект bum для работы с бампером I2C-flash, указав адрес модуля на шине I2C
iarduino_I2C_Matrix_8x8 disp(0x0D); // Объявляем объект disp для работы с LED матрицей 8x8, указывая её адрес на шине I2C
iarduino_I2C_IR ir(0x09); // Объявляем объект ir для работы с функциями и методами библиотеки iarduino_I2C_IR, указывая адрес модуля на шине I2C
iarduino_HC_SR04_tmr len (12,11); // Объявляем объект len для работы с датчиком расстояния, указав номера выводов arduino, подключённых к выводам TRIG (12) и ECHO (11) датчика
const float min_Speed = 20; // Минимальная скорость движения в км/ч. Используется при движении вблизи школ, а также на опасных участках дороги
const float low_Speed = 40; // Низкая скорость движения в км/ч. Используется при движении по опасным участкам дороги или при соответствующем знаке
const float mid_Speed = 60; // Средняя скорость движения в км/ч. Используется при движении по дорогам
const float max_Speed = 90; // Максимальная скорость движения в км/ч. Используется при движении по скоростным дорогам
float speed = mid_Speed; // Указываем, что изначально скорость равна средней (60 км/ч)
int8_t val_Turn = 0; // Выбранное направление движения на перекрёстке: 0 прямо, -1 влево, +1 вправо
bool flg_CrossWait = false ; // Флаг ожидания перекрёстка (машина «увидела» светофор или знак, сообщающий о наличии перекрёстка)
bool flg_CrossFind = false; // Флаг обнаружения перекрёстка (бампер заехал на перекрёсток)
bool flg_Turn = false; // Флаг необходимости разворота
bool flg_25 = false; // Флаг знака 2.5 «Движение без остановки запрещено»
uint32_t timeSign; // Время для задержки срабатывания знака 2.5
bool flg_TLIGHT = false; // Флаг получения данных от светофора
byte Image_TLIGHT[8]; // Генерируется функцией fnc_CreateImage()
const uint16_t min_Distance = 15; // Минимальное расстояние до препятствия в см, при котором требуется остановиться
const uint32_t tim_Wait = 1000; // Определяем время ожидания в мс до начала движения после остановки
void setup() {
mot_R.begin(); // Инициируем работу с левым мотором I2C-flash
mot_L.begin(); // Инициируем работу с правым мотором I2C-flash
bum.begin(); // Инициируем работу с бампером I2C-flash
mot_R.setDirection(true); // Указываем правому мотору, что его вращение должно быть прямым (по часовой стрелке при положительных скоростях)
mot_L.setDirection(false); // Указываем левому мотору, что его вращение должно быть обратным (против часовой стрелки при положительных скоростях)
ir.begin(); // Инициируем работу с ИК-приёмником/передатчиком I2C-flash
ir.setProtocol(IR_IARDUINO); // Указываем протокол для приёма/передачи данных по ИК-каналу
disp.begin(); // Инициируем работу с LED матрицей 8x8
disp.codingDetect("п"); // Автоматическое определение кодировки (если выводим русский язык)
disp.setTimeIdleFirst(100); // Указываем бегущей строке задерживаться на первом символе в течение 100 мс (допускаются значения от 0 до 2550 мс)
disp.setTimeIdleLast(100); // Указываем бегущей строке задерживаться на последнем символе в течение 100 мс (допускаются значения от 0 до 2550 мс)
randomSeed(analogRead(A2)); // Выбираем начальную позицию для генерации случайных чисел
len.begin(); // Инициируем работу с датчиком расстояния HC-SR04
}
void loop() {
// ОСТАНОВКА ПЕРЕД ПРЕПЯТСТВИЕМ:
fnc_StopObstacle(min_Distance); // Остановка при наличии препятствия ближе min_Distance см
// АНИМАЦИЯ ДВИЖЕНИЯ НА СВЕТОФОРЕ:
if( flg_TLIGHT ){ // Если от светофора получены данные
fnc_CreateImage(ir.track_L, ir.track_F, ir.track_R, val_Turn); // Генерируем изображение перекрёстка светофора
disp.drawImage(Image_TLIGHT); // Выводим изображение перекрёстка светофора
}
// РАЗВОРОТ:
//if (bum.getCross(3, 1000)) flg_Turn = true;// Если обнаружен перекрёсток, устанавливаем флаг разворота
if( flg_Turn ){ // 1 ФАЗА. Если под бампером обнаружен перекрёсток (указываем толщину линии трассы и время на преодоление перекрёстка в мс) ...
// Под бампером обнаружен перекрёсток:
flg_Turn = false; // Сбрасываем флаг разворота
mot_R.setSpeed( convertSpeed(min_Speed), MOT_PWM ); // Устанавливаем скорость правого мотора в %
mot_L.setSpeed(-convertSpeed(min_Speed), MOT_PWM ); // Устанавливаем скорость левого мотора в %
// Ждём завершения манёвра:
while( bum.getLineSum() > 0 ) {;} // 2 ФАЗА. Ждём выхода линии за пределы бампера. Цикл выполняется, пока под бампером есть линия
while( bum.getLineSum() == 0 ){;} // 3 ФАЗА. Ждём появления линии под бампером. Цикл выполняется, пока под бампером нет линии
// Останавливаемся:
mot_R.setStop(); // Останавливаем правое колесо
mot_L.setStop(); // Останавливаем левое колесо
val_Turn = 0; // 4 ФАЗА. Выбираем движение прямо
speed = mid_Speed; // Снимаем наложенные ранее ограничения скорости
}
// ПРОВЕРКА НАЛИЧИЯ ПЕРЕКРЁСТКОВ:
if( bum.getCross(3,1000) ){ // Если под бампером обнаружен перекрёсток (указываем толщину линии трассы и время на преодоление перекрёстка в мс) ...
flg_TLIGHT = false; // Сбрасываем флаг наличия данных от светофора
disp.clrScr(); // Чистим экран светодиодной матрицы
if( flg_CrossWait ){ // Если ожидается появление перекрёстка ...
flg_CrossWait = false; // Сбрасываем флаг ожидания перекрёстка
flg_CrossFind = true; // Устанавливаем флаг обнаружения перекрёстка
}
}
else{ // Если под бампером обычная линия ...
if (flg_CrossFind) { // Если ранее был обнаружен перекрёсток
flg_CrossFind = false; // Сбрасываем флаг обнаружения перекрёстка
val_Turn = 0; // Выбираем движение прямо
speed = mid_Speed; // Снимаем наложенные ранее ограничения скорости
bum.setTurnSignal(BUM_TURN_OFF); // Отключаем поворотники
}
}
if( val_Turn == -1 ){ bum.setTurnSignal(BUM_TURN_LEFT ); } // Если поворот будет осуществляться налево, включаем левый поворотник
if( val_Turn == 0 ) { bum.setTurnSignal(BUM_TURN_OFF ); } // Выключаем поворотники, если машинка движется прямо
if( val_Turn == 1 ) { bum.setTurnSignal(BUM_TURN_RIGHT); } // Если поворот будет осуществляться направо, включаем правый поворотник
// ПРОВЕРКА НАЛИЧИЯ ПРИНЯТЫХ ИК-ДАННЫХ:
if( ir.check(true) ){ // Если принят пакет данных от знака
switch (ir.device) {
case MODUL_TLIGHT: // ПРИНЯТЫ ДАННЫЕ ОТ МОДУЛЯ СВЕТОФОР:
if( flg_TLIGHT == false ){ // Если данные от светофора еще не получались
flg_TLIGHT = true; // Фиксируем факт получения данных от светофора
// Выбираем направление поворота:
if( !ir.track_L && ir.track_F && !ir.track_R ){ val_Turn=0; } // Перекрёсток светофора имеет дорогу только прямо (F), выбираем движение прямо 0
if( !ir.track_L && ir.track_F && ir.track_R ){ val_Turn=random( 0,2); } // Перекрёсток светофора имеет дорогу прямо (F) и направо (R), выбираем движение прямо 0 или направо 1
if( ir.track_L && ir.track_F && !ir.track_R ){ val_Turn=random(-1,1); } // Перекрёсток светофора имеет дорогу прямо (F) и налево (L), выбираем движение налево -1 или прямо 0
if( ir.track_L && ir.track_F && ir.track_R ){ val_Turn=random(-1,2); } // Перекрёсток светофора имеет дорогу в любом направлении (LFR), выбираем движение налево -1, прямо 0 или направо 1
if( ir.track_L && !ir.track_F && ir.track_R ){ val_Turn=random( 0,2)? -1:1;} // Перекрёсток светофора имеет дорогу налево и направо (LR) , выбираем движение налево -1 или направо 1
flg_CrossWait = val_Turn ? true : false; // Ждём начала перекрёстка, если требуется поворачивать (если val_Turn != 0)
}
// Снижаем скорость или останавливаемся на светофоре:
if( flg_CrossWait ){ speed = low_Speed; } // Если ожидается перекрёсток, то снижаем скорость перед ним
else { speed = mid_Speed; } // Иначе выбираем обычную скорость
if( val_Turn== 0 && ir.forvard==0 ){ speed = 0; } // Если выбрано движение прямо и оно запрещено светофором, то останавливаемся (снижаем скорость до 0 %)
if( val_Turn==-1 && ir.left ==0 ){ speed = 0; } // Если выбрано движение влево и оно запрещено светофором, то останавливаемся (снижаем скорость до 0 %)
if( val_Turn== 1 && ir.right ==0 ){ speed = 0; } // Если выбрано движение вправо и оно запрещено светофором, то останавливаемся (снижаем скорость до 0 %)
break;
case MODUL_SIGN: // Данные отправлены дорожным знаком
if( !strncmp( ir.sign_str, "3.24", 4) ){ // Обнаружен знак ПДД 3.24 «Ограничение максимальной скорости»:
disp.print(ir.sign[2]); // Выводим цифру скорости
if( ir.sign[2]< 3 ){ speed = min_Speed; }else // ir.sign[2] < 3 значит, на знаке написано меньше «30»
if( ir.sign[2]< 5 ){ speed = low_Speed; }else // ir.sign[2] < 5 значит, на знаке написано меньше «50»
if( ir.sign[2]< 7 ){ speed = mid_Speed; }else // ir.sign[2] < 7 значит, на знаке написано меньше «70»
if( ir.sign[2]!=0 ){ speed = max_Speed; } // ir.sign[2] != 0 значит, на знаке написано «90»
}
if( !strcmp( ir.sign_str, "2.5" )){ // Если номер знака 2.5 - «Движение без остановки запрещено»
if (!flg_25) { // Если не установлен флаг знака (знак зафиксирован первый раз)
timeSign = millis(); // Сбрасываем значение переменной времени обнаружения знака
flg_25 = true; // Устанавливаем флаг знака
}
}
break;
case MODUL_CAR: // Данные отправлены другим автомобилем
// Здесь пока пусто, с другими машинками не взаимодействуем
break;
}
}
// Срабатывание по времени знака «Движение без остановки запрещено»
if(flg_25 && timeSign + 2000 < millis()){ // Если флаг установлен и с момента последней фиксации прошло больше 2 сек
flg_25 = false; // Сбрасываем флаг
disp.print("STOP"); // Загружаем текст для бегущей строки
disp.autoScroll(245); // Выводим загруженный текст однократно со скоростью 245
mot_R.setSpeed( 0, MOT_PWM ); // Выключаем правый мотор
mot_L.setSpeed( 0, MOT_PWM ); // Выключаем левый мотор
delay(1000);
while(bum.getErrPID() > -4) { // Поворот вправо
mot_R.setSpeed(-convertSpeed(min_Speed), MOT_PWM);
mot_L.setSpeed(convertSpeed(min_Speed), MOT_PWM);
}
while(bum.getErrPID() < 4) { // Поворот влево
mot_R.setSpeed(convertSpeed(min_Speed), MOT_PWM);
mot_L.setSpeed(-convertSpeed(min_Speed), MOT_PWM);
}
while(bum.getErrPID() > 0) { // Поворот вправо, возвращаемся к центру
mot_R.setSpeed(-convertSpeed(min_Speed), MOT_PWM);
mot_L.setSpeed(convertSpeed(min_Speed), MOT_PWM);
}
}
// ОПРЕДЕЛЯЕМ ОШИБКУ ЦЕНТРИРОВАНИЯ ЛИНИИ ПОД БАМПЕРОМ:
// Если ожидается перекрёсток: машина должна ехать по центру линии, немного сместившись в сторону поворота
float bum_Error; // Объявляем переменную: ошибка для П-регулятора
if( flg_CrossWait ){ bum_Error = bum.getErrPID()+val_Turn; }else // Получаем ошибку центрирования линии, смещённую на 1 датчик в сторону ожидаемого поворота «val_Turn»
//Если бампер заехал на перекрёсток: // Машина должна ехать по краю линии, выполняя поворот
if( flg_CrossFind ){ bum_Error = bum.getSidePID(val_Turn)*3; }else // Получаем ошибку нахождения на краю линии, со стороны поворота «val_Turn». Умножаем ошибку — это увеличит резкость поворота (актуально для тонких линий трассы)
//Если не ждём перекрёсток и не находимся на нём: // Машина должна ехать по центру линии
{ bum_Error = bum.getErrPID(); } // Получаем ошибку центрирования линии
float kP = 3 + 0.125*(convertSpeed(speed)-20); // Коэффициент П-регулятора
float P = bum_Error * kP; // Получаем значение от П-регулятора
mot_R.setSpeed(convertSpeed(speed) - P, MOT_PWM); // Устанавливаем скорость правого мотора
mot_L.setSpeed(convertSpeed(speed) + P, MOT_PWM); // Устанавливаем скорость левого мотора
}
float convertSpeed(float speedLevel){ // Функция преобразования скорости из км/ч в % ШИМ
if (speedLevel == 0) return 0; // Если скорость равна 0, возвращаем 0
else if (speedLevel < 20) return 20; // Если скорость меньше минимальной, устанавливаем минимальное заполнение ШИМ
else if (speedLevel > 90) return 60; // Если скорость больше максимальной, устанавливаем максимальное заполнение ШИМ
else return map(speedLevel, 20, 90, 20, 60); // Преобразование диапазона остальных скоростей
}
// Функция создания анимации движения на перекрёстке светофора:
void fnc_CreateImage(bool l, bool f, bool r, int8_t t){ // Параметры: l - наличие дороги влево, f - прямо, r - вправо, t - выбранное направление (L=-1 F=0 R=+1)
// Рисуем дорогу к перекрёстку:
Image_TLIGHT[0]=0x18; Image_TLIGHT[1]=0x18; Image_TLIGHT[2]=0x18; Image_TLIGHT[3]=0x18; Image_TLIGHT[4]=0x18; Image_TLIGHT[5]=0; Image_TLIGHT[6]=0; Image_TLIGHT[7]=0;
// Добавляем дороги от перекрёстка:
if( f ){ Image_TLIGHT[5]|=0x18; Image_TLIGHT[6]|=0x18; Image_TLIGHT[7]|=0x18; }
if( l ){ Image_TLIGHT[3]|=0x1F; Image_TLIGHT[4]|=0x1F; } // Дорога влево
if( r ){ Image_TLIGHT[3]|=0xF8; Image_TLIGHT[4]|=0xF8; } // Дорога вправо
// Добавляем точку движения:
static uint8_t i=0, j=0; // Определяем переменные: i - позиция точки, f - флаг задержки
if( millis()%50 < 25 ){
if( j==0 ){ j=1; i++; if(i>8){i=0;} } // Увеличиваем позицию i
}else{ j=0; } // Сбрасываем задержку
if(i<=3){ Image_TLIGHT[i]&=0xEF; } // Точка на дороге к перекрёстку
else{ if(t==0){ if(i<8){Image_TLIGHT[i]&=0xEF; } } // Точка на дороге от перекрёстка прямо
if(t <0){ if(i<9){Image_TLIGHT[4]&=~(1<<(8-i)); } } // Точка на дороге от перекрёстка влево
if(t >0){ if(i<7){Image_TLIGHT[3]&=~(1<<(i+1)); } } // Точка на дороге от перекрёстка вправо
}
}
// ФУНКЦИЯ ОСТАНОВКИ ПЕРЕД ПРЕПЯТСТВИЕМ:
void fnc_StopObstacle(uint16_t distance){ // Функция получает дистанцию в см, при которой требуется остановиться
// Проверяем наличие препятствия:
if( len.distance() > distance ){ return; } // Выходим из функции, если расстояние до препятствия больше «distance» см
// Определяем время обнаружения препятствия:
uint32_t currentTime = millis(); // Получаем текущее время
// Выполняем остановку:
mot_R.setStop(); // Останавливаем правый мотор
mot_L.setStop(); // Останавливаем левый мотор
disp.print("STOP"); // Загружаем текст «STOP» для бегущей строки
disp.scrollPos(0); // Переходим к началу бегущей строки
disp.autoScroll(245, 300); // Выводим загруженный текст со скоростью 245 и паузой между прокрутками строки в 300 мс
// Ждём освобождения дороги от препятствия:
while( currentTime > millis()-tim_Wait ){ // Пока не пройдёт «tim_Wait» мс после времени «currentTime»
if( len.distance() <= distance+3 ){ // Если до препятствия меньше «distance»+3 см ...
currentTime = millis(); // Обновляем текущее время
}
}
// Информируем о готовности продолжить движение:
disp.clrScr(); // Чистим экран светодиодной матрицы
delay(500);
disp.print("GO"); // Загружаем текст «GO» для бегущей строки
disp.scrollPos(0); // Переходим к началу бегущей строки
disp.autoScroll(245); // Выводим загруженный текст однократно со скоростью 245
delay(1000);
disp.clrScr(); // Чистим экран светодиодной матрицы
}
Попробуйте повернуть знак таким образом, чтобы машинка поймала ИК-сигнал (поставьте его под углом 45°, как все остальные знаки), и посмотрите на поведение машинки.
Она ведёт себя, как на перекрёстке: отрисовывает его изображение и останавливается, словно на светофоре. При этом скетч мы не меняли. Попробуйте ответить на вопрос — почему так происходит?
Обсуждение