В этом уроке:
- Устанавливаем знак «Движение без остановки запрещено» на трассу
- Учимся откладывать момент выполнения действия
- Добиваемся корректной работы со знаком
Установка знака на трассу
На данном этапе наша машинка движется со скоростью 20 км/ч после знака «Ограничение максимальной скорости». Скоро мы добавим перекрёсток, который будет отменять действие этого знака, а пока давайте установим на нашу трассу еще один знак: «Движение без остановки запрещено». Не забывайте предварительно его собрать и запрограммировать, если еще этого не сделали. Теперь трасса выглядит следующим образом:
Учим машинку корректно реагировать на знак
С прошлым знаком мы выполняли ключевое действие (изменение скорости) сразу же при фиксации знака. При этом не забывайте, что его регистрация происходит не один раз, а многократно — несколько раз в секунду. В случае, когда мы говорим об изменении скорости, в этом нет проблемы. Переменная скорости просто будет перезаписываться на новое (такое же) значение.
Однако, в данном случае автомобиль должен остановиться перед знаком. Причём сделать это не сразу при фиксации знака, а чуть позже, ведь машинка зафиксирует знак примерно за 20 см до переезда.
Таким образом, если мы используем наш стандартный алгоритм, машинка зафиксирует знак, остановится, тронется с места, снова остановится и так, рывками, будет выбираться из зоны действия знака. Поэтому нам необходимо немного усложнить алгоритм работы с данным знаком. Давайте посмотрим код:
if( !strcmp( ir.sign_str, "2.5" )){ // Если номер знака 2.5 - «Движение без остановки запрещено»
if (!flg_25) { // Если не установлен флаг знака (знак зафиксирован первый раз)
timeSign = millis(); // Сбрасываем значение переменной времени обнаружения знака
flg_25 = true; // Устанавливаем флаг знака
}
При фиксации знака (86 строка) проверяем, не зафиксировали ли мы его ранее (87 строка). Если нет, отмечаем время и меняем флаг (88, 89 строки).
Далее, уже независимо от приёма сигнала от знака, проверяем, прошло ли время (2 секунды) с момента установки флага (обнаружения знака). Если да, то через 2 секунды условие сработает, а флаг сбросится и начнётся выполнение основного кода знака:
// Срабатывание по времени знака «Движение без остановки запрещено»
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(2000); // Задержка 2 сек (остановка)
}
На дисплей выводим надпись «STOP» (97 строка). Как это делается, мы разбирали в прошлом уроке.
Далее останавливаем моторы (99, 100 строки) и выполняем задержку (101 строка). Мы используем delay(), хотя ранее писали, что его использование крайне нежелательно — контроллер не может параллельно выполнять никаких действий. Но в данном случае мы уверены, что никаких действий выполнять не нужно, — машинка просто стоит на месте. Поэтому в данной ситуации мы упростили код, обойдясь без введения новых переменных для другого таймера.
Меняя значения времени в 95 и 101 строках, можно изменить время движения после первого обнаружения знака и остановки соответственно. Вам наверняка потребуется сделать это, если у Вас установлено напряжение питания моторов выше минимального (подробнее об этом мы писали в первой главе).
Загрузите получившийся скетч в машинку и проверьте, что она правильно работает и реагирует на знак, а на дисплей выводится надпись «STOP».
Расширяем функционал программы
Сейчас наш автомобиль останавливается, когда встречает знак «Движение без остановки запрещено». В реальной жизни этот знак необходим для того, чтобы водитель убедился в безопасности проезда. Давайте вместо обычной остановки (101 строка с функцией delay()) сделаем так, чтобы наша машинка немного «повиляла» из стороны в сторону, как бы осмотревшись по сторонам.
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);
}
После обычной остановки с delay() мы добавили циклы с прямой установкой скорости для моторов. Все используемые функции Вы отлично знаете, и, уверены, без проблем разберётесь в коде.
Готовый скетч урока:
#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
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
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; // Флаг необходимости разворота
bool flg_25 = false; // Флаг знака 2.5 «Движение без остановки запрещено»
uint32_t timeSign; // Время для задержки срабатывания знака 2.5
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 мс)
}
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) ){ // Если под бампером обнаружен перекрёсток (указываем толщину линии трассы и время на преодоление перекрёстка в мс) ...
disp.clrScr(); // Чистим экран светодиодной матрицы
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 «Ограничение максимальной скорости»:
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; // Устанавливаем флаг знака
}
}
}
// Срабатывание по времени знака «Движение без остановки запрещено»
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); // Преобразование диапазона остальных скоростей
} 
Обсуждение