Документация: ROS2 пакет neck_driver

Назначение

Пакет neck_driver управляет шеей робоголовы по двум осям:

• vertical (наклон / pitch) — вверх/вниз
• horizontal (поворот / yaw) — влево/вправо

Физически шея реализована на двух сервоприводах, подключённых к ШИМ-контроллеру PCA9685 по I2C (/dev/i2c-1). Два сервопривода работают дифференциально: их комбинация даёт независимое управление наклоном и поворотом.

Движение к цели выполняется:

• плавно по кубическому полиному при duration > 0 (частота обновления ~50 Гц),
• мгновенно при duration = 0 (мгновенно = с максимально возможной скоростью движения).

---

Структура пакета

Типичная структура:

neck_driver/
├── CMakeLists.txt                  # Файл CMake сборки пакета
├── config
│   └── neck_driver.yaml            # Конфиг-файл
├── launch
│   └── neck_driver.launch.py       # Launch-файл запуска
├── package.xml
├── README.md
└── src
    ├── main.cpp                    # Исходный код neck_driver
    └── servo_pulse_check.cpp       # Исходный код утилиты калибровки импульсов PCA9685

---

Принцип работы

1) PCA9685 и PWM

PCA9685 генерирует PWM с 12-битным разрешением: значения 0..4095.
В коде драйвера используется частота 50 Гц, то есть период 20 мс.

Пересчёт в микросекунды (приблизительно):

• 1 count ≈ 20000 / 4096 ≈ 4.8828 мкс
• pulse_us ≈ pulse_count * 4.8828

Например:

• 120 → ~586 мкс
• 550 → ~2686 мкс

Именно поэтому в конфиге используются servo_pulse_low, servo_pulse_high в диапазоне порядка 100..600.

---

2) Преобразование “углы шеи” → “углы серв”

Пользователь задаёт:

• vertical (angle_a)
• horizontal (angle_b)

В драйвере вычисляются целевые углы сервоприводов (в градусах 0..180):

servo_1_angle = 90 - vertical - horizontal + servo_1_coef   (правый серво)
servo_2_angle = 90 + vertical - horizontal + servo_2_coef   (левый серво)

Затем servo_1_angle и servo_2_angle:

• ограничиваются в [0..180],
• переводятся в PWM counts линейной интерполяцией между servo_pulse_low и servo_pulse_high.

---

3) Плавное движение (duration > 0)

Если duration > 0, применяется кубический полином с нулевой начальной и конечной скоростью:

θ(t) = a0 + a2·t² + a3·t³
a0 = θ_current
a2 = 3·(θ_goal − θ_current) / T²
a3 = −2·(θ_goal − θ_current) / T³

Обновление целевого положения выполняется в отдельном потоке с частотой ~50 Гц.

Если duration = 0 — установка цели сразу.

---

ROS2 API

Нода создаётся с именем neck_driver и в launch запускается в namespace neck_driver, поэтому сервисы по умолчанию выглядят как:

• /neck_driver/neck_set_angle
• /neck_driver/is_idle

1) Сервис neck_set_angle

Тип: robohead_interfaces/srv/Move
Имя: настраивается параметром srv_neck_set_angle_name (по умолчанию neck_set_angle)

Запрос

Поле	Тип	Описание
angle_a	int16	Вертикальный угол (наклон), градусы
angle_b	int16	Горизонтальный угол (поворот), градусы
duration	float64	Время движения к цели (сек). 0.0 — мгновенно

Семантика:

• angle_a > 0 — наклон “вверх”, angle_a < 0 — “вниз”
• angle_b > 0 — поворот “вправо”, angle_b < 0 — “влево”

Ответ

Поле	Тип	Описание
data	int16	Код результата

Коды:

• 0 — успех
• -1 — angle_a вне диапазона [constraints.v_from, constraints.v_to]
• -2 — angle_b вне диапазона [constraints.h_from, constraints.h_to]
• -3 — duration < 0

Примеры вызова

# Наклон вверх на 20° и поворот вправо на 15° за 1.5 секунды
ros2 service call /neck_driver/neck_set_angle robohead_interfaces/srv/Move \
  "{angle_a: 20, angle_b: 15, duration: 1.5}"

# Вернуться в нейтраль за 0.8 сек
ros2 service call /neck_driver/neck_set_angle robohead_interfaces/srv/Move \
  "{angle_a: 0, angle_b: 0, duration: 0.8}"

# Поворот влево на 27° за 1.5 секунды
ros2 service call /neck_driver/neck_set_angle robohead_interfaces/srv/Move \
  "{angle_a: 0, angle_b: -27, duration: 1.5}"

# Мгновенно (осторожно)
ros2 service call /neck_driver/neck_set_angle robohead_interfaces/srv/Move \
  "{angle_a: 0, angle_b: 0, duration: 0.0}"

---

2) Сервис is_idle

Тип: robohead_interfaces/srv/SimpleCommand
Имя: настраивается параметром srv_is_idle_name (по умолчанию is_idle)

Запрос: поле запроса игнорируется (вызывается “как есть”).

Ответ:

• data = 1 — нода находится в режиме ожидания (текущие углы равны целевым)
• data = 0 — движение ещё продолжается

Пример:

# Во время отсутсвия движения вернет 1
ros2 service call /neck_driver/is_idle robohead_interfaces/srv/SimpleCommand "{}"

# Запустите долгое движение (duration=10.0) и успейте повторить запрос is_idle
ros2 service call /neck_driver/neck_set_angle robohead_interfaces/srv/Move \
  "{angle_a: 30, angle_b: 0, duration: 10.0}"
ros2 service call /neck_driver/is_idle robohead_interfaces/srv/SimpleCommand "{}"
# is_idle во время движения должен вернуть 0

---

Конфигурация (neck_driver.yaml)

Файл: neck_driver/config/neck_driver.yaml

Описание параметров

Имена сервисов

Параметр	Тип	По умолчанию	Описание
srv_neck_set_angle_name	string	neck_set_angle	Имя сервиса установки углов
srv_is_idle_name	string	is_idle	Имя сервиса проверки состояния

Начальная поза

Параметр	Тип	По умолчанию	Описание
std_vertical_angle	int	0	Начальный вертикальный угол
std_horizontal_angle	int	0	Начальный горизонтальный угол

При старте нода выставляет начальную позу: сначала один сервопривод, пауза ~1 сек, затем второй.

I2C / PCA9685

Параметр	Тип	По умолчанию	Описание
i2c_address	int	64	I2C-адрес PCA9685 (обычно 0x40)
servo_1_channel	int	5	Канал PCA9685 для правого сервопривода
servo_2_channel	int	4	Канал PCA9685 для левого сервопривода

Важно: каналы в конфиге должны соответствовать реальной проводке (к какому каналу PCA9685 подключён какой серво). Каналы нумеруются с 0 до 15. На плате RPI-HEAD доступны для подключения только каналы 0...7

Калибровка (механика)

Параметр	Тип	Описание
servo_1_coef	int	Поправка угла (в градусах) для правого серво
servo_2_coef	int	Поправка угла (в градусах) для левого серво

Используется для выравнивания “нуля” (чтобы при vertical=0, horizontal=0 механика реально смотрела прямо).

Диапазон PWM (ключевой для безопасности)

Параметр	Тип	Описание
servo_pulse_low	int	PWM “count” для 0° (минимальный импульс)
servo_pulse_high	int	PWM “count” для 180° (максимальный импульс)

Эти границы зависят от конкретных сервоприводов и механики. Неправильные значения могут:

• привести к упору в механику,
• вызвать перегрузку/жужжание,
• повредить серво или крепление,
• вызывать самопроизвольные подергивания в крайних положениях (0 и 180 градусов),
• не реагировать на задание углов для крайних положений сервоприводов (0 и 180 градусов).

Ограничения углов шеи

Параметр	Тип	По умолчанию	Описание
constraints.v_from	int	-30	Мин. вертикальный угол
constraints.v_to	int	30	Макс. вертикальный угол
constraints.h_from	int	-30	Мин. горизонтальный угол
constraints.h_to	int	30	Макс. горизонтальный угол

Рекомендуется не расширять диапазоны без понимания механических ограничений конструкции.

Обратите внимание, ограченичения вводятся на положение шеи, а не сервпоприводов, которые ею управляют.

---

Сборка и запуск

Сборка

cd ~/robohead_ws
colcon build --symlink-install --packages-select robohead_interfaces neck_driver
source install/setup.bash

Запуск (через launch)

ros2 launch neck_driver neck_driver.launch.py

Ожидаемый лог:

[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [neck_driver_node-1]: process started with pid [169298]
[neck_driver_node-1] [INFO] [1776421088.315170083] [neck_driver.neck_driver]: INITED

---

Режим отладки

Если на устройстве всё запускается через systemd (например robohead.service), для ручного теста удобно остановить автозапуск:

sudo systemctl stop robohead.service

Далее запустить neck_driver вручную как показано выше.

---

Утилита калибровки: servo_pulse_check

Внимание: при обычном использовании Робоголовы нет необходимости изменять параметры ниже.

Зачем нужна

servo_pulse_low и servo_pulse_high в конфиге — это “границы” PWM, соответствующие 0° и 180°. Для разных моделей сервоприводов и реальной механики безопасные границы могут отличаться.

servo_pulse_check позволяет:

• выбрать канал PCA9685,
• вручную подавать “сырые” значения PWM count,
• наблюдать, где начинаются упоры/перегрузка,
• подобрать безопасный диапазон и записать его в конфиг.

Важные меры безопасности

Перед калибровкой:

1. Обязательно снимите с сервопривода все тяги и рычаги - он должен свободно вращаться в любом направлении.
2. Начинайте с “середины” (около 400) и двигайтесь малыми шагами (с шагом в 10).
3. При появлении:
   • сильного упора,
   • характерного “жужжания”,
   • заметного нагрева,
   • деформации механики
     немедленно вернитесь назад и уменьшите шаг.

Запуск

ros2 run neck_driver servo_pulse_check

Работа с утилитой

Убедитесь, что пакет neck_driver (и robohead_controller) выключены, иначе утилита не сможет управлять сервоприводами

1. Сначала вводится номер канала PCA9685:

Input servo channel [0..15]:

Например, для левого серво (если он на 4 канале):

Input servo channel [0..15]: 4

В случае корректного ввода утилита выведет

OK. Now input PWM 'off' count [0..4095]. Ctrl+D to exit.
Input pulse:

2. Потом бесконечно вводите значение pulse (0..4095):

Input pulse: 400

Утилита будет отвечать реально установленным значением.

Set channel=4 pulse=350

Если введенное значение выходит из дапазона 0..4095, то оно будет округлено до ближайшей границы.

Для завершения работы нажмите Ctrl+D на клавиатуре.

Пример работы:

ros2 run neck_driver servo_pulse_check
Input servo channel [0..15]: 4
OK. Now input PWM 'off' count [0..4095]. Ctrl+D to exit.
Input pulse: 400
Set channel=4 pulse=400
Input pulse: 350
Set channel=4 pulse=350
Input pulse: 450
Set channel=4 pulse=450
Input pulse: -100
Set channel=4 pulse=0
Input pulse: 10000
Set channel=4 pulse=4095
Input pulse: 400
Set channel=4 pulse=400
Input pulse: 
Exit.

Рекомендуемая процедура подбора servo_pulse_low и servo_pulse_high

1. Выберите канал одного серво.
2. Первой командой поставьте безопасную середину, например 400.
3. Уменьшайте pulse шагом 5–10, пока:
   • серво ещё уверенно держит,
   • механика серво не упирается,
   • нет сильного жужжания.
   • серво действительно поворачивается. Можно попеременно задавать pulse: 400, 350, 400, 330, 400, 300, 400, 280, ... - так даже при небольшом шаге вы будете видеть, что поворот действительно совершается.
4. Зафиксируйте найденную минимальную безопасную границу как кандидата для servo_pulse_low.
5. Вернитесь к середине и увеличивайте pulse, аналогично найдите максимум → кандидат для servo_pulse_high.
6. Повторите для всех сервоприводов.
7. Возьмите общие границы (обычно выбирают более “узкий” диапазон. Пример: у серво 1 получился диапазон 112...560, у серво 2 - 107...547. Тогда возьмите дипазон 112..547. Рекомендуется уменьшить найденный диапазон на 10-20 пунктов с каждого конца для устойчивой работы: в примере получим 130...530
8. Запишите в neck_driver.yaml:

   servo_pulse_low: 130
   servo_pulse_high: 530

---

Частые проблемы и диагностика

1) Cannot open I2C device

• Проверьте, что существует /dev/i2c-1
• Включён ли I2C в системе (raspi-config / dtparam / overlay)
• Права доступа (часто лечится запуском через sudo)

2) Cannot set I2C slave address

• Неверный i2c_address
• PCA9685 реально на другом адресе (смотрите i2cdetect)

3) Дёргается / упирается / не совпадает “ноль”

• Сначала подберите servo_pulse_low и servo_pulse_high
• Затем подстройте servo_1_coef/servo_2_coef, чтобы при vertical=0,horizontal=0 голова смотрела ровно

4) Перепутаны направления (вверх/вниз или лево/право)

• Это обычно следствие перепутанных каналов servo_1_channel и `servo_2_channel. Поменяйте либо физически подключение к плате, либо в конфиг-файле поменяйте каналы.

---