В этом уроке:
- Устанавливаем знак и машинку на трассу
- Учимся распознавать знак и снижать после него скорость
Установка знака на трассу
В прошлом уроке мы научились принимать информацию от знака. Пришла пора добавить его распознавание в наш скетч и в зависимости от знака корректировать поведение машинки.
Установите знак на отведённое для него место. Обратите внимание, что направление знака указано стрелкой. Крестовину знака нужно поставить так, как показано на рисунке синим крестиком.
Определяем номер знака
Знак 1.12.1 «Опасные повороты» предполагает, что водителю необходимо снизить скорость перед преодолением сложного участка дороги. Итак, нам необходимо:
- Двигаясь по линии, получить сигнал от знака (это мы уже реализовывали ранее, используем готовый скетч);
- Если номер знака 1.12.1, то нужно снизить скорость.
// ПРОВЕРКА НАЛИЧИЯ ПРИНЯТЫХ ИК-ДАННЫХ:
if( ir.check(true) ){ // Если принят пакет данных от знака
if(!strcmp(ir.sign_str, "1.12.1")){ // Если номер знака 1.12.1 - «Опасные повороты»
speed = min_Speed; // Ограничиваем скорость до ближайшего перекрёстка или нового знака
}
}
ir.sign_str — строка, полученная от знака и хранящая в себе его номер. Однако, мы не можем сравнить эту строку со строкой знака («1.12.1») с помощью «=», как привыкли делать это с числами.
По сути, нам нужно сравнить строки посимвольно, и если все символы совпадают, значит, строки равны. Если прописывать этот код вручную, он получится довольно громоздким. Хорошо, что для этой задачи есть специальная функция.
Функция strcmp() (71 строка) сравнивает номер знака (ir.sign_str) с номером, который мы указали (1.12.1). Если строки совпали, функция возвращает 0. Поэтому мы инвертируем её значение (оператор !), чтобы условие if срабатывало при совпадении строк.
Таким образом, при выполнении условия мы присваиваем переменной speed новое значение минимальной скорости (72 строка), указанное выше в скетче.
Готовый скетч урока
В прошлой главе мы написали скетч, который сейчас используем как основу. Нам остаётся добавить туда наше новое условие, в результате чего получаем готовую программу.
#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; // Указываем, что изначально скорость равна средней
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(!strcmp(ir.sign_str, "1.12.1")){ // Если номер знака 1.12.1 - «Опасные повороты»
speed = min_Speed; // Ограничиваем скорость до ближайшего перекрёстка или нового знака
}
}
// ОПРЕДЕЛЯЕМ ОШИБКУ ЦЕНТРИРОВАНИЯ ЛИНИИ ПОД БАМПЕРОМ:
// Если ожидается перекрёсток: машина должна ехать по центру линии, немного сместившись в сторону поворота
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; // Указываем, что изначально скорость равна средней
Обратите внимание, что изначально мы указали движение со средней скоростью (16 строка), поскольку машинка движется в населенном пункте, а значит скорость ограничена 60км/ч.
Мы закомментировали (фактически удалили из кода) условие, по которому начинается разворот (34 строка). В дальнейшем разворот будет происходить только при встрече соответствующего знака, а пока он нам не нужен.
//if (bum.getCross(3, 1000)) flg_Turn = true; // Если обнаружен перекрёсток, устанавливаем флаг разворота
Кроме того, помните, в предыдущих уроках после проезда перекрёстка мы увеличивали скорость? Действие многих знаков распространяется только до ближайшего перекрёстка, поэтому данный блок кода мы не будем менять. Когда машинка проедет опасный участок дороги и доберётся до перекрёстка, её скорость снова увеличится до 60км/ч (61 строка), т.е., фактически, действие знака «Опасные повороты» будет отменено.
if (flg_CrossFind) { // Если ранее был обнаружен перекрёсток
flg_CrossFind = false; // Сбрасываем флаг обнаружения перекрёстка
val_Turn = 0; // Выбираем движение прямо
speed = mid_Speed; // Снимаем наложенные ранее ограничения скорости
} 
Обсуждение