Работаем со знаком «Движение без остановки запрещено»

В этом уроке: 

  • Устанавливаем знак «Движение без остановки запрещено» на трассу
  • Учимся откладывать момент выполнения действия
  • Добиваемся корректной работы со знаком 

Установка знака на трассу

На данном этапе наша машинка движется со скоростью 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);          // Преобразование диапазона остальных скоростей
}
Поздравляю с изучением данного урока!
Следующий урок:
№10. Работа с перекрёстком.
приступить к изучению

Обсуждение

Гарантии и возврат. Используя сайт, Вы соглашаетесь с условиями.