Электрическая часть и ШИМ
В предыдущей статье мы подробно рассмотрели механическую часть робота. Скелет готов, теперь наш друг должен обрасти внутренностями. Электрическая схема робота приведена ниже.
Питание осуществляется пока от компьютерного блока питания. Удалось найти старый блок, который по каналу 5В может выдавать ток до 30А. Один сервопривод в пике может потреблять ток до 1.2А. Поэтому к вопросу питания надо отнестись серьезно. В будущем планирую поставить LiPo-батарею для автономного электропитания.
Итак, чтобы повернуть сервопривод на определенный угол и удерживать его в этом положении нужно подавать на его управляющий вход сигнал ШИМ частотой 50 Гц и длительностью от 500 до 2500 мкс. В контроллере это можно легко реализовать с помощью встроенных таймеров. Однако, мы смотрим далеко вперед и понимаем, что в будущем нам могут понадобиться еще дополнительные каналы ШИМ. Например, для добавления нашему монстру головы с челюстями или хвоста. А количество каналов таймера ограничено и нам возможно придется поменять контроллер. На просторах интернета есть информация как управлять множеством сервоприводов, задействовав для этого только один таймер. Я взял одну из таких идей за основу и переработал под свои нужды.
Вкратце, управление происходит так: есть массив длительностей управляющих импульсов, он сортируется в порядке возрастания. Для каждого сервопривода настроен свой выход микроконтроллера. В начале каждого цикла происходит установка всех выходов в «1». Загружается в регистр сравнения таймера первое значение ширины импульса. По прерыванию по совпадению мы сбрасываем выход в «0» и загружаем в таймер следующее значение. После перебора всех значений процесс повторяется. Привожу для примера кусочек кода.
Инверсная кинематика
Периферия теперь корректно обрабатывается, достаточно записать в массив нужную длительность импульса и сервопривод повернется в нужное нам положение. По сути надо задавать для каждого привода свой угол поворота. Но в угловых координатах неудобно работать, мы привыкли к обычным декартовым координатам XYZ. Как раз основная задача обратной кинематики и сводится к нахождению угловых координат шарниров конечности робота по заданным пространственным координатам.
Таким образом, мы задаем пространственные координаты точки, в которую должен переместиться кончик ноги робота, а наш алгоритм должен рассчитать три угла, на которые должны повернуться сервоприводы. Задачу инверсной кинематики мы решим, не выходя за рамки школьного курса алгебры и геометрии.
Давайте, сразу определимся с терминами. Наша нога будет состоять из трех элементов, которые могут вращаться относительно друг друга. Назовем их, как это принято, Coxa, Femur и Tibia. В нашем случае, для полного описания конечности нужен еще один размер, назовем его Coxa_off. Это смещение кончика ноги относительно оси Coxa. Смотрите рисунок для большей наглядности.
Каждая нога связана со своей локальной системой координат, центр которой лежит на оси вращения Coxa и по уровню оси Femur. Точка А – это место, куда должен дотянуться кончик ноги. Для вычисления необходимых нам углов cAngle, fAngle, tAngle нужно решить несколько тригонометрических задач на рисунке ниже. Угол cAngle определяется элементарно как разность двух углов. Для нахождения двух остальных углов нужно перейти в систему координат с осями z и t и центром в O1, далее использовать теорему косинусов.
На этом вся геометрия заканчивается. В коде программы ниже я в качестве tAngle использую смежный угол, нулевое положение для Tibia составляет 60 градусов. Математическая модель каждой ноги описывается десятью параметрами: 4 линейных размера, 3 коэффициента усиления сервоприводов и 3 угла ошибки (для компенсации ошибки при установке нуля привода).
Как правильно подобрать эти параметры, провести калибровку всех конечностей я расскажу в следующей статье «Настройка и отладка».
Учимся ходить
Для того, чтобы наш друг начал ходить, мы в нашей программе заведем таймер, который будет генерировать прерывания с периодом 0.5 секунд. В каждом новом прерывании мы будем устанавливать новые координаты шага. Алгоритм ходьбы простой: ноги двигаются тройками. В первую тройку входят левая средняя, правые передняя и задняя ноги. Во вторую три оставшихся ноги. Так обеспечивается устойчивость, робот всегда остается стоять минимум на трех ногах.
Ходьба – это циклический процесс, в котором повторяются выше указанные фазы движения: подъем, вперед, исходное положение и назад. Для зеркальной тройки ног выполняются точно такие же движения, сдвинутые на две фазы. То есть когда первая тройка идет вверх, вторая – на исходную, первая – вперед, вторая – назад.
В нашей модели движение вперед осуществляется вдоль оси Y для каждой ноги, координат по оси X не меняется. Для движения назад надо выполнять те же самые движения, но в обратной последовательности. Перемещение робота вправо или влево осуществляется аналогично, только вместо координаты по оси Y меняется координата по оси X. Для себя я сделал универсальный алгоритм, где задаю только угол направления движения: 0 – вперед, 180 – назад и т.д.
Поворот вправо и влево имеет более сложный алгоритм, связанный с вращением туловища вокруг оси Z.
Продвинутые способы движения
Базовые движения реализованы. Дальше я добавил возможность смещения корпуса в любую сторону по трем осям координат и вращение корпуса вокруг осей X и Y. Для расчета используются матричные преобразования в пространстве, элементы векторной алгебры. В общем, без бутылки минералки не разберешься))) Если будет интерес к данной теме, я раскрою ее в отдельной статье.
Для реализации плавного медленного движения я использовал следующий алгоритм: после получения команды на движение в заданную точку программа разделяет всю траекторию на несколько точек и в каждый период ШИМ движется к своей промежуточной точке пока не достигнет заданных координат.
Дополнительно я научился менять ширину, скорость и период шага прямо в процессе движения. Обо всех этих наворотах я обязательно расскажу позднее.
О том, как проводить настройку и отладку, с помощью чего и каким образом будет дистанционно управляться наш механический питомец, я подробно опишу в следующей статье «Настройка и отладка». Спасибо за внимание!