В этом уроке:
- Установка знака «Въезд запрещён»
- Учим машинку реагировать на знак
Установка знака на трассу
В прошлом уроке наш автомобиль преодолел ЖД-переезд и движется дальше, но на пути у него непроходимая дорога. Нужно разворачиваться и ехать назад.
Давайте соберём и запрограммируем знак, если ещё этого не сделали, после чего установим его на трассу и научим машинку реагировать на него.
Учим машинку реагировать на знак
В уроке про разворот на перекрёстке мы уже обучили нашу машинку совершать разворот. Там условием начала разворота был перекрёсток. По сути, при встрече данного знака нам нужно сделать то же самое, просто условием для разворота будет не наличие перекрёстка, а знак.
Итак, при фиксации знака выводим на дисплей его изображение и меняем флаг необходимости разворота:
if( !strcmp( ir.sign_str, "3.1" )){ // Обнаружен знак ПДД 3.1 «Въезд запрещен»:
disp.drawImage(Image_3_1);
flg_Turn = true; // Устанавливаем флаг разворота
bum.setTurnSignal(BUM_TURN_LEFT ); // Включаем левый поворотник
}
Изображение знака представлено массивом:
// 3.1 «Въезд запрещен»
byte Image_3_1[8] = { 0b00111100, // ####
0b01000010, // # #
0b10000001, // # #
0b10111101, // # #### #
0b10111101, // # #### #
0b10000001, // # #
0b01000010, // # #
0b00111100 }; // ####
Сам скетч разворота мы уже с Вами хорошо знаем. Вернитесь к уроку о развороте, если нужны подробные пояснения, как он работает. Мы лишь добавили в него функцию очистки дисплея и отключения поворотников.
disp.clrScr(); // Чистим экран светодиодной матрицы bum.setTurnSignal(BUM_TURN_OFF); // Отключаем поворотники
Готовый скетч урока
#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 }; // ####
// 3.1 «Въезд запрещен»
byte Image_3_1[8] = { 0b00111100, // ####
0b01000010, // # #
0b10000001, // # #
0b10111101, // # #### #
0b10111101, // # #### #
0b10000001, // # #
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( flg_Turn ){ // 1 ФАЗА. Если под бампером обнаружен перекрёсток (указываем толщину линии трассы и время на преодоление перекрёстка в мс) ...
flg_Turn = false; // Сбрасываем флаг разворота
mot_R.setSpeed( convertSpeed(low_Speed), MOT_PWM ); // Устанавливаем скорость правого мотора в %
mot_L.setSpeed(-convertSpeed(low_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; // Снимаем наложенные ранее ограничения скорости
disp.clrScr(); // Чистим экран светодиодной матрицы
bum.setTurnSignal(BUM_TURN_OFF); // Отключаем поворотники
}
// ПРОВЕРКА НАЛИЧИЯ ПЕРЕКРЁСТКОВ:
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; // Устанавливаем флаг знака
}
}
if( !strcmp( ir.sign_str, "3.1" )){ // Обнаружен знак ПДД 3.1 «Въезд запрещен»:
disp.drawImage(Image_3_1);
flg_Turn = true; // Устанавливаем флаг разворота
bum.setTurnSignal(BUM_TURN_LEFT ); // Включаем левый поворотник
}
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(); // Чистим экран светодиодной матрицы
} 
Обсуждение