В этом уроке:
- Устанавливаем знак «Железнодорожный переезд со шлагбаумом» на трассу
- Учим машинку реагировать на шлагбаум
- Остановка перед препятствием
Установка знака со шлагбаумом на трассу
В прошлом уроке с мы преодолели перекрёсток и теперь не знаем заранее, в каком направлении поедет автомобиль. Но что-то новое его ждёт только при повороте налево, так что продолжим рассматривать это направление.
Нам снова на пути встречается железнодорожный переезд, но на этот раз со шлагбаумом. Шлагбаум установлен на соответствующем знаке, который, как и любой другой знак, может передавать информацию ИК-сигналом, как мы привыкли.
Однако, давайте на этот раз не будем считывать ИК-сигнал от знака, а реализуем остановку автомобиля при опущенном шлагбауме с помощью датчика расстояния. Это также поможет нам не врезаться в другие объекты на трассе, например, в другие машинки. Мы выбрали такой способ распознавания шлагбаума еще и из-за технической особенности знаков. Вы же помните, что обычно они устанавливаются под углом 45° к трассе для того, чтобы ИК-сигнал попадал на приёмник машинки?
Для того, чтобы шлагбаум был перпендикулярен дороге, мы устанавливаем знак так, что его сигнал не будет воспринят машинкой. Подробнее об установке шлагбаума можете почитать в соответствующей статье.
Итак, соберите и настройте шлагбаум, если еще этого не сделали, после чего установите его на трассу.
Поиск препятствий на пути
Для обнаружения препятствий используем ультразвуковой датчик расстояния, поэтому не забываем подключить библиотеку и создать объект датчика:
#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
Запуск функции будем производить следующим образом, передавая в неё значение минимальной дистанции для остановки (вводим в переменных выше, строка 19):
// ОСТАНОВКА ПЕРЕД ПРЕПЯТСТВИЕМ:
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(); // Чистим экран светодиодной матрицы
}
Алгоритм работы следующий:
В функцию передаётся значение минимального расстояния (67 строка). Происходит вызов функции (строка 227), после чего проверяется условие (230 строка): если расстояние до препятствия больше минимального, то происходит немедленный выход из функции (return). Если нет, то продолжаем, — нужно остановить машинку.
Определяем время обнаружения препятствия (строка 233), останавливаем моторы (строки 236,237), далее как обычно работаем с дисплеем, выводя «STOP».
В строке 243 попадаем в цикл, длящийся до тех пор, пока не пройдёт время ожидания tim_Wait, указанное в начале скетча. После этого снова измеряется расстояние, и, если препятствие еще перед машинкой, — отсчёт времени начинается заново (строка 245).
Иначе машинка продолжает движение (строки 250-256). Все команды здесь Вы уже отлично знаете.
Готовый скетч урока:
#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 I2C-flash
#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 для работы с светодиодной матрицей I2C-flash, указав адрес модуля на шине 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; // Максимальная скорость движения в км/ч. Используется при движении по скоростным дорогам
const uint16_t min_Distance = 15; // Минимальное расстояние до препятствия в см, при котором требуется остановиться
const uint32_t tim_Wait = 1000; // Определяем время ожидания в мс до начала движения после остановки
float speed = mid_Speed; // Указываем, что изначально скорость равна средней
int8_t val_Turn = 0; // Выбранное направление движения на перекрёстке: 0 прямо, -1 влево, +1 вправо
bool flg_TLIGHT = false; // Флаг получения данных от светофора
bool flg_CrossWait = false ; // Флаг ожидания перекрёстка (машина «увидела» светофор или знак, сообщающий о наличии перекрёстка)
bool flg_CrossFind = false; // Флаг обнаружения перекрёстка (бампер заехал на перекрёсток)
bool flg_Turn = false; // Флаг необходимости разворота
bool flg_25 = false; // Флаг повторной фиксации знака 2.5 «Движение без остановки запрещено»
uint32_t timeSign; // Время для отсчёта времени перед повторной фиксацией знака
byte Image_TLIGHT[8]; // Генерируется функцией fnc_CreateImage()
// 1.12.1 «Опасные повороты» // Изображение знака «Опасные повороты»
byte Image_1_12_1[8] = { 0b00000000, //
0b00000010, // #
0b00100010, // # #
0b01010010, // # # #
0b01001010, // # # #
0b01000100, // # #
0b01000000, // #
0b00000000 }; //
// 3.31 «Конец зоны ограничений» //
byte Image_3_31[8] = { 0b00111100, // ####
0b01000010, // # #
0b10000101, // # # #
0b10001001, // # # #
0b10010001, // # # #
0b10100001, // # # #
0b01000010, // # #
0b00111100 }; // ####
void setup() {
mot_R.begin(); // Инициируем работу с левым мотором I2C-flash
mot_L.begin(); // Инициируем работу с правым мотором I2C-flash
bum.begin(); // Инициируем работу с бампером I2C-flash
disp.begin(); // Инициируем работу с светодиодной матрицей I2C-flash
disp.codingDetect("п"); // Выполняем автоопределение кодировки скетча
mot_R.setDirection(true); // Указываем правому мотору, что его вращение должно быть прямым (по часовой стрелке при положительных скоростях)
mot_L.setDirection(false); // Указываем левому мотору, что его вращение должно быть обратным (против часовой стрелки при положительных скоростях)
ir.begin(); // Инициируем работу с ИК-приёмником/передатчиком I2C-flash
ir.setProtocol(IR_IARDUINO); // Указываем протокол для приёма/передачи данных по ИК-каналу
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( 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( 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( 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(!strcmp(ir.sign_str, "1.12.1")){ // Если номер знака 1.12.1 - «Опасные повороты»
speed = min_Speed; // Ограничиваем скорость до ближайшего перекрёстка или нового знака
disp.drawImage(Image_1_12_1); // Выводим изображение знака
}
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, "1.23" ) ){ // Eсли номер знака 1.23 - «Дети»
disp.scrollPos(0); // Переходим к началу бегущей строки
disp.print("Дети"); // Загружаем текст для бегущей строки
disp.autoScroll(245); // Выводим загруженный текст однократно со скоростью 245
speed = min_Speed; // Ограничиваем скорость до ближайшего перекрёстка или нового знака
}
if( !strcmp( ir.sign_str, "3.31" ) ){ // Если обнаружен знак ПДД 3.31 «Конец зоны ограничений»
disp.drawImage(Image_3_31); // Выводим изображение знака
speed = mid_Speed; // Снимаем наложенные ранее ограничения скорости
}
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 + 3000 < millis()){ // Если флаг установлен и с момента последней фиксации прошло больше 3 сек
flg_25 = false; // Сбрасываем флаг
disp.setTimeIdleFirst(100); // Указываем бегущей строке задерживаться на первом символе в течении 100 мс (допускаются значения от 0 до 2550 мс)
disp.setTimeIdleLast(100); // Указываем бегущей строке задерживаться на последнем символе в течении 100 мс (допускаются значения от 0 до 2550 мс)
disp.scrollPos(0); // Переходим к началу бегущей строки
disp.print("STOP"); // Загружаем текст для бегущей строки
disp.autoScroll(245); // Выводим загруженный текст однократно со скоростью 245
mot_R.setSpeed( 0, MOT_PWM ); // Выключаем правый мотор
mot_L.setSpeed( 0, MOT_PWM ); // Выключаем левый мотор
delay(2000); // Задержка 2 сек (остановка)
}
// ОПРЕДЕЛЯЕМ ОШИБКУ ЦЕНТРИРОВАНИЯ ЛИНИИ ПОД БАМПЕРОМ:
// Если ожидается перекрёсток: машина должна ехать по центру линии, немного сместившись в сторону поворота
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°, как все остальные знаки), и посмотрите на поведение машинки.
Она ведёт себя, как на перекрёстке: отрисовывает его изображение и останавливается, словно на светофоре. При этом скетч мы не меняли. Попробуйте ответить на вопрос — почему так происходит?
Обсуждение