Подержите перед этим роботом книгу, и он подъедет к ней и остановится в 20 см от неё. Отодвиньте книгу — робот поедет за ней. Придвиньте её ближе — робот отъедет назад. Скорость робот выбирает по тому, насколько он далёк от отметки 20 см: когда далеко — едет быстро, когда почти на месте — медленно. Это пропорциональный регулятор, или П-регулятор. С помощью таких регуляторов многие устройства поддерживают нужную скорость, температуру или положение. Вы узнаете, что такое ошибка и коэффициент усиления и как подобрать этот коэффициент, а также зачем роботу минимальная скорость и мёртвая зона. Это несложный проект, на него уйдёт около 45 минут.
Что понадобится
Arduino Uno R3Есть в симуляторе× 1Цену смотрите в магазине
Модуль драйвера моторов L298NЕсть в симуляторе× 1Цену смотрите в магазине
Мотор-редуктор TT 1:48 с колесом 65 ммЕсть в симуляторе× 2Цену смотрите в магазине
Батарейный отсек 4 × AAЕсть в симуляторе× 1Цену смотрите в магазине
Шаровая опора 20 ммЕсть в симуляторе× 1Цену смотрите в магазине
Ультразвуковой датчик HC-SR04Есть в симуляторе× 1Цену смотрите в магазине
Набор проводов-перемычек× 1Цену смотрите в магазине
Это тот же робот, что и в проекте «Робот, объезжающий препятствия»: машина с HC-SR04 спереди. Новый здесь только скетч. В качестве цели книга или небольшая коробка подходят лучше руки: твёрдые плоские поверхности возвращают более сильное эхо.
Как это работает
Ошибка: насколько робот отклонился от цели
Робот хочет находиться в 20 см от предмета. Желаемое расстояние — это цель, в скетче она называется TARGET_CM. После каждого измерения робот вычисляет ошибку:
ошибка = расстояние - 20
На расстоянии 30 см ошибка равна 10: робот слишком далеко и должен ехать вперёд. На 20 см ошибка равна 0: робот там, где нужно. На 15 см ошибка равна -5: робот слишком близко и должен ехать назад.
Коэффициент усиления: насколько сильно реагировать
Скорость — это ошибка, умноженная на коэффициент усиления KP. В этом скетче он равен 12:
скорость = 12 × ошибка
Большая ошибка даёт большую скорость, а маленькая — маленькую. Поэтому вдали от цели робот спешит, а по мере приближения замедляется.
Три ограничения для настоящих моторов
- Наибольшая скорость. Скорость удерживается в пределах от -255 до 255, то есть до наибольшего значения для широтно-импульсной модуляции (ШИМ). На расстоянии от 42 до 150 см робот едет на полной скорости.
- Минимальная скорость. Ниже примерно 90 моторы TT только гудят и не крутятся. Поэтому маленькая скорость поднимается до 90, а при езде назад — до -90.
- Мёртвая зона. Когда ошибка не больше 2 см, то есть на расстоянии от 18 до 22 см, скорость равна 0. Без мёртвой зоны робот бесконечно дёргался бы то вперёд, то назад около отметки 20 см: показания каждый раз немного различаются, а его самая маленькая скорость — 90.
И ещё одно правило: если в пределах 150 см ничего нет, робот ждёт. Он не станет гнаться за стеной через всю комнату.
| Расстояние | Ошибка | 12 × ошибка | Скорость, которую получают моторы |
|---|---|---|---|
| 10 см | -10 | -120 | -120: назад |
| 17 см | -3 | -36 | -90: поднята до минимума |
| от 18 до 22 см | от -2 до 2 | 0: мёртвая зона, робот стоит | |
| 25 см | 5 | 60 | 90: поднята до минимума |
| 30 см | 10 | 120 | 120 |
| 40 см | 20 | 240 | 240 |
| 60 см | 40 | 480 | 255: максимум |
| 200 см | 0: в пределах 150 см ничего нет, робот ждёт |
Что меняет KP
- KP слишком маленький, например 4: вблизи цели все расчётные скорости меньше 90, поэтому робот ползёт на минимальной скорости. Он вялый и отстаёт от движущейся цели.
- KP слишком большой, например 30: уже ошибка в 9 см даёт полную скорость. Робот несётся, не успевает вовремя остановиться, проскакивает 20 см, и ему приходится возвращаться. Часто он раскачивается вперёд-назад. Такие раскачивания называют колебаниями.
- KP в самый раз: робот быстро едет к цели, замедляется и останавливается около 20 см без раскачки.
Почему при большом KP робот раскачивается? Он не может остановиться сразу: ещё мгновение он катится, а моторам нужно время, чтобы замедлиться. В мёртвой зоне моторы вращаются по инерции, поэтому робот немного прокатывается дальше. Кроме того, он вносит поправку всего около 20 раз в секунду. При большом KP робот успевает проскочить цель ещё до следующей поправки.
Сборка
1. Возьмите робота в симуляторе
Робот и его подключение такие же, как в проекте «Робот, объезжающий препятствия»: если вы уже собрали его в игре, возьмите его. Если нет, начните с Obstacle avoider, машины, которая есть в игре с самого начала, и подключите её провода, как в таблице на шаге 2.
В реальной сборке: возьмите свою машину из проекта «Робот, объезжающий препятствия». Проверьте, что HC-SR04 стоит ровно и смотрит прямо вперёд.
2. Проверьте подключение
| Откуда | Куда | Зачем |
|---|---|---|
| Батарея + | L298N +12V | Питание моторов: около 6 В от четырёх батареек AA |
| Батарея - | L298N GND | Минус батареи |
| L298N +5V | Uno 5V | Стабилизатор драйвера питает Uno: около 4,4 В |
| L298N GND | Uno GND | Общая земля: сигналам нужен общий 0 В |
| Uno D5 | L298N ENA | Скорость левого мотора (ШИМ); перемычка ENA снята |
| Uno D4 | L298N IN1 | Направление левого мотора |
| Uno D7 | L298N IN2 | Направление левого мотора |
| Uno D8 | L298N IN3 | Направление правого мотора |
| Uno D12 | L298N IN4 | Направление правого мотора |
| Uno D6 | L298N ENB | Скорость правого мотора (ШИМ); перемычка ENB снята |
| Левый мотор M+ | L298N OUT1 | |
| Левый мотор M- | L298N OUT2 | |
| Правый мотор M+ | L298N OUT4 | Переставлено специально: этот мотор стоит зеркально |
| Правый мотор M- | L298N OUT3 | Переставлено специально: этот мотор стоит зеркально |
| HC-SR04 VCC | L298N +5V | 5 В для датчика |
| HC-SR04 GND | Uno GND | Земля |
| Uno D9 | HC-SR04 TRIG | Запускает измерение: импульс 10 мкс |
| Uno D10 | HC-SR04 ECHO | Импульс длиной во время полёта эха туда и обратно |
В реальной сборке: если ваша машина уже работает как робот, объезжающий препятствия, значит, подключение правильное. Остаётся только загрузить новый скетч.
3. Загрузите и проверьте
Откройте «Код», вставьте скетч KeepDistance с этой страницы, нажмите «Проверить», а затем «Загрузить».
В реальной сборке: поставьте машину на коробку, чтобы колёса висели в воздухе, и загрузите скетч. Откройте монитор порта (Serial Monitor) на скорости 9600 бод. Держите книгу в 35 см перед датчиком: колёса закрутятся вперёд, а монитор покажет скорость 180. Придвиньте книгу, чтобы до датчика осталось около 20 см, — и колёса остановятся. Если поднести её ближе 18 см, колёса закрутятся назад.
4. Пусть робот следует за целью
Запустите заезд на арене и откройте монитор порта. В каждой строке — расстояние и скорость, например 35 cm, speed 180. Если в пределах 150 см перед роботом ничего нет, он ждёт и выводит скорость 0. Когда в зоне действия датчика оказывается стена или коробка, робот подъезжает к ней и останавливается примерно в 20 см от неё.
В реальной сборке: поставьте машину на пол и держите перед ней книгу. Медленно отходите назад — робот поедет следом. Остановитесь — остановится и он. Придвиньте книгу к роботу — он отъедет назад.
5. Настройте KP
Попробуйте KP 4, 12 и 30 и сравните то, что увидите, со списком в разделе «Что меняет KP» выше. Меняйте по одному числу за раз, загружайте скетч, проверяйте и записывайте, что получилось. Пока робот успокаивается, следите за монитором порта. Если скорость всё время становится то положительной, то отрицательной, робот колеблется: уменьшите KP. Если скорость долго держится на 90 или -90, робот вялый: увеличьте KP. Выберите KP чуть меньше того значения, при котором начинается раскачка.
Подберите и MIN_SPEED. Это должна быть самая маленькая скорость, при которой машина действительно едет. Проверяйте её на полу, где колёсам приходится везти робота. Если при 90 моторы только гудят, попробуйте 100 или 110.
На настоящем роботе лучший KP может оказаться другим: батарейки, пол и моторы влияют на то, как робот реагирует.
Код
// Keep-distance robot: it stays TARGET_CM away from whatever is in front of it.
// Put your hand in front of it and move: it follows. A proportional (P) controller.
// L298N: ENA -> D5, IN1 -> D4, IN2 -> D7 (left), ENB -> D6, IN3 -> D8, IN4 -> D12 (right)
// HC-SR04: TRIG -> D9, ECHO -> D10
const byte ENA = 5, IN1 = 4, IN2 = 7;
const byte ENB = 6, IN3 = 8, IN4 = 12;
const byte TRIG = 9, ECHO = 10;
const int TARGET_CM = 20; // the distance to keep
const float KP = 12.0; // how hard to react: speed per centimetre of error
const int MIN_SPEED = 90; // below this the motors only hum and do not turn
const int DEADBAND_CM = 2; // this close to the target is good enough: stand still
const int FAR_CM = 150; // nothing nearer than this: wait
void setMotor(byte enablePin, byte inA, byte inB, int speed) {
speed = constrain(speed, -255, 255);
digitalWrite(inA, speed > 0);
digitalWrite(inB, speed < 0);
analogWrite(enablePin, abs(speed));
}
void drive(int left, int right) {
setMotor(ENA, IN1, IN2, left);
setMotor(ENB, IN3, IN4, right);
}
int readDistanceCm() {
digitalWrite(TRIG, LOW);
delayMicroseconds(2);
digitalWrite(TRIG, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG, LOW);
unsigned long us = pulseIn(ECHO, HIGH, 25000UL);
if (us == 0) return 400;
return us / 58;
}
void setup() {
const byte pins[] = {ENA, IN1, IN2, ENB, IN3, IN4, TRIG};
for (byte pin : pins) pinMode(pin, OUTPUT);
pinMode(ECHO, INPUT);
Serial.begin(9600);
}
void loop() {
int cm = readDistanceCm();
int error = cm - TARGET_CM; // positive: too far, negative: too close
int speed = 0;
if (cm <= FAR_CM && abs(error) > DEADBAND_CM) {
speed = (int)constrain(KP * error, -255.0, 255.0);
if (abs(speed) < MIN_SPEED) speed = (speed > 0) ? MIN_SPEED : -MIN_SPEED;
}
drive(speed, speed);
Serial.print(cm);
Serial.print(" cm, speed ");
Serial.println(speed);
delay(50); // about 20 corrections a second
}Настройки
const int TARGET_CM = 20; // the distance to keep
const float KP = 12.0; // how hard to react: speed per centimetre of error
const int MIN_SPEED = 90; // below this the motors only hum and do not turn
const int DEADBAND_CM = 2; // this close to the target is good enough: stand still
const int FAR_CM = 150; // nothing nearer than this: waitВсе пять чисел регулятора собраны в начале скетча. KP имеет тип float — это число с десятичной точкой, поэтому можно попробовать и такие значения, как 8.5.
Регулятор
int cm = readDistanceCm();
int error = cm - TARGET_CM; // positive: too far, negative: too close
int speed = 0;
if (cm <= FAR_CM && abs(error) > DEADBAND_CM) {
speed = (int)constrain(KP * error, -255.0, 255.0);
if (abs(speed) < MIN_SPEED) speed = (speed > 0) ? MIN_SPEED : -MIN_SPEED;
}
drive(speed, speed);Сначала скорость равна 0. Она меняется, только если что-то находится в пределах FAR_CM, а ошибка больше мёртвой зоны. KP * error — это и есть П-регулятор. constrain() удерживает результат в пределах от -255 до 255, а (int) снова превращает его в целое число. Следующая строка поднимает маленькую скорость до MIN_SPEED и сохраняет её знак. (speed > 0) ? MIN_SPEED : -MIN_SPEED означает: если скорость положительная, взять MIN_SPEED, иначе взять -MIN_SPEED. И наконец, drive(speed, speed) даёт обоим колёсам одинаковую скорость, поэтому робот едет прямо вперёд или назад.
Если эхо не вернулось, readDistanceCm() возвращает 400. Это больше FAR_CM, поэтому робот ждёт.
Вывод и задержка
Serial.print() выводит расстояние и скорость в одну строку, а Serial.println() завершает её. Затем delay(50) ждёт 50 мс, поэтому робот измеряет расстояние и вносит поправку примерно 20 раз в секунду. readDistanceCm(), setMotor() и drive() работают так же, как в проекте «Робот, объезжающий препятствия».
Если что-то не работает
- Робот совсем не двигается. Посмотрите в монитор порта. Если расстояние больше 150, поблизости ничего нет: поднесите цель ближе. Если расстояние от 18 до 22 см, робот уже там, где нужно.
- Робот уезжает от цели, а не к ней. Колёса крутятся не в ту сторону. Проверьте, куда они крутятся, когда висят в воздухе, как в проекте «Первый заезд». Если какой-то мотор крутится назад, поменяйте местами его провода.
- Робот раскачивается вперёд-назад и никак не успокоится. KP слишком большой: уменьшите его. Можно также расширить мёртвую зону до 3 см.
- Возле цели робот гудит, но не едет. Значение MIN_SPEED слишком маленькое для ваших моторов или батареек. Увеличьте его или поставьте новые батарейки.
- Робот останавливается чуть ближе или чуть дальше 20 см. Это нормально. Мёртвая зона допускает от 18 до 22 см, а после выключения моторов робот ещё немного катится.
- Расстояние скачет. Рука — слабая цель, а мягкие или наклонные поверхности возвращают мало звука. Держите книгу плоской стороной к датчику. Подробнее — в статье о том, как «видят» ультразвуковые датчики.
- Робот перезагружается, когда меняет направление. Резкая смена хода с «вперёд» на «назад» просаживает напряжение батареи. Подробнее — в статье о том, почему роботы перезагружаются.
Что дальше
Ваш робот использует обратную связь: он измеряет, сравнивает с целью и вносит поправку — снова и снова. П-регулятор — это первая часть ПИД-регулятора. И-составляющая убирает небольшую ошибку, которая долго не исчезает, а Д-составляющая реагирует на то, как быстро меняется ошибка.
Дальше соберите робота, который едет по линии на полу. Можно также заменить delay(50) таймером на millis(), как показано в уроке про millis() и состояния. Больше о датчике — в уроке про HC-SR04.
Сделайте лучше
- Измените TARGET_CM на 10, а затем на 40. Нужно ли при этом менять и KP?
- Сгладьте показания расстояния медианой трёх измерений, как это делает скетч DistanceMeter.
- Пусть в мёртвой зоне моторы тормозят, а не вращаются по инерции: переведите оба пина направления каждого мотора в HIGH, а на его пин разрешения подайте 255.