Создадим систему управления роботом и разработаем алгоритм управления его движением.

Задача системы управления (СУ) – определять ошибку между требуемым и текущим положениями схвата робота, рассчитывать перемещения отдельных звеньев и выдавать управляющий сигнал на серво-двигатели.

Наша СУ будет состоять из сервоприводов (SG-90), платы Arduino и ПК с программой, получающей координаты цели и рассчитывающей требуемые углы поворота серв.

Общий алгоритм управления Роботом такой:

• Определить требуемое положение схвата: схват должен придти в точку цели. Цель можно указать мышкой, написав программу на компьютере (например в Processing) или обработать изображение с камеры, висящей над рабочей зоной робота, выделив на нем объект нужного цвета или формы.
• Рассчитать требуемые положения звеньев робота, т..е. углы поворота двигателей. Ввиду простоты схемы робота, сильно напоминающей треугольник, нам понадобятся некоторые знания геометрии (теорема Пифагора, синусы, теорема косинусов). В нашей программе на Processing, определив координаты цели, сразу же рассчитаем и все углы, высвобождая этим ресурсы Arduino.
• Передача рассчитанных требуемых углов в Arduino. А дальше есть библиотека Servo и команда Servo::write(угол). Но попытка просто использовать эту функцию может привести к тому, что в случае требования повернуться на большой угол (например 180 град), серво-двигатель попытается мгновенно придти в указанное положение. Робот имеет некоторую массу и обладает инерцией, а это значит, что он будет сопротивляться изменению своей скорости. Силы инерции могут сорвать зубья шестеренок сервы. Необходимо попытаться сгладить переход между положениями, растянув его во времени.

Разобьем весь переход на несколько временных шагов и на каждом будем двигаться на долю, пропорциональную (составляющую часть) ошибке: Kp*error. Это еще называется пропорциональный регулятор (П-регулятор). 

Итак, микроконтроллер, получив значение требуемого угла поворота, рассчитает ошибку положения (текущее положение доступно из Servo::read()) и за некоторое количество тактов плавно переместит робота. От коэффициента пропорциональности зависит плавность.

Сейчас о том, как рассчитать требуемые положения звеньев робота.

Тут понадобятся базовые знания геометрии о прямоугольном треугольнике, соотношении его сторон в теореме Пифагора, откуда берутся синусы и косинусы. 

Робот имеет 3 степени свободы (3 цилиндрических шарнира – 3 оси вращения), что позволяет перемещаться схвату по трем координатам xyz, но мы ограничимся плоскостью xy. 

На виде в плане (сверху то есть) мы видим схему робота и точку положения мишени с координатами xy, которые нам известны. Зная координаты, рассчитываем общий вылет робота – С (гипотенуза = сумме квадратов катетов). Угол поворота основания рассчитывается как дуга, соответствующая sin (y/c).

Алексей Игоревич (IT robotics -Школа программирования и робототехники в Королеве)