Домашнее задание
В этом задании менять скетч на плате не нужно — мы используем уже загруженную на предыдущем шаге универсальную прошивку io_board (которая публикует /analog_signal и слушает /servo_angle и /led). Ваша задача — написать на стороне компьютера Python-узел smooth_servo_controller.py, который усредняет последние измерения датчика, плавно поворачивает сервопривод D2 и зажигает светодиод D48 при превышении порога.
Зачем нужно усреднение сигнала в Python-узле
Сырые значения с аналогового входа A8 всегда немного колеблются (особенно если датчик не подключён и пин ловит наводки). Если пересчитывать каждое отдельное измерение напрямую в угол, сервопривод будет постоянно подрагивать.
Если же на стороне Python-узла складывать последние 5 измерений в небольшой список и считать их среднее арифметическое, случайные скачки сгладятся, и вал сервопривода будет двигаться плавно.
Что должно получиться
Напишите Python-скрипт ~/ros2_ws/smooth_servo_controller.py с узлом smooth_servo_controller:
- Подписка на
/analog_signal(std_msgs/msg/Int32):- узел хранит последние
5полученных значений (удобно использовать очередь фиксированной длиныcollections.deque(maxlen=5)); - при каждом новом сообщении вычисляет среднее значение
avg_val = sum(self.window) / len(self.window).
- узел хранит последние
- Пропорциональное управление сервоприводом (
/servo_angle,std_msgs/msg/Int32):- переводит сглаженное значение
avg_val(0..1023) в угол от0до180градусов по формуле:angle = int(avg_val * 180 / 1023); - ограничивает угол пределами
0..180и публикует его в топик/servo_angle.
- переводит сглаженное значение
- Пороговая сигнализация на светодиоде
D48(/led,std_msgs/msg/Bool):- если сглаженное значение
avg_val > 700— отправляетTrueв топик/led(зажигает светодиодD48) и выводит предупреждение в журналself.get_logger().warn(...); - если
avg_val <= 700— отправляетFalseв топик/led(гасит светодиодD48).
- если сглаженное значение
Как составить алгоритм
- Импортируйте
from collections import deque,rclpy,Nodeи сообщенияBool, Int32изstd_msgs.msg. - В конструкторе
__init__()создайте буферself.window = deque(maxlen=5), подписку на/analog_signalи два издателя: для/servo_angle(Int32) и/led(Bool). - В методе-обработчике
on_sensor(self, msg: Int32):- добавьте новое измерение в буфер:
self.window.append(msg.data); - посчитайте среднее:
avg_val = sum(self.window) / len(self.window); - рассчитайте угол
angle = max(0, min(180, int(avg_val * 180 / 1023)))и опубликуйте его в/servo_angle; - проверьте условие
alarm = avg_val > 700и при смене состояния отправьтеBool(data=alarm)в/led.
- добавьте новое измерение в буфер:
Как проверить результат
- Убедитесь, что в плату загружен скетч
io_boardсо страницы «Управляющий узел на Python и launch-файл». - В Терминале 1 запустите
rosduino_agent:ros2 run rosduino_agent rosduino_agent --port /dev/ttyACM0 - В Терминале 2 запустите ваш скрипт:
python3 ~/ros2_ws/smooth_servo_controller.py - Плавно вращайте ручку потенциометра на разъёме GPIO
A8(или поднесите палец к контактуGPIO): сервопривод на разъёмеD2будет мягко следовать за сглаженным значением сигнала, а при превышении уровня700загорится светодиодD48.
Назад: Управляющий узел на Python и launch-файл.
Дальше: Урок 8. Работа на роботе TurtleBro.