В этом уроке:
- Добавляем перекрёсток на трассу
- Проверяем работу машинки с перекрёстком
- Структурируем код перед дальнейшей работой
Создание перекрёстка и проверка работы машинки
На данный момент наша машинка знает два знака, один из которых «Ограничение максимальной скорости». Пока что действие этого знака никак не отменяется, и машинка постоянно движется со скоростью 20 км/ч, один раз увидев данный знак.
Однако, в скетче мы уже задали условие отмены действия знака: при обнаружении перекрёстка дисплей очищается (59 строка), а скорость снова становится равной 60 км/ч (69 строка).
Фрагмент скетча из предыдущего урока
// ПРОВЕРКА НАЛИЧИЯ ПЕРЕКРЁСТКОВ:
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; // Снимаем наложенные ранее ограничения скорости
}
}
Давайте добавим перекрёсток и проверим, что действие знака действительно отменяется после его проезда.
Для создания дополнительной дороги Вы можете использовать изоленту черного цвета, наклеив её на трассу, или перманентный маркер, нарисовав полоску примерно такой же толщины, как и линия трассы.
Обратите внимание, что начало перекрёстка перпендикулярно трассе, а съёзд с этой дороги плавный. При этом места, где начинается и заканчивается наша дополнительная дорога, делят трассу на три примерно равные части.
Поставьте машинку на трассу и проверьте её работу.
Отлично: теперь она увеличивает скорость после проезда перекрёстка. Но если Вы оставите её проезжать второй круг, то заметите, что скорость уже не изменилась, т.е. 69 строка кода не выполнилась во второй раз. Догадались, почему так произошло?
В следующем уроке мы ответим на этот вопрос.
Более удобный способ записи кода программы
Прежде чем двигаться дальше, давайте сделаем небольшое отступление и поговорим о конструкции, которая позволит навести порядок в нашем коде.
Поскольку мы предполагаем использование довольно большого разнообразия знаков, предлагаем воспользоваться конструкцией switch-case, чтобы удобно записывать информацию о них в коде программы.
Мы уже использовали данную конструкцию, когда создавали игру, но теперь пришла пора подробнее разобраться в этой записи.
switch (var) {
case label:
//выполняется, когда var равно 1
break;
case label:
//выполняется, когда var равно 12
break;
default:
// выполняется, если не выбрана ни одна альтернатива
// default необязателен
}
В данной конструкции выполняются кейсы (первый кейс — строки 2-4, второй — строки 5-7 и т.д). В случае, если метка этого кейса label равна переменной var в операторе switch, то выполняется код данного кейса.
Проще говоря, — происходит сравнение var и label, и, если они равны, кейс выполняется.
Если не выполнился ни один кейс, то выполнится кейс «По умолчанию», записанный как default (8-10 строка). Его использование не обязательно.
Оператор break (4, 7 строки) служит для выхода из кейса после его выполнения.
Использовать данную конструкцию очень удобно, когда в проект постоянно добавляются новые функции. Теперь нам будет удобнее ориентироваться в скетче, а его запись станет более структурированной. По сути, для добавления новой функции в программу достаточно всего лишь вставить в неё новый кейс, при этом не нужно искать нужное место в коде или вносить правки построчно в разные части программы.
Аналог записи через if
Давайте рассмотрим один и тот же код программы, записанный через хорошо знакомый нам оператор if и через switch.
Совсем скоро нам необходимо будет знать, от какого модуля получен сигнал: знак это, светофор или другой автомобиль. Информация об источнике сигнала хранится в переменной device, обратиться к которой можно следующим образом: ir.device.
Блок кода для этой задачи будет выглядеть следующим образом:
if (ir.device == MODUL_TLIGHT){
// Данные отправлены светофором
}
else if (ir.device == MODUL_SIGN){
// Данные отправлены дорожным знаком
}
else if (ir.device == MODUL_CAR){
// Данные отправлены другим автомобилем
}
Аналогично записанный код через switch будет выглядеть так:
switch (ir.device) {
case MODUL_TLIGHT:
// Данные отправлены светофором
break;
case MODUL_SIGN:
// Данные отправлены дорожным знаком
break;
case MODUL_CAR:
// Данные отправлены другим автомобилем
break;
}
Программа, написанная с помощью этого оператора, зачастую лучше читаема, а это особенно важно в больших проектах, которые состоят из более чем сотни строчек кода. Это как раз наш случай. Поэтому, чтобы не путаться в дальнейшем, давайте введём такую форму записи уже сейчас.
Скетч, записанный с новой конструкцией
Данный скетч не отличается функционалом от скетча предыдущего урока. Мы лишь добавили в него нашу новую конструкцию, чтобы в следующем уроке в нём было не так много изменений.
#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) ){ // Если принят пакет данных от знака
switch (ir.device) {
case MODUL_TLIGHT: // ПРИНЯТЫ ДАННЫЕ ОТ МОДУЛЯ СВЕТОФОР:
// здесь пока пусто, Светофора нет
break;
case MODUL_SIGN: // Данные отправлены дорожным знаком
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; // Устанавливаем флаг знака
}
}
break;
case MODUL_CAR: // Данные отправлены другим автомобилем
// Здесь пока пусто, с другими машинками не взаимодействуем
break;
}
}
// Срабатывание по времени знака «Движение без остановки запрещено»
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); // Преобразование диапазона остальных скоростей
} 
Обсуждение