Домашнее задание
На предыдущей странице мы отправляли из ROS 2 логические значения true и false для включения светодиода. В этом задании вы создадите узел управления сервоприводом, который принимает целочисленный угол из топика /servo_angle, проверяет допустимые границы 0°–180° и зажигает светодиод D48, если присланный угол вышел за пределы диапазона.
Перед подключением сервопривода к разъёму D2 отключите USB-кабель от платы и сверьте контакты −, +, S с цветами проводов.
Что должно получиться
- Плата регистрирует в ROS 2 узел с именем
servo_driver_node. - Узел подписывается на топик
servo_angleс типом сообщенияstd_msgs/msg/Int32(заголовочный файл#include <std_msgs/msg/Int32.h>, число хранится в полеmsg.data). - После включения платы сервопривод на разъёме
D2встаёт в среднее положение90°, а светодиодD48погашен. - Когда в топик
/servo_angleприходит новое сообщение:- если значение
msg.dataнаходится в допустимых пределах от0до180включительно — сервопривод поворачивается на уголmsg.data, а светодиодD48выключается (LOW); - если значение
msg.dataменьше0или больше180(например,-30или220) — программа ограничивает угол безопасным пределом (0°или180°) с помощьюconstrain(msg.data, 0, 180), поворачивает сервопривод на этот ограниченный угол и включает светодиод ошибкиD48(HIGH).
- если значение
Требования к программе
- Подключены библиотеки
<Servo.h>,<RosDuino.h>и<std_msgs/msg/Int32.h>. - В функции
loop()вызываетсяros.update()без использованияdelay(). - Перед загрузкой скетча агент
rosduino_agentостанавливается клавишамиCtrl+C.
Как составить алгоритм
- Возьмите за основу скетч со страницы управления светодиодом и добавьте подключение
<Servo.h>и<std_msgs/msg/Int32.h>. - Объявите константы
SERVO_PIN = 2,ERROR_LED_PIN = 48и объектServo servo;. - Напишите функцию-обработчик
void onServoAngle(std_msgs::Int32 msg):- проверьте условие
if (msg.data < 0 || msg.data > 180)(оператор||означает «логическое ИЛИ»); - если условие истинно — включите светодиод
D48(HIGH), иначе — выключите его (LOW); - вычислите безопасный угол
int safeAngle = constrain(msg.data, 0, 180);и передайте его вservo.write(safeAngle);.
- проверьте условие
- В
setup()настройте пин светодиода, привяжите сервоприводservo.attach(SERVO_PIN), поставьте начальный угол90°, вызовитеros.begin("servo_driver_node")и зарегистрируйте подпискуros.subscribe("servo_angle", onServoAngle). - В
loop()оставьте вызовros.update().
Как проверить результат из терминала ROS 2
- Загрузите скетч в плату и запустите
rosduino_agentв Терминале 1. - В Терминале 2 проверьте, что появились узел
/servo_driver_nodeи топик/servo_angleс типомstd_msgs/msg/Int32. - Отправьте допустимый угол
45: Терминал 2:
Сервопривод повернётся наros2 topic pub --once /servo_angle std_msgs/msg/Int32 "{data: 45}"45°, светодиодD48останется погашенным. - Отправьте угол за пределами диапазона, например
220: Терминал 2:
Сервопривод остановится на безопасной границеros2 topic pub --once /servo_angle std_msgs/msg/Int32 "{data: 220}"180°, а светодиодD48загорится, сигнализируя о некорректной команде. - Снова отправьте допустимый угол
90— сервопривод вернётся в середину, а светодиод ошибкиD48погаснет.
Назад: Управление светодиодом из ROS.
Дальше: Урок 5. Топики в RosDuino.