В этом уроке:
- Как происходит разворот на перекрёстке
- Реализуем разворот
Видео версия урока
В прошлом уроке мы научили машинку поворачивать на перекрёстке. Сегодня мы научимся выполнять разворот. Его можно производить как на самом перекрёстке, так и просто на линии
Стоит отметить, что разворот всегда производится через левую сторону (как и на реальных дорогах города). Не забывайте об этом
Принцип разворота на перекрёстке
Разворот можно условно разделить на четыре фазы:
- Распознавание перекрёстка;
- Начало разворота. Длится до тех пор, пока хотя бы один датчик фиксирует линию;
- Продолжение разворота. Длится до тех пор, пока ни один датчик не фиксирует линию;
- Завершение разворота. Хотя бы один датчик зафиксировал линию, можно переходить на обычное П-регулирование.
Программная реализация разворота
Давайте рассмотрим код, реализующий непосредственно разворот.
// РАЗВОРОТ:
if (bum.getCross(3, 1000)) flg_Turn = true;// Если обнаружен перекрёсток, устанавливаем флаг разворота
if( flg_Turn ){ // 1 ФАЗА. Если под бампером обнаружен перекрёсток (указываем толщину линии трассы и время на преодоление перекрёстка в мс) ...
// Под бампером обнаружен перекрёсток:
flg_Turn = false; // Сбрасываем флаг разворота
mot_R.setSpeed( min_Speed, MOT_PWM ); // Устанавливаем скорость правого мотора в %
mot_L.setSpeed(-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; // Снимаем наложенные ранее ограничения скорости
}
Поскольку условие для разворота может быть любым (перекрёсток, дорожный знак и пр.), мы присваиваем флагу flg_Turn (27 строка) значение true, при котором выполнится блок кода с разворотом (28 строка).
- 1 ФАЗА. При фиксации перекрёстка начинаем разворот, установив моторам среднюю скорость и разные направления (31-32 строки).
- 2 ФАЗА. Далее с помощью цикла while() мы ждём, пока машинка покинет линию. Этот цикл будет выполняться бесконечно, пока будет истинно условие bum.getLineSum() > 0 (34 строка). В уроке о движении по линии мы уже использовали функцию getLineSum(). Она возвращает число датчиков, под которыми есть линия.
- 3 ФАЗА. Теперь мы ждём, пока машинка вновь въедет на линию (35 строка). Принцип здесь такой же, как и в прошлом шаге.
- 4 ФАЗА. После того, как линия зафиксирована, можно выключить моторы (37-38 строка) и вернуться в основной код, где реализовано управление с помощью П-регулятора.
Также мы изменили переменную val_Turn для того, чтобы машинка на перекрёстке не поворачивала (12 строка).
int8_t val_Turn = 0 ; // Выбранное направление движения на перекрёстке: 0 прямо, -1 влево, +1 вправо
Вставим этот скетч в код, который мы написали на прошлом уроке.
Готовый скетч для разворота на перекрёстке
Для того, чтобы Вы могли свериться со своим скетчем, мы приводим готовый вариант кода.
#include <Wire.h> // Подключаем библиотеку для работы с аппаратной шиной I2C
#include <iarduino_I2C_Motor.h> // Подключаем библиотеку для работы с мотором I2C-flash
#include <iarduino_I2C_Bumper.h> // Подключаем библиотеку для работы с бампером 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
const float min_Speed = 30; // Минимальная скорость движения в %. Используется при движении по дорогам
const float mid_Speed = 60; // Средняя скорость движения в %. Используется при движении по дорогам
float speed = min_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); // Указываем левому мотору, что его вращение должно быть обратным (против часовой стрелки при положительных скоростях)
}
void loop() {
// РАЗВОРОТ:
if (bum.getCross(3, 1000)) flg_Turn = true;// Если обнаружен перекрёсток, устанавливаем флаг разворота
if( flg_Turn ){ // 1 ФАЗА. Если под бампером обнаружен перекрёсток (указываем толщину линии трассы и время на преодоление перекрёстка в мс) ...
// Под бампером обнаружен перекрёсток:
flg_Turn = false; // Сбрасываем флаг разворота
mot_R.setSpeed( min_Speed, MOT_PWM ); // Устанавливаем скорость правого мотора в %
mot_L.setSpeed(-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); } // Если поворот будет осуществляться направо, включаем правый поворотник
// ОПРЕДЕЛЯЕМ ОШИБКУ ЦЕНТРИРОВАНИЯ ЛИНИИ ПОД БАМПЕРОМ:
// Если ожидается перекрёсток: машина должна ехать по центру линии, немного сместившись в сторону поворота
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*(speed-20); // Коэффициент П-регулятора
float P = bum_Error * kP; // Получаем значение от П-регулятора
mot_R.setSpeed(speed - P, MOT_PWM); // Устанавливаем скорость правого мотора
mot_L.setSpeed(speed + P, MOT_PWM); // Устанавливаем скорость левого мотора
} 
Обсуждение