- •Содержание
- •1 Основная часть 4
- •2 Техническая часть 13
- •3 Реализация алгоритмов управления роботом 25
- •Введение
- •1 Основная часть
- •1.1 Описание платформы Hexapod.
- •1.1.1 Описание работы сервомоторов cds5516
- •1.1.2 Микроконтроллер Arduino Mega 2560
- •1.2 Последовательный порт (com)
- •1.3 Средства Visual Studio для работы с com портом
- •2 Техническая часть
- •2.1 Основные инструкции для управления сервомоторами
- •2.2 Инверсная кинематика
- •2.3 Вычисление углов для сервомоторов
- •3 Реализация алгоритмов управления роботом
- •Заключение
- •Список использованных источников
2 Техническая часть
2.1 Основные инструкции для управления сервомоторами
Как было сказано выше управление сервомоторами осуществляется с помощью пакетов инструкций. Микроконтроллер выступает в качестве ведущего устройства, сервомоторы в качестве ведомого. Последовательность отправки сообщений такова:
1. Ведущий посылает инструкцию в виде пакета
2. Ведомый получив инструкцию, выполняет ее, и взамен отсылает ответный пакет ведущему.
Есть два типа пакетов – это «Instruction Packet» (пакет – инструкция), который передается сервомоторам, и «Status Packet» (статус – пакет), который возвращается от сервомотора микроконтроллеру.
«Instruction Packet» имеет формат представленный в таблице 4
Таблица 4. Формат пакета – инструкции
Header |
ID |
LENGTH |
INSTRUCTION |
PARAMETER |
CHECK SUM |
0xFF 0xFF |
ID |
Length |
Instruction |
Parameter(1..N) |
0x?? |
HEADER (заголовок) |
Два байта 0xFF в пакете указывают на начало входящего пакета команд |
ID |
Уникальное значение для каждого сервомотора. Может принимать значения от 0 до 254 (от 0х00 и 0хFВ). 0хFE – используется в качестве широковещательного ID. Пакеты отправленные по данному ID будут выполнены всеми сервомоторами CDS5516. Отсюда вытекает, что пакеты отправленные по широковещательному ID не вернут статус пакет. |
LENGTH (длина пакета) |
Длина пакета вычисляется, как сумма количества параметров плюс два (Number of parameters (N)+2) |
INSTRUCTION (инструкция) |
Инструкция для сервомотора, выбирается из таблицы адресов в памяти контроллера сервомотора |
PARAMETER0..N (параметры) |
Данный байты используются в качестве информационных, которые следует отправить |
CHECK SUM (контрольная сумма) |
Используется в качестве проверки переданных данных. Вычисление контрольной суммы производится по следующей формуле CS=~(ID+Length+Instruction+Parameter1+…+ParameterN), где «~» логическое отрицание. Если вычисленное значение больше 255, то выбирается младший байт контрольной суммы. |
«Status packet» возвращается микроконтроллеру после выполнения пакета инструкции. Формат статус – пакета представлен в таблице 5
Таблица 5. Формат статус – пакета
Header |
ID |
LENGTH |
ERROR |
PARAMETER |
CHECK SUM |
0xFF 0xFF |
ID |
Length |
Status |
Parameter(1..N) |
0x?? |
HEADER (заголовок) |
Два байта 0xFF указывают на начало пакета |
ID |
Возвращает уникальный ID от сервомотора |
LENGTH |
Возвращает байты параметров, «Number of parameters (N) + 2» |
ERROR (ошбка) |
Байт возвращает биты ошибок. Биты ошибок представлены в таблице 6 |
Таблица 6.Биты ошибок возвращаемые сервомотором
БИТ |
Название |
Описание |
BIT7 |
0 |
--- |
BIT6 |
Ошибка в инструкции |
Возвращает единицу, если отправлена неизвестная инструкция |
BIT5 |
Перегрузка |
Возвращает единицу, если превышена максимальная нагрузка |
BIT4 |
Контрольная сумма |
Возвращает единицу, если произошла ошибка в подсчете контрольной суммы |
BIT3 |
Вне диапазона значений |
Возвращает единицу, если принятая величина угла поворота находится вне диапазона значений |
BIT2 |
Перегрев |
Возвращает единицу, если произошел перегрев, значение температуры может устанавливаться вручную |
BIT1 |
Значение поворота выше предельного |
Возвращает единицу, когда пересланное значение находится за пределами установленных лимитов |
BIT0 |
Ошибка во входном напряжении |
Возвращает единицу, если напряжение выходит за пределы установленного значения |
PARAMETER0..N |
Используется, когда необходимо вернуть дополнительную информацию |
CHECK SUM (контрольная сумма) |
Используется в качестве проверки переданных данных. Вычисление контрольной суммы производится по следующей формуле CS=~(ID+Length+Instruction+Parameter1+…+ParameterN), где «~» логическое отрицание. Если вычисленное значение больше 255, то выбирается младший байт контрольной суммы. |
В качестве примера рассмотрим пакет: «Установка ID сервомотора». Для установки ID следует по широковещательному адресу, используя инструкцию WRITE_DATA (0x03), записать по адресу 0x03 (отвечает за ID сервомотора) записать 0x01 (номер сервомотора). Инструкция будет иметь следующий вид:
Таблица 7. Установка ID
Header |
ID |
LENGTH |
INSTRUCTION |
PARAMETER |
CHECK SUM |
0xFF 0xFF |
0xFE |
0x04 |
0x03 |
0x03 0x01 |
0xF6 |
Переведем данную инструкцию в программный код. Она примет следующий вид:
void setup //начальные установки
{
Serial.begin(9600); //установка скорости работы с платой
Serial1.begin(1000000); //установка скорости работы сервомотор
}
void loop
{
byte header = 0xff; //байт заголовка
byte id=0xfe; //широковещательный адрес
byte length=0x04; //длина пакета
byte instruct=0x03 ; //инструкция, запись данных
byte pararm1=0x03; //параметр, адрес в памяти
byte pararm2=0x01; //параметр, значение записываемое в память
byte cs = (~(id+length+instruc+param1+param2)&0xff); //контрольная сумма
byte pack[8]={ header, header,id, length, instruct, pararm1, pararm2,cs}; //пакет в виде массива
Serial1.write(pack,8); //отправка сообщения
delay(1000); //задержка 1 секунда
}