В этом уроке:
- Устанавливаем знак на трассу
- Учимся распознавать знак и снижать после него скорость
Установка знака на трассу
В прошлом уроке мы научились принимать информацию от знака. Пришла пора добавить его распознавание в наш скетч, чтобы в зависимости от информации, передаваемой знаком, корректировать поведение машинки.
Установите знак, как показано на рисунке. Обратите внимание, что направление, в котором должен быть направлен знак, указано стрелкой, а крестовина знака должна стоять так же, как изображён синий крестик.
Распознаём знак
При обнаружении знака 3.24 «Ограничение максимальной скорости», машинка должна изменить скорость и продолжить движение со скоростью, указанной на знаке.
Итак, нам необходимо:
- Двигаясь по линии, получить сигнал от знака (движение по линии мы реализовали в предыдущей главе, чуть позже возьмём оттуда код за основу);
- Если номер знака 3.24, то нужно снизить скорость.
В прошлом уроке мы научились принимать номер знака. Он выглядит так:
3.24.2
- 3.24 — номер знака, который нам нужно «достать» из этой строки, чтобы сравнить с номером в нашем скетче;
- Последняя цифра 2 — пункт знака, а в случае со знаком ограничения скорости — скорость (обозначает десятки). У нас это скорость 20 км/ч.
// ПРОВЕРКА НАЛИЧИЯ ПРИНЯТЫХ ИК-ДАННЫХ:
if( ir.check(true) ){ // Если принят пакет данных от знака
if( !strncmp( ir.sign_str, "3.24", 4) ){ // Обнаружен знак ПДД 3.24 «Ограничение максимальной скорости»:
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»
}
}
ir.sign_str — строка, полученная от знака и хранящая в себе его номер. Однако, мы не можем сравнить эту строку со строкой номера знака («3.24») с помощью «=», как привыкли делать с числами. По сути, нам нужно сравнить строки посимвольно, и, если все символы совпадают, значит, строки равны. Если прописывать этот код вручную, он получится довольно громоздким. Хорошо, что для этой задачи есть специальная функция.
Функция strcmp() (70 строка) сравнивает номер знака (ir.sign_str) с номером, который мы указали (3.24). Третий аргумент — цифра 4 — означает, что сравниваются первые 4 символа строк. С данным знаком поступаем именно так, потому что шестой символ (после точки) обозначает скорость, которую передаёт знак. С другими знаками этого делать не потребуется, и функция будет иметь два аргумента.
Если строки совпали, функция strcmp() возвращает 0. Чтобы условие if срабатывало при совпадении строк, мы инвертируем её значение (оператор !).
Отлично, у нас есть условие, которое сработает при совпадении номера, переданного знаком, с номером, который мы указали в скетче. Теперь нужно в зависимости от этого номера изменить скорость машинки. Для этого нужно узнать, какую скорость передаёт знак.
Запись ir.sign[2] (71-74 строки) позволяет получить пункт знака или, в нашем случае, информацию об указанной на нём скорости. Например, цифра 4 обозначает ограничение скорости 40 км/ч.
Готовый скетч урока
В прошлой главе мы написали скетч, который сейчас используем как основу. Нам остаётся добавить туда наше новое условие, в результате чего получаем готовую программу.
#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
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_IR ir(0x09); // Объявляем объект ir для работы с функциями и методами библиотеки iarduino_I2C_IR, указывая адрес модуля на шине I2C
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 = true ; // Флаг ожидания перекрёстка (машина «увидела» светофор или знак, сообщающий о наличии перекрёстка)
bool flg_CrossFind = false; // Флаг обнаружения перекрёстка (бампер заехал на перекрёсток)
bool flg_Turn = false; // Флаг необходимости разворота
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); // Указываем протокол для приёма/передачи данных по ИК-каналу
}
void loop() {
// РАЗВОРОТ:
//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) ){ // Если под бампером обнаружен перекрёсток (указываем толщину линии трассы и время на преодоление перекрёстка в мс) ...
if( flg_CrossWait ){ // Если ожидается появление перекрёстка ...
flg_CrossWait = false; // Сбрасываем флаг ожидания перекрёстка
flg_CrossFind = true; // Устанавливаем флаг обнаружения перекрёстка
}
}
else{ // Если под бампером обычная линия ...
if (flg_CrossFind) { // Если ранее был обнаружен перекрёсток
flg_CrossFind = false; // Сбрасываем флаг обнаружения перекрёстка
val_Turn = 0; // Выбираем движение прямо
speed = mid_Speed; // Снимаем наложенные ранее ограничения скорости
}
}
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) ){ // Если принят пакет данных от знака
if( !strncmp( ir.sign_str, "3.24", 4) ){ // Обнаружен знак ПДД 3.24 «Ограничение максимальной скорости»:
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»
}
}
// ОПРЕДЕЛЯЕМ ОШИБКУ ЦЕНТРИРОВАНИЯ ЛИНИИ ПОД БАМПЕРОМ:
// Если ожидается перекрёсток: машина должна ехать по центру линии, немного сместившись в сторону поворота
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); // Преобразование диапазона остальных скоростей
}
Мы задали основные скорости движения машинки, принятые на дорогах: 20, 40, 60 и 90 км/ч (11-14 строки).
const float min_Speed = 20; // Минимальная скорость движения в км/ч. Используется при движении вблизи школ, а также на опасных участках дороги const float low_Speed = 40; // Низкая скорость движения в км/ч. Используется при движении по опасным участкам дороги или при соответствующем знаке const float mid_Speed = 60; // Средняя скорость движения в км/ч. Используется при движении по дорогам const float max_Speed = 90; // Максимальная скорость движения в км/ч. Используется при движении по скоростным дорогам float speed = mid_Speed; // Указываем, что изначально скорость равна средней
Обратите внимание, что изначально мы указали движение со средней скоростью (15 строка), поскольку машинка движется в населенном пункте, — значит, скорость ограничена 60 км/ч.
Мы закомментировали (фактически удалили из кода) условие, по которому начинается разворот (33 строка). В дальнейшем разворот будет происходить только при встрече соответствующего знака, а пока он нам не нужен.
//if (bum.getCross(3, 1000)) flg_Turn = true; // Если обнаружен перекрёсток, устанавливаем флаг разворота
Не забываем подключить библиотеку (4 строка), создать объект ИК-приёмопередатчика (9 строка), инициализировать его (27 строка) и указать протокол передачи данных (28 строка). Всё это мы делали ранее, поэтому подробных пояснений здесь не даём.
Обсуждение