В этом уроке:
- Устанавливаем знак «Железнодорожный переезд со шлагбаумом» на трассу
- Учим машинку реагировать на шлагбаум
- Остановка перед препятствием
Установка знака со шлагбаумом на трассу
Представим, что нам на пути встречается железнодорожный переезд со шлагбаумом. Шлагбаум установлен на соответствующем знаке, который, как и любой другой знак, может передавать информацию ИК-сигналом.
Однако, давайте на этот раз не будем считывать ИК-сигнал от знака, а реализуем остановку автомобиля при опущенном шлагбауме с помощью датчика расстояния. Это также поможет нам не врезаться в другие объекты на трассе, если вдруг они появятся. Мы выбрали такой способ распознавания шлагбаума ещё и из-за технической особенности знаков. Вы же помните, что обычно они устанавливаются под углом 45° к трассе для того, чтобы ИК-сигнал попадал на приёмник машинки?
Для того, чтобы шлагбаум был перпендикулярен дороге, мы устанавливаем знак так, что его сигнал не будет воспринят машинкой. Подробнее об установке шлагбаума можете почитать в соответствующей статье.
Итак, соберите и настройте шлагбаум, если еще этого не сделали, после чего установите его на таким образом, чтобы круглая табличка «STOP» располагалась точно над линией трассы.
![](http://iarduino.ru/img/upload/e28acbeec19129b2d2cdf692d266e766.png)
Поиск препятствий на пути
Для обнаружения препятствий используем ультразвуковой датчик расстояния, поэтому не забываем подключить библиотеку (её мы установили в первой главе) и создать объект датчика:
#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°, как все остальные знаки), и посмотрите на поведение машинки.
Она ведёт себя, как на перекрёстке: отрисовывает его изображение и останавливается, словно на светофоре. При этом скетч мы не меняли. Попробуйте ответить на вопрос — почему так происходит?
Обсуждение