Оживляем гексапода. Часть первая
В прошлой статье мы поделились опытом создания гексапода с использованием технологии 3D печати. Теперь речь пойдет о программной составляющей, которая позволила его оживить.
Первоначально планировалось изложить всю информацию в одной статье, но в процессе написания стало понятно, что такое изложение будет поверхностным и неинформативным. Поэтому было принято решение написать несколько статей с более детальным изложением темы.
Устройство гексапода
На текущий момент в качестве основного контроллера используется плата UNO R3 с Wi-Fi ESP8266. По сути эта плата с двумя контроллерами на борту, взаимодействующих между собой через UART-интерфейс.
Несмотря на то, что Uno имеет довольно ограниченный объем вычислительных ресурсов, ее достаточно чтобы научить робота выполнять базовые команды:
- движение по прямой с заданной скоростью и продолжительностью
- круговое движение влево или вправо (разворот на месте)
- принимать заданные положения конечностей
ESP8266 отвечает за организацию беспроводного канала связи и служит шлюзом, через который Uno получает управляющие команды.
Управление роботом может осуществляться через локальную сеть в рамках установленной с ним telnet сессии или через проводное подключение к контроллеру (для прошивки или отладки). Для удобства мы также написали простое андроид-приложение, реализующее минимальный интерактивный функционал для управления роботом.
На представленном ниже рисунке схематично изображено устройство гексапода.
Все сервоприводы подключены к плате расширения Multiservo Shield, позволяющей управлять 18 сервоприводами. Её общение с Arduino осуществляется по шине I²C. Поэтому, даже при одновременном рулении 18 сервоприводами, практически все пины Arduino останутся свободными.
Следует отметить, что плата расширения имеет разъем для питания подключенных сервоприводов. Но максимально допустимый ток, на который рассчитана плата составляет порядка 10А, что не достаточно для питания сервоприводов серии MG996R, суммарный максимальный ток потребления которых может превышать указанное значение. Поэтому в нашем варианте каждый сервопривод был подключен к отдельной линии питания, минуя плату расширения.
Управление гексаподом
Логика управления отдельной конечностью гексапода реализована в программе с помощью класса GeksaFoot.
class GeksaFoot {
private:
// Положение центра локальной системы координат лапы относительно центра робота
Vector3D m_p0;
// Углы поворота осей локальной системы координат лапы относительно центра робота
Vector3D m_r0;
// объекты Multiservo, для непосредственного управления сервоприводами
Multiservo m_coxaServo; // сервопривод плеча
Multiservo m_femoraServo; // сервопривод бедра
Multiservo m_tibiaServo; // сервопривод голени
public:
GeksaFoot(Vector3D p0,Vector3D r0);
// выполняет активацию сервоприводов
void begin(int coxaPin, int femoraPin, int tibiaPin);
// выполняет дезактивацию сервоприводов
void end();
// интерфейс управления лапой
void coxaAngle(int); // повернуть плечо на заданный угол (-90 .. 90 градусов)
int coxaAngle(); // возвращает текущий угол поворота плеча
void femoraAngle(int); // наклонить бедро на заданный угол (-90 .. 90 градусов)
int femoraAngle(); // возвращает текущий угол наклона бедра
void tibiaAngle(int); // наклонить голень на заданный угол (-90 .. 90 градусов)
int tibiaAngle(); // возвращает текущий угол наклона голени
// вспомогательные методы для расчетов прямой и инверсной кинематики
// вычисляет углы поворотов ноги для достижения конечностью заданной точки
int getAngles(Vector3D p, int& coxaAngle, int& femoraAngle, int& tibiaAngle);
// вычисляет координату кончика лапы по заданным углам
int getPoint(int coxaAngle, int femoraAngle, int tibiaAngle, Vector3D& p);
};
Методы coxaAngle, femoraAngle, tibiaAngle позволяют задать или узнать угол поворота отдельного сустава ноги. Вспомогательные методы getAngles и getPoint реализуют логику вычислений прямой и инверсной кинематики, с помощью которых можно вычислить значение углов ноги для заданной точки пространства ее конечности. Или наоборот, точку пространства для текущих значений углов.
Среднее положение каждого сустава соответствует нулевому значения угла, а диапазон поворота сустава лежит в области от -90 до 90 градусов.
Классом верхнего уровня является класс Geksapod. Он реализует логику работы всего робота. Каждая нога гексапода включена в состав этого класса в виде отдельного экземпляра класса GeksaFoot.
class Geksapod: public AJobManager {
friend class MotionJob;
friend CommandProcessorJob;
// конечности робота
GeksaFoot m_LeftFrontFoot;
GeksaFoot m_LeftMidleFoot;
GeksaFoot m_LeftBackFoot;
GeksaFoot m_RigthFrontFoot;
GeksaFoot m_RigthMidleFoot;
GeksaFoot m_RigthBackFoot;
// задача, отвечающая за движение конечностей
MotionJob m_MotionJob;
private:
// устанавливает конечности робота в заданное положение
// с предварительной проверкой на предельную допустимость углов
// и корректность взаимного расположения конечностей
int _setPose(int idx, int ca, int fa, int ta);
int _setPose(int[FOOTS_COUNT][3]);
int _setPose(Vector3D points[FOOTS_COUNT]);
protected:
// перемещение конечностей робота в заданное положение в течение указанного времени
int setPose(int idx, int ca, int fa, int ta, int actionTime);
int setPose(int pose[FOOTS_COUNT][3], int actionTime);
int setPose(int idx, Vector3D p, int actionTime);
int setPose(Vector3D points[FOOTS_COUNT], int actionTime = 0);
int setPose(int ca, int fa, int ta, int actionTime);
// текущее положение конечностей робота
void getPose(int idx, int& ca, int& fa, int& ta);
void getPose(int pose[FOOTS_COUNT][3]);
void getPose(int idx, Vector3D& p);
void getPose(Vector3D points[FOOTS_COUNT]);
// запускает выполнение указанного движения
int execute(Motion* pMotion);
public:
Geksapod();
void setup();
// основные управляющие команды
int move(int speed, int time); // движение по прямой
int rotate(int speed, int time); // поворот на месте
void stop(); // остановить движение
// вспомогательные методы для расчетов прямой и инверсной кинематики
int getAngles(int idx, Vector3D p, int& ca, int& fa, int& ta);
int getPoint(int idx, int coxaAngle, int femoraAngle, int tibiaAngle, Vector3D& p);
int getAngles(Vector3D points[FOOTS_COUNT], int pose[FOOTS_COUNT][3]);
int getPoints(int pose[FOOTS_COUNT][3], Vector3D points[FOOTS_COUNT]);
};
Перегруженные методы getPose и setPose предназначены для внутреннего использования и позволяют получить текущее положение конечностей робота или установить новое. При этом положение лап задается в виде набора значений углов поворота каждого сустава или как набор координат концевых точек конечностей робота относительно его центра.
Для плавного движения конечностей при вызове методов setPose можно указать время (параметр actionTime), через которое лапы должны достичь заданного положения.
Управления роботом осуществляется публичными методами move, rotate и stop.
Эмуляция мультизадачности
Класс Geksapod наследует реализацию класса AJobManager и содержит в себе экземпляр класса MotionJob, который в свою очередь наследуется от класса AJob. Эти классы позволяют реализовать так называемую невытесняющую многозадачность, позволяя абстрагироваться от линейности программ и выполнять несколько задач одновременно.
class AJob {
friend class AJobManager;
private:
AJobManager* m_pAJobManager;
AJob* mJobNext;
unsigned long m_counter; // Счетчик вызовов метода onRun
unsigned long m_previousMillis; // метка времени предыдущего вызова onRun
unsigned long m_currentMillis; // текущая метка времени
unsigned long m_delayMillis; // интервал вызова метода onRun
void run();
public:
AJob(AJobManager*, unsigned long delay = 0L);
~AJob();
void finish(); // принудительное завершение задачи
long counter(); // возвращает количество вызовов onRun с момента старта задачи
long setDelay(unsigned long); // устанавливает интервал вызова onRun
unsigned long previousMillis();// возвращает метку времени предыдущего вызова onRun
unsigned long currentMillis(); // возвращает текущую метку времени
virtual void onInit(); // вызывается один раз для инициализации задачи
virtual void onRun(); // вызывается многократно в процессе работы
virtual void onDone(); // вызывается при завершении задачи с помощью метода finish
};
Класс AJob является базовым классом для всех задач, требующих одновременного выполнения. В его наследниках должен быть переопределен метод onRun, реализующий логику выполняемой задачи. Учитывая специфику вытесняющей многозадачности, вызов данного метода не должен быть слишком тяжелым по времени. Рекомендуется разбить логику задачи на несколько более легких подзадач, каждый из которых будет выполняться за отдельный вызов onRun.
class AJobManager {
friend class AJob;
AJob* mJobFirst; // указатель на первый элемент списка задач
void attach(AJob*); // добавить задачу в список
void dettach(AJob*); // удалить задачу из списка
void dettachAll(); // удалить все задачи
public:
AJobManager();
~AJobManager();
void setup();
void loop();
};
Класс AJobManager имеет более скромное объявление и содержит всего два публичных метода: setup и loop. Метод setup должен быть вызван один раз перед запуском основного цикла программы. В нем происходит поочередная инициализация всех задач, посредством вызова соответствующего метода onInit для каждой задачи из списка.
Задача добавляется в список автоматически при вызове ее конструктора и может быть удалена вызовом публичного метода finish самой задачи.
Метод loop вызывается многократно в основном цикле программы и отвечает за поочередное исполнение логики каждой задачи из списка через заданные промежутки времени (если они установлены).
Таким образом при создании экземпляра класса Geksapod, унаследованного от класса AJobManager, мы получаем в наше распоряжение удобный инструмент мультизадачности.
Реализация движений
Любое движение тела можно описать некоторой функцией, определяющей его положение в заданный момент времени. Такая функция может быть составной, то есть представлять собой набор функций, каждая из которых применима только на определенном отрезке времени.
Различные виды движения конечностей гексапода определены и могут быть расширены с помощью классов, унаследованных от класса Motion
class Motion {
friend class MotionJob;
protected:
long m_MaxTime; // время выполнения движения в одном цикле
long m_TotalTime; // время отведенное на выполнение движения
bool m_IsLooped; // признак цикличности движения
Motion* m_pNext; // указатель на следующий движения
public:
Motion(long maxTime, bool isLooped, long totalTime = -1, Motion* pNext = NULL);
~Motion();
inline long maxTime() { return m_MaxTime; }
inline long totalTime() { return m_TotalTime; }
inline bool isLooped() { return m_IsLooped; }
// хотя бы один из этих методов должен иметь полноценную реализацию
// возвращает координаты конечностей на момент времени 0 <= time <= m_MaxTime
virtual int getPose(long time, Vector3D points[FOOTS_COUNT]) { return E_NOT_IMPL; };
// возвращает углы поворота конечностей на момент времени 0 <= time <= m_MaxTime
virtual int getPose(long time, int pose[FOOTS_COUNT][3]) { return E_NOT_IMPL; };
};
Для реализации движения переопределенный метод getPose должен возвращать положение конечностей робота на заданный промежуток времени time в интервале от 0 до mMaxTime (в миллисекундах). В том случае, когда движение зациклено (m_IsLooped == true) время движения можно ограничить задав длительность в m_TotalTime. И наконец, можно организовать последовательность движений объединив их в список.
Таким образом, у нас есть возможность описать движения робота. Но само описание (в нашем случае — некоторый экземпляр класса, унаследованного от Motion) не заставит робота двигаться. Нужен механизм, который будет переставлять ноги гексапода, руководствуясь данным ему описанием.
И этим механизмом является экземпляр класса MotionJob, объявленный в классе Geksapod.
class MotionJob: public AJob {
enum STATUS {
NONE, RUNING, STOPING
} m_Status;
Geksapod* m_pGeksapod;
Motion* m_pMotion;
long m_MotionTime;
long m_TotalTime;
public:
MotionJob(Geksapod* pGeksapod);
int execute(Motion* pMotion);
void onRun();
};
Будучи унаследованной от AJob экземпляр класса MotionJob является задачей, у которой через определенные промежутки времени вызывается метод onRun. В нем и реализован механизм, заставляющий нашего робота выполнять движения. Нам лишь остается указать ему как двигаться, указав описание движения при вызове метода execute.
На этом пока все. Осталось еще много неосвещенных моментов, про которые постараюсь написать в следующей статье. Надеюсь, что не слишком утомил читателей обилием кода. Готов ответить на все ваши вопросы.
Продолжение следует…
