Впровадження пропорційного контролера

Тепер, коли ви дізналися про пропорційні контролери, давайте реалізуємо один для нашої діяльності з відстеження відстані, де ми хочемо тримати робота на деякій відстані від об’єкта перед ним за допомогою далекоміра!

Визначення термінології

Давайте визначимо терміни, які нам знадобляться в нашому коді:

Задана точка або бажане значення: у цьому прикладі це певна відстань від далекоміра, на якій ми хочемо, щоб знаходився робот. Для цього прикладу візьмемо 20 см.

Змінна процесу або поточне значення: Ми отримуємо виміряне значення, зчитуючи значення далекоміра. Це rangefinder.distance().

Наша мета полягає в тому, щоб змінна процесу (відстань дальноміра) наблизилася до заданого значення (20 см).

Таким чином, наша помилка обчислюється як error = rangefinder.distance() - 20. Зверніть увагу, що error = 20 - rangefinder.distance() також є «правильним». Різниця в знаках полягає просто в тому, що має більше сенсу для вашого застосування. Тут, якщо ми мали б похибку 30 см, ми хотіли б проїхати вперед на 10 см, тому нам потрібна позитивна похибка, щоб наші двигуни оберталися вперед.

Вихідний сигнал управління: У даному випадку це зусилля двигуна. Це пов’язано з тим, що ми хочемо рухатися зі швидкістю, пропорційною помилці відстані. Нагадаємо, що для P-регулювання це буде розраховуватися як motor_effort = Kp * error.

Kp: Це наш пропорційний коефіцієнт підсилення. Хоча нам потрібно буде налаштувати це значення, ми можемо припустити досить розумне значення, враховуючи діапазон значень, які може приймати наша похибка, та область нашого контрольного виходу. У цьому випадку, якщо ми знаходимося на відстані 30 см від об’єкта, наша похибка буде дорівнювати 10. Ми можемо припустити, що на такій достатній відстані ми захочемо рухатися вперед з максимальним зусиллям 1, оскільки зусилля обмежене областю \([-1, 1]\). Таким чином, ми можемо припустити, що Kp = 1/10 = 0,1. Звичайно, це, ймовірно, не остаточне значення, яке найкраще підходить для вашого робота, але це хороший відправний пункт.

Впровадження контролера

Тепер, коли ми визначили терміни, давайте напишемо код!

Почнемо з визначення пропорційного коефіцієнта підсилення та заданого значення:

Kp = 0.1
desired_distance = 20
../../_images/variables.png

Далі ми хочемо ввести якийсь цикл, щоб постійно зчитувати значення датчика відстані та оновлювати зусилля двигуна з виходу контролера.

Kp = 0.1
desired_distance = 20
while True:
    error = rangefinder.distance() - desired_distance
    motor_effort = Kp * error
    drivetrain.set_effort(motor_effort, motor_effort)
    time.sleep(0.05)
../../_images/pcode.png
Кожна ітерація циклу складається з наступних кроків:

  1. Зчитати значення дальноміра, щоб отримати поточну відстань

  2. Обчислити похибку

  3. Обчислити вихідний сигнал управління за допомогою Kp * похибка

  4. Встановити зусилля двигуна трансмісії на вихідний сигнал управління

  5. Зачекати короткий проміжок часу

Цей код повинен дати нам робоче рішення для підтримки заданої відстані від об’єкта, що знаходиться перед роботом!

Спробуйте

Спробуйте перемістити об’єкт перед роботом і спостерігайте, як робот намагається підтримувати задану відстань! Що відбувається, коли ви збільшуєте Kp? А коли зменшуєте? Яке значення Kp найкраще підходить для вашого робота?