Глава 6. Использование периферийных функциональных модулей
6.1. Общие сведения о работе с периферийными функциональными модулями
Для упрощения процесса подключения к управляющему контроллеру, а также для обеспечения возможности подключения большого коли- чества модулей к одному управляющему контроллеру, на каждом функциональном модуле, производимом ООО «Прикладная робототехника», имеется микроконтроллер, реализующий 3-пиновый интерфейс с протоколом Dynamixel. Для коммутации на каждом из периферийных модулей имеется два 3-пиновых разъема Molex, позволяющих подключать модули последовательно друг за другом.
Для обмена данными с каким-либо модулем в сети модулей необходимо знать, как минимум, следующие данные о модуле:
- Его уникальный номер – ID. По умолчанию, каждый модуль имеет свой уникальный ID. Однако, если необходимо использовать несколько одинаковых модулей в цепи, то необходимо изменить значения ID этих модулей. Также надо следить, чтобы значения ID модулей и сервоприводов DYNAMIXEL не пересекались, если такие присутствуют в сети. Таким образом, нужно следить за тем, чтобы к управляющему контроллеру не было подключено одновременно несколько устройств с одинаковыми ID.
- Скорость обмена данными – BAUD_RATE. По умолчанию, все устройства настроены на скорость в 57600 бод. При необходимости можно изменить данную скорость. Например, для использования в цепи с сервоприводами, работающими на более низкой скорости, или при использовании длинных соединительных кабелей (чем длиннее используется кабель, тем ниже скорость рекомендуется использовать).
- Регистры памяти – значения регистров памяти модуля, с которыми нужно работать. В зависимости от типа модуля и его назначения, одни и те же номера регистров могут быть использованы либо только для чтения, либо только для записи, либо для чтения и записи одновременно. Информация по доступным для взаимодействия регистрам, а также их назначения, приведены ниже. В таблицах показан тип данных, который применяется для параметров, допустимый диапазон значений (если регламентировано) и значения, записанные в регистрах по умолчанию.
Более подробную информацию о каждом из периферийных модулей
- описание его устройства и регистров, можно получить в соответствующем учебном пособии «Периферийные функциональные модули».
Далее будут рассмотрены конкретные примеры взаимодействия микрокомпьютера с различными модулями. В данном случае требуется обратить внимание на то, что подключение всех модулей к микроконтроллеру выполняется исключительно через некую совместимую периферийную плату, поддерживающую коммутацию интерфейса 1-Wire TTL, напрямую с микрокомпьютера NanoPi-AR. Такими платами на данный момент являются: робототехнический контроллер STEM Board 2 и плата расширения для интегрирования одноплатного микрокомпьютера NanoPi-AR Shield.
6.2. Модуль «Светодиод»
Модуль «Светодиод» представляет собой стандартный светодиод белого, зеленого или красного цвета, размещенный на плате с микроконтроллером. Данный модуль предполагается использовать для более удобного подключения такого индикационного элемента, как светодиод, к популярным контроллерам.
Для работы с модулем «Светодиод» используется проект «example01 led». Программа запускает тройной цикл для включения и выключения светодиода, выводя в терминал его состояние.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define LED_ID 0x09
#define LED_REG 0x1A //W 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS : set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILURE : set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS : open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILURE : open PORT {}\n", DEVICENAME);
}
uint8_t dxl_error;
for ( int i = 0; i < 3; i++ ) {
packetHandler->write1ByteTxRx(portHandler, LED_ID, LED_REG, 254, &dxl_error);
fmt::print("LED ON\n");
sleep(1);
packetHandler->write1ByteTxRx(portHandler, LED_ID, LED_REG, 0, &dxl_error);
fmt::print("LED OFF\n");
sleep(1);
}
fmt::print("Finish\n");
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Пример выполнения программы показан на Рис. 6.1.
Рис. 6.1. Пример выполнения проекта «example_01_led»
6.3. Модуль «Трёхцветный светодиод»
Модуль «Трехцветный светодиод» представляет собой стандартный RGB светодиод, размещенный на плате с микроконтроллером. Для управления каждым каналом в отдельности на модуле выведены отдельные линии. Данный модуль предполагается использовать для более удобного подключения такого индикационного элемента, как RGB-светодиод, к популярным контроллерам.
Для работы с модулем «Трёхцветный светодиод» используется проект «example_02_led». Программа запускает тройной цикл для включения и выключения светодиода красного, синего и зелёного цвета, выводя в терминал состояние текущего светодиода.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define RGB_ID 0x15
#define RGB_G 26 //W 8u
#define RGB_R 27 //W 8u
#define RGB_B 28 //W 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS : set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILURE : set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS : open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILURE : open PORT {}\n", DEVICENAME);
}
uint8_t RGB_addreses[3] = {RGB_G, RGB_R, RGB_B};
const char* RGB_data[3] = {"GREEN", "RED", "BLUE"};
uint8_t dxl_error;
for ( int i = 0; i < 3; i++ ) {
packetHandler->write1ByteTxRx(portHandler, RGB_ID, RGB_addreses[i], 254, &dxl_error);
fmt::print("{} LED ON \n", RGB_data[i]);
sleep(1);
packetHandler->write1ByteTxRx(portHandler, RGB_ID, RGB_addreses[i], 0, &dxl_error);
fmt::print("{} LED OFF \n", RGB_data[i]);
sleep(1);
}
fmt::print("Finish \n");
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Пример выполнения программы показан на Рис. 6.2.
Рис. 6.2. Пример выполнения проекта «example_02_RGB»
6.4. Модуль «Тактовая кнопка»
Модуль «Тактовая кнопка», по своей сути, является обычной тактовой кнопкой, размещенной на плате с микроконтроллером. Данный модуль предполагается использовать для более удобного подключения кнопок к популярным микроконтроллерам.
Для работы с модулем «Тактовая кнопка» используется проект «example_03_btn». Программа запускает бесконечный цикл для отправки данных о состоянии кнопки в терминал.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define BTN_ID 0x03
#define BTN_REG 27 //R 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint8_t Read1Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint8_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read1ByteTxRx(port, BTN_ID, reg, &data, &error);
if (com_result != COMM_SUCCESS) {
fmt::print("FAILURE : {} : {}\n", announce, com_result);
} else {
fmt::print("SUCCESS \n");
}
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t btnData;
while (true) {
btnData = Read1Byte(portHandler, packetHandler, BTN_REG, "");
if (btnData == 1) {
fmt::print("button is pressed ");
}
if (btnData == 0) {
fmt::print("button not pressed ");
}
sleep(1);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис. 6.3.
Рис. 6.3. Пример выполнения проекта «example_03_btn»
6.5. Модуль «Концевой выключатель»
Модуль «Концевой выключатель», по сути, представляет собой кнопку, имеющую два состояния: нажата и отпущена. При нажатии на микропереключатель происходит изменение состояния линии, анализируя которое можно делать вывод о состоянии переключателя. Данный модуль рекомендуется использовать совместно с какими-либо исполнительными механизмами, причем, таким образом, чтобы при функционировании исполнительного механизма с помощью микропереключателя фиксировать его положение. Например, модуль «Концевой выключатель» можно установить рядом с сервоприводом, перемещающим некий объект, и при достижении объектом микропереключателя можно делать вывод о достижении объектом требуемого положения. В отличие от кнопки, микропереключатель обладает язычком, для нажатия на который требуется гораздо меньше усилий, чем на нажатие кнопки, тем самым, объект при нажатии не деформируется. Для работы с модулем «Концевой выключатель» используется проект «example_04_lim». Программа запускает конечный цикл вывода состояния микропереключателя модуля до тех пор, пока микропереключатель не будет нажат. Также во время вывода происходит подсчёт итераций цикла, которые тоже выводятся в терминал в строке состояния микропереключателя.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define LIM_ID 0x17
#define LIM_REG 27 //R 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint8_t Read1Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint8_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read1ByteTxRx(port, LIM_ID, reg, &data, &error);
if (com_result != COMM_SUCCESS) {
fmt::print("FAILURE : {} : {}\n", announce, com_result);
} else {
fmt::print("SUCCESS : {} : {}\n", announce, data);
}
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t limData;
uint64_t count;
count = 0;
while (limData == 0) {
count ++;
fmt::print("Count = {}. limit switch is not pressed\n", count);
limData = Read1Byte(portHandler, packetHandler, LIM_REG, "");
sleep(0.1);
}
fmt::print("limit switch is pressed. Total count is {}\n", count);
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Пример выполнения программы показан на Рис. 6.4.
Рис. 6.4. Пример выполнения проекта «example_04_lim»
6.6. Модуль «Датчик наклона»
Модуль «Датчик наклона» представляет собой простейшее устройство для определения положения в пространстве. В отличие от подобных датчиков продвинутого уровня, позволяющих фиксировать положение сразу по всем осям и с высокой точностью, модуль «Датчик наклона» (MEMS) способен фиксировать факт наклона в 45 угловых градусов. Основным элементом данного модуля является цилиндр, внутри которого находится металлический шарик и контакты. При изменении положения модуля в пространстве, шарик перекатывается и замыкает собой контакты, что сигнализирует о наклоне модуля в 45 и более градусов. Поскольку такой цилиндр способен фиксировать факт наклона в пределах одной оси, на модуле их расположено два, перпендикулярно друг другу. Таком образом, можно будет фиксировать положение модуля в пространстве, относительно двух осей одновременно. Также, поскольку внутри каждого из двух цилиндров находятся шарики, то данный модуль можно использовать в качестве детектора вибраций. При определенном уровне внешнего воздействия на модуль, шарики внутри цилиндров придут в движение и будут замыкать собой контакты. Таким образом, можно будет фиксировать уровень внешнего воздействия.
Для работы с модулем «Датчик наклона» используется проект «example_05_pos». Программа запускает бесконечный цикл для отправки данных о состоянии датчика наклона в терминал.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define POS_ID 0x02
#define POS_REG1 26 //R 8u
#define POS_REG2 27 //R 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint8_t Read1Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint8_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read1ByteTxRx(port, POS_ID, reg, &data, &error);
if (com_result != COMM_SUCCESS) {
fmt::print("FAILURE : {} : {}\n", announce, com_result);
} else {
fmt::print("SUCCESS : {} : {}\n", announce, data);
}
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t pos1Data;
uint8_t pos2Data;
while (true) {
pos1Data = Read1Byte(portHandler, packetHandler, POS_REG1, "");
pos2Data = Read1Byte(portHandler, packetHandler, POS_REG2, "");
fmt::print("position sensor state is x = {} y = {}\n ", pos1Data, pos2Data);
sleep(1);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.5.
Рис. 6.5. Пример выполнения проекта «example_05_lim»
6.7. Модуль «Потенциометр»
Модуль «Потенциометр» представляет собой стандартный 10 кОм потенциометр с линейной характеристикой, размещенный на плате с микроконтроллером. Данный модуль предполагается использовать для более удобного подключения такого органа управления, как потенциометр, к популярным контроллерам.
Для работы с модулем «Потенциометр» используется проект «example_06_pot». Программа запускает бесконечный цикл для отправки данных о состоянии потенциометра в терминал.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define POT_ID 0x13
#define POT_REG 26 //R 16u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint16_t Read2Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint16_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read2ByteTxRx(port, POT_ID, reg, &data, &error);
if (com_result != COMM_SUCCESS) {
fmt::print("FAILURE : {} : {}\n", announce, com_result);
} else {
fmt::print("SUCCESS : {} : {}\n", announce, data);
}
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint16_t potData;
while (true) {
potData = Read2Byte(portHandler, packetHandler, POT_REG, "");
fmt::print("Potentiometer data is {}\n ", potData);
sleep(1);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.6.
Рис. 6.6. Пример выполнения проекта «example_06_pot»
6.8. Модуль «Датчик силы»
Модуль «Датчик силы» представляет собой пленочный тензорезистор, при нажатии на который изменяется сопротивление модуля.
Для работы с модулем «Датчик силы» используется проект «example_07_for». Программа запускает бесконечный цикл для отправки данных о состоянии нагрузки на датчик в терминал.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define FOR_ID 0x20
#define FOR_REG 26 //R 16u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint16_t Read2Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint16_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read2ByteTxRx(port, FOR_ID, reg, &data, &error);
if (com_result != COMM_SUCCESS) {
fmt::print("FAILURE : {} : {}\n", announce, com_result);
} else {
fmt::print("SUCCESS : {} : {}\n", announce, data);
}
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint16_t forData;
while (true) {
forData = Read2Byte(portHandler, packetHandler, FOR_REG, "");
fmt::print("Force sensor data is {}\n ", forData);
sleep(1);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.7.
Рис. 6.7. Пример выполнения проекта «example_07_for»
6.9. Модуль «Датчик шума»
Модуль «Датчик шума» представляет собой устройство, способное определять относительный уровень окружающего шума и фиксировать одиночные всплески (хлопки). Чувствительным элементом в этом модуле является микрофон, чувствительность которого настраивается при помощи подстроечного резистора.
Для работы с модулем «Датчик шума» используется проект «example_08_noi». Программа запускает бесконечный цикл для отправки уровня шума, количества отслеженных хлопков и значения временного счётчика, который записывает момент последнего хлопка.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define NOI_ID 0x12
#define NOI_REG_LEV 26 //R 8u
#define NOI_REG_COU 27 //R 8u
#define NOI_REG_TIM 28 //R 16u
#define NOI_REG_VAL 30 //R 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint8_t Read1Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint8_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read1ByteTxRx(port, NOI_ID, reg, &data, &error);
if (com_result != COMM_SUCCESS) {
fmt::print("FAILURE : {} : {}\n", announce, com_result);
} else {
fmt::print("SUCCESS : {} : {}\n", announce, data);
}
return data;
}
uint16_t Read2Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint16_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read2ByteTxRx(port, NOI_ID, reg, &data, &error);
if (com_result != COMM_SUCCESS) {
fmt::print("FAILURE : {} : {}\n", announce, com_result);
} else {
fmt::print("SUCCESS : {} : {}\n", announce, data);
}
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t dxl_error;
uint8_t levData;
uint8_t couData;
uint16_t timData;
packetHandler->write1ByteTxRx(portHandler, NOI_ID, NOI_REG_VAL, 128, &dxl_error);
while (true) {
levData = Read1Byte(portHandler, packetHandler, NOI_REG_LEV, "");
couData = Read1Byte(portHandler, packetHandler, NOI_REG_COU, "");
timData = Read2Byte(portHandler, packetHandler, NOI_REG_TIM, "");
fmt::print("Noise level - {}; number of detected claps: {}; time data: {}\n ", levData, couData, timData);
sleep(1);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.8.
Рис. 6.8. Пример выполнения проекта «example_08_noi»
6.10. Модуль «Датчик линии»
Модуль «Датчик линии» является одним из самых часто применяемых модулей в мобильной робототехнике. Данный модуль способен различать контрастные поверхности (обычно применяется к черным и белым), тем самым, позволяя решать задачи класса «следование по линии». Для использования модуль необходимо разместить на небольшом расстоянии от поверхности, на которой нанесены контрастные метки. Осно- вой данного модуля является инфракрасный (ИК) модуль, состоящий из ИК-передатчика и ИК-приемника. Фиксируя количество отраженного излучения от поверхности, модуль возвращает соответствующий сигнал.
Если зафиксировано количество излучения меньше определенного диапазона – значит, модуль находится на темной поверхности, если больше – то на белой.
Для работы с модулем «Датчик линии» используется проект «example_09_lin». Программа запускает бесконечный цикл для отправки состояния регистра данных, и, если датчик обнаружил черную линию, выводит сообщение о том, что линия обнаружена.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define LIN_ID 0x0A
#define LIN_REG 27 //R 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint8_t Read1Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint8_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read1ByteTxRx(port, LIN_ID, reg, &data, &error);
if (com_result != COMM_SUCCESS) {
fmt::print("FAILURE : {} : {}\n", announce, com_result);
} else {
fmt::print("SUCCESS : {} : {}\n", announce, data);
}
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t linData;
while (true) {
linData = Read1Byte(portHandler, packetHandler, LIN_REG, "");
fmt::print("line sensor state is {}\n", linData);
if (linData == 0) {
fmt::print("line detected\n ");
}
sleep(1);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.9.
6.11. Модуль «Датчик цвета»
Модуль «Датчик цвета» представляет собой устройство для определения цветового оттенка находящегося рядом предмета в RGB нотации. Модуль построен на классическом сенсоре – TSC3472, способном выдавать данные по I2C интерфейсу. По умолчанию, данный модуль на I2C интерфейсе имеет адрес 0x29, что позволяет обмениваться с ним данными, используя стандартные подходы.
Для работы с модулем «Датчик цвета» используется проект «example_10_col». Программа устанавливает стандартные параметры для считывания цвета и запускает бесконечный цикл для отправки параметров отслеживаемой области в терминал.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define COL_ID 0x04
#define COL_REG_CLE 24 //R 16u
#define COL_REG_RED 26 //R 16u
#define COL_REG_GRE 28 //R 16u
#define COL_REG_BLU 30 //R 16u
#define COL_REG_LUX 32 //R 16u
#define COL_REG_CTE 34 //R 16u
#define COL_REG_HUE 36 //R 16u
#define COL_REG_GAI 38 //R 8u
#define COL_REG_COL 40 //R 8u
#define COL_REG_LIG 42 //W 8u
#define COL_REG_BLA 46 //W 16u
#define COL_REG_WHI 48 //W 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint16_t Read2Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint16_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read2ByteTxRx(port, COL_ID, reg, &data, &error);
return data;
}
uint16_t Read1Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint8_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read1ByteTxRx(port, COL_ID, reg, &data, &error);
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t dxl_error;
packetHandler->write1ByteTxRx(portHandler, COL_ID, COL_REG_LIG, 0, &dxl_error);
packetHandler->write2ByteTxRx(portHandler, COL_ID, COL_REG_BLA, 850, &dxl_error);
packetHandler->write1ByteTxRx(portHandler, COL_ID, COL_REG_WHI, 7, &dxl_error);
while (true) {
fmt::print("CLEAR data is {}\n ", Read2Byte(portHandler, packetHandler, COL_REG_CLE, ""));
fmt::print("RED data is {}\n ", Read2Byte(portHandler, packetHandler, COL_REG_RED, ""));
fmt::print("GREEN data is {}\n ", Read2Byte(portHandler, packetHandler, COL_REG_GRE, ""));
fmt::print("BLUE data is {}\n ", Read2Byte(portHandler, packetHandler, COL_REG_BLU, ""));
fmt::print("LUX data is {}\n ", Read2Byte(portHandler, packetHandler, COL_REG_LUX, ""));
fmt::print("CALVIN TEMP data is {}\n ", Read2Byte(portHandler, packetHandler, COL_REG_CTE, ""));
fmt::print("GAIN data is {}\n ", Read1Byte(portHandler, packetHandler, COL_REG_GAI, ""));
fmt::print("COLOR data is {}\n\n ", Read2Byte(portHandler, packetHandler, COL_REG_COL, ""));\
sleep(1);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.10.
Рис. 6.10. Пример выполнения проекта «example_10_col»
6.12. Модуль «Датчик температуры и влажности воздуха»
Модуль «Датчик температуры и влажности воздуха» построен на базе сенсора DHT11, способного определять температуру и влажность окружающей среды в области около нормальных климатических условий (при температуре от 0 до 50 градусов и влажности от 20 до 90%).
Для работы с модулем «Датчик температуры и влажности воздуха» используется проект «example_11_hum». Программа запускает бесконечный цикл для отправки параметра влажности в процентах, параметра температуры в градусах Цельсия и параметра температуры в двухбайтовом значении.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOHUM 1.0
#define BAUDRATE 57600
#define HUM_ID 0x16
#define HUM_REG_RH_INT 24 //R 8u
#define HUM_REG_RH_DEC 26 //R 8u
#define HUM_REG_TEM_INT 28 //R 8u
#define HUM_REG_TEM_DEC 30 //R 8u
#define HUM_REG_TEM_16 34 //R 16u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint16_t Read2Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint16_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read2ByteTxRx(port, HUM_ID, reg, &data, &error);
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOHUM);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t rh_int;
uint8_t rh_dec;
uint8_t temp_int;
uint8_t temp_dec;
float temp_16;
while (true) {
rh_int = Read2Byte(portHandler, packetHandler, HUM_REG_RH_INT, "");
rh_dec = Read2Byte(portHandler, packetHandler, HUM_REG_RH_DEC, "");
temp_int = Read2Byte(portHandler, packetHandler, HUM_REG_TEM_INT, "");
temp_dec = Read2Byte(portHandler, packetHandler, HUM_REG_TEM_DEC, "");
temp_16 = Read2Byte(portHandler, packetHandler, HUM_REG_TEM_16, "")/10.0;
fmt::print("HUMIDITY is {},{} %\n ", rh_int, rh_dec);
fmt::print("TEMPERATURE is {},{} *C\n ", temp_int, temp_dec);
fmt::print("TEMP16 data is {}\n\n ", temp_16);
sleep(1);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.11.
Рис. 6.11. Пример выполнения проекта «example_11_hum»
6.13. Модуль «Звуковой пьезоизлучатель»
Модуль «Звуковой пьезоизлучатель 4 кГц» представляет собой индикационный модуль, способный издавать звуки фиксированной частоты, путем задания длины управляющего импульса.
Для работы с модулем «Звуковой пьезоизлучатель 4 кГц» используется проект «example_12_buz». Программа проигрывает 8 нот на заданных частотах от «До третьей октавы» до «До четвёртой октавы» с периодом в 1 секунду.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define BUZ_ID 0x18
#define BUZ_REG_FRE 26 //W 16u
#define BUZ_REG_POR 28 //W 8u
uint16_t C3 = 1046;
uint16_t D3 = 1174;
uint16_t E3 = 1318;
uint16_t F3 = 1397;
uint16_t G3 = 1600;
uint16_t A3 = 1720;
uint16_t B3 = 1975;
uint16_t C4 = 2093;
uint16_t note_table[8] = {C3, D3, E3, F3, G3, A3, B3, C4};
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t dxl_error;
packetHandler->write1ByteTxRx(portHandler, BUZ_ID, BUZ_REG_POR, 128, &dxl_error);
for ( int i = 0; i < 8; i++ ) {
packetHandler->write2ByteTxRx(portHandler, BUZ_ID, BUZ_REG_FRE, note_table[i], &dxl_error);
packetHandler->write1ByteTxRx(portHandler, BUZ_ID, BUZ_REG_POR, 128, &dxl_error);
fmt::print("Buzzer play note to frequency {}\n ", note_table[i]);
sleep(1);
packetHandler->write1ByteTxRx(portHandler, BUZ_ID, BUZ_REG_POR, 0, &dxl_error);
sleep(1);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Пример выполнения программы показан на Рис 6.12.
Рис. 6.12. Пример выполнения проекта «example_12_buz»
6.14. Модуль «Драйвер двигателя постоянного тока»
Модуль «Драйвер двигателя постоянного тока» (одноканальный) построен на микросхеме, которая способна управлять скоростью и направлением вращения двигателя постоянного тока за счет двух входных параметров – сигнала, задающего направление вращения (постоянный цифровой сигнал низкого или высокого уровня), и PWM сигнала, задающего скорость вращения сервопривода.
Для работы с модулем «Драйвер двигателя постоянного тока» используется проект «example_13_mot1». Программа запускает бесконечный цикл, вращающий двигатель постоянного тока в обоих направлениях на максимальной скорости, и выводя в терминал направление вращения двигателя.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define MOT1_ID 0x05
#define MOT1_REG_MPOW 24 //W 16u
#define MOT1_REG_ENCM 27 //W 8u
#define MOT1_REG_POW 28 //W 16u
#define MOT1_REG_ENC 32 //R 16u
#define MOT1_REG_PSPE 34 //R 16u
#define MOT1_REG_GSPE 40 //W 16u
#define MOT1_REG_CPER 45 //W 8u
#define MOT1_REG_PIDP 47 //W 8u
#define MOT1_REG_PIDI 48 //W 8u
#define MOT1_REG_PIDD 49 //W 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint16_t Read2Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint16_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read2ByteTxRx(port, MOT1_ID, reg, &data, &error);
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t dxl_error;
int16_t forward = 1023;
int16_t backward = -1023;
while (true) {
packetHandler->write2ByteTxRx(portHandler, MOT1_ID, MOT1_REG_POW, 0, &dxl_error);
fmt::print("STOP\n");
packetHandler->write2ByteTxRx(portHandler, MOT1_ID, MOT1_REG_POW, 1023, &dxl_error);
fmt::print("Rotate clockwise\n");
sleep(1);
packetHandler->write2ByteTxRx(portHandler, MOT1_ID, MOT1_REG_POW, 0, &dxl_error);
fmt::print("STOP\n");
packetHandler->write2ByteTxRx(portHandler, MOT1_ID, MOT1_REG_POW, -1023, &dxl_error);
fmt::print("Rotate counterclockwise\n");
sleep(1);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.13.
Рис. 6.13. Пример выполнения проекта «example_13_mot1»
6.15. Модуль «Драйвер двигателя постоянного тока (2 канала)»
Модуль «Драйвер двигателя постоянного тока (2 канала)» построен на микросхеме, которая способна управлять скоростью и направлением вращения двигателя постоянного тока за счет двух входных параметров – сигнала, задающего направление вращения (постоянный цифровой сигнал низкого или высокого уровня), и PWM сигнала, задающего скорость вращения сервопривода. Данный модуль аналогичен по системе управления модулю одноканального драйвера двигателя постоянного тока с тем отличием, что в данный модуль можно подключать два мотора одновременно.
Для работы с модулем «Драйвер двигателя постоянного тока (2 канала)» используется проект «example_13_mot2». Программа запускает бесконечный цикл, постепенно увеличивая скорость вращения обоих двигателей до максимальной скорости в одном направлении, после останавливаясь и увеличивая скорость вращения до максимальной в обратном направлении, при этом выводя в терминал направление вращения двигателя.
Листинг программы представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 57600
#define MOT2_ID 0x06
#define MOT2_REG_MPOW 24 //W 16u
#define MOT2_REG_ENCM 27 //W 8u
#define MOT2_REG_M1POW 28 //W 16u
#define MOT2_REG_M2POW 30 //W 16u
#define MOT2_REG_M1ENC 32 //R 16u
#define MOT2_REG_M1PSPE 34 //R 16u
#define MOT2_REG_M2ENC 36 //R 16u
#define MOT2_REG_M2PSPE 38 //R 16u
#define MOT2_REG_M1GSPE 40 //W 16u
#define MOT2_REG_M2GSPE 42 //W 16u
#define MOT2_REG_CPER 45 //W 8u
#define MOT2_REG_PIDP 47 //W 8u
#define MOT2_REG_PIDI 48 //W 8u
#define MOT2_REG_PIDD 49 //W 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint16_t Read2Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint16_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read2ByteTxRx(port, MOT2_ID, reg, &data, &error);
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t dxl_error;
int16_t speed;
while (true) {
for ( speed = 0; speed < 1023; speed++ ) {
packetHandler->write2ByteTxRx(portHandler, MOT2_ID, MOT2_REG_M1POW, speed, &dxl_error);
fmt::print("speed M1 = {}\n", speed);
}
for ( speed = 1023; speed > -1023; speed-- ) {
packetHandler->write2ByteTxRx(portHandler, MOT2_ID, MOT2_REG_M1POW, speed, &dxl_error);
fmt::print("speed M1 = {}\n", speed);
}
for ( speed = -1023; speed < 0; speed++ ) {
packetHandler->write2ByteTxRx(portHandler, MOT2_ID, MOT2_REG_M1POW, speed, &dxl_error);
fmt::print("speed M1 = {}\n", speed);
}
for ( speed = 0; speed < 1023; speed++ ) {
packetHandler->write2ByteTxRx(portHandler, MOT2_ID, MOT2_REG_M2POW, speed, &dxl_error);
fmt::print("speed M2 = {}\n", speed);
}
for ( speed = 1023; speed > -1023; speed-- ) {
packetHandler->write2ByteTxRx(portHandler, MOT2_ID, MOT2_REG_M2POW, speed, &dxl_error);
fmt::print("speed M2 = {}\n", speed);
}
for ( speed = -1023; speed < 0; speed++ ) {
packetHandler->write2ByteTxRx(portHandler, MOT2_ID, MOT2_REG_M2POW, speed, &dxl_error);
fmt::print("speed M2 = {}\n", speed);
}
sleep(10);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.14.
Рис. 6.14. Пример выполнения проекта «example_14_mot2»
6.16. Управление сервоприводами DYNAMIXEL
Для работы с сервоприводом DYNAMIXEL будет рассмотрено три проекта: «example_15_servor», «example_16_servow» и «example_17_servoj».
Проект «example_15_servor» выводит в терминал следующие считываемые данные сервопривода:
- номер ID;
- скорость передачи данных;
- допустимое время возвращения обратного пакета;
- предел вращения по часовой стрелке;
- предел вращения против часовой стрелки;
- текущий угол поворота привода;
- текущая скорость вращения привода;
- текущая нагрузка на валу привода;
- текущая температура привода.
Листинг проекта «example_15_servor» представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 1000000
#define SER_ID 0x01
#define SER_REG_ID 3 //WR 8u
#define SER_REG_BAU 4 //WR 8u
#define SER_REG_RET 5 //WR 8u
#define SER_REG_CW_LIM 6 //WR 16u
#define SER_REG_CCW_LIM 8 //WR 16u
#define SER_REG_PRE_POS 36 //R 16u
#define SER_REG_PRE_SPE 38 //R 16u
#define SER_REG_PRE_LOA 40 //R 16u
#define SER_REG_PRE_TEM 43 //R 8u
#define SER_REG_MOV_SPE 32 //WR 16u
#define SER_REG_GOA_POS 30 //WR 16u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint16_t Read1Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint8_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read1ByteTxRx(port, SER_ID, reg, &data, &error);
return data;
}
uint16_t Read2Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint16_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read2ByteTxRx(port, SER_ID, reg, &data, &error);
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
while (true) {
fmt::print("ID = {}\n ", Read1Byte(portHandler, packetHandler, SER_REG_ID, ""));
fmt::print("Baudrate = {}\n ", Read1Byte(portHandler, packetHandler, SER_REG_BAU, ""));
fmt::print("Return Delay Time = {}\n ", Read1Byte(portHandler, packetHandler, SER_REG_RET, ""));
fmt::print("CW Angle Limit = {}\n ", Read2Byte(portHandler, packetHandler, SER_REG_CW_LIM, ""));
fmt::print("CCW Angle Limit = {}\n ", Read2Byte(portHandler, packetHandler, SER_REG_CCW_LIM, ""));
fmt::print("Present position = {}\n ", Read2Byte(portHandler, packetHandler, SER_REG_PRE_POS, ""));
fmt::print("Present speed = {}\n ", Read2Byte(portHandler, packetHandler, SER_REG_PRE_SPE, ""));
fmt::print("Present load = {}\n ", Read2Byte(portHandler, packetHandler, SER_REG_PRE_LOA, ""));
fmt::print("Present temperature = {}\n ", Read1Byte(portHandler, packetHandler, SER_REG_PRE_TEM, ""));
sleep(1);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.15.
Рис. 6.15. Пример выполнения проекта «example_15_servor»
Проект «example_16_servow» запускает бесконечный цикл для вращения сервопривода в режиме «Wheel» по часовой и против часовой стрел- ки на максимальной скорости. Для установки сервопривода в режим «Wheel» необходимо задать регистрам CW Angle Limit и CCW Angle Limit значения «0» для отключения ограничения угла вращения вала.
Листинг проекта «example_16_servor» представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 1000000
#define SER_ID 0x01
#define SER_REG_ID 3 //WR 8u
#define SER_REG_BAU 4 //WR 8u
#define SER_REG_RET 5 //WR 8u
#define SER_REG_CW_LIM 6 //WR 16u
#define SER_REG_CCW_LIM 8 //WR 16u
#define SER_REG_PRE_POS 36 //R 16u
#define SER_REG_PRE_SPE 38 //R 16u
#define SER_REG_PRE_LOA 40 //R 16u
#define SER_REG_PRE_TEM 43 //R 8u
#define SER_REG_MOV_SPE 32 //WR 16u
#define SER_REG_GOA_POS 30 //WR 16u
#define SER_REG_TOR 24 //WR 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint16_t Read1Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint8_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read1ByteTxRx(port, SER_ID, reg, &data, &error);
return data;
}
uint16_t Read2Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint16_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read2ByteTxRx(port, SER_ID, reg, &data, &error);
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t dxl_error;
packetHandler->write2ByteTxRx(portHandler, SER_ID, SER_REG_CW_LIM, 0, &dxl_error);
packetHandler->write2ByteTxRx(portHandler, SER_ID, SER_REG_CCW_LIM, 0, &dxl_error);
packetHandler->write1ByteTxRx(portHandler, SER_ID, SER_REG_TOR, 1, &dxl_error);
fmt::print("Start wheel mode \n ");
while (true) {
packetHandler->write2ByteTxRx(portHandler, SER_ID, SER_REG_MOV_SPE, 2047, &dxl_error);
fmt::print("Moving speed = {}\n ", Read2Byte(portHandler, packetHandler, SER_REG_MOV_SPE, ""));
sleep(3);
packetHandler->write2ByteTxRx(portHandler, SER_ID, SER_REG_MOV_SPE, 1023, &dxl_error);
fmt::print("Moving speed = {}\n ", Read2Byte(portHandler, packetHandler, SER_REG_MOV_SPE, ""));
sleep(3);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.16.
Рис. 6.16. Пример выполнения проекта «example_16_servow»
Проект «example_17_servoj» запускает бесконечный цикл для установки крайних положений угла поворота вала сервопривода в режиме «Joint». Для установки сервопривода в режим «Joint» необходимо задать регистрам «CW Angle Limit» и «CCW Angle Limit» значения «0» и «1023», соответственно.
Листинг проекта «example_17_servoj» представлен ниже:
cpp
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <map>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <thread>
#include "dynamixel_sdk.h"
#include "fmt/format.h"
#define DEVICENAME "/dev/ttyS2"
#define PROTOCOL 1.0
#define BAUDRATE 1000000
#define SER_ID 0x01
#define SER_REG_ID 3 //WR 8u
#define SER_REG_BAU 4 //WR 8u
#define SER_REG_RET 5 //WR 8u
#define SER_REG_CW_LIM 6 //WR 16u
#define SER_REG_CCW_LIM 8 //WR 16u
#define SER_REG_PRE_POS 36 //R 16u
#define SER_REG_PRE_SPE 38 //R 16u
#define SER_REG_PRE_LOA 40 //R 16u
#define SER_REG_PRE_TEM 43 //R 8u
#define SER_REG_MOV_SPE 32 //WR 16u
#define SER_REG_GOA_POS 30 //WR 16u
#define SER_REG_TOR 24 //WR 8u
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
void setRegister(int id, int reg, int size, int value) {
fmt::print("setRegister\n DEVICE: {}\n REG: {}\n SIZE: {}\n VALUE: {}\n", id, reg, size, value);
int retry = 0;
do {
if ( packetHandler->writeTxRx(portHandler, id, reg, size, (uint8_t *)&value) == COMM_SUCCESS ) {
fmt::print("--->OK\n");
return;
}
retry++;
}
while ( retry < 3 );
fmt::print("--->FAILED\n");
}
uint16_t Read1Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint8_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read1ByteTxRx(port, SER_ID, reg, &data, &error);
return data;
}
uint16_t Read2Byte(dynamixel::PortHandler *port, dynamixel::PacketHandler *packet, uint8_t reg = 0, std::string announce = "" ) {
uint16_t data = 0;
uint8_t error = 0;
int com_result = 0;
com_result = packet->read2ByteTxRx(port, SER_ID, reg, &data, &error);
return data;
}
int main() {
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL);
if (portHandler->setBaudRate(BAUDRATE)) {
fmt::print("SUCCESS: set BAUDRATE to {}\n",BAUDRATE);
} else {
fmt::print("FAILED: set BAUDRATE to {}\n",BAUDRATE);
return 1;
}
if (portHandler->openPort()) {
fmt::print("SUCCESS: open PORT {}\n", DEVICENAME);
} else {
fmt::print("FAILED: open PORT {}\n", DEVICENAME);
}
uint8_t dxl_error;
packetHandler->write2ByteTxRx(portHandler, SER_ID, SER_REG_CW_LIM, 0, &dxl_error);
packetHandler->write2ByteTxRx(portHandler, SER_ID, SER_REG_CCW_LIM, 1023, &dxl_error);
packetHandler->write1ByteTxRx(portHandler, SER_ID, SER_REG_TOR, 1, &dxl_error);
fmt::print("Start joint mode \n ");
while (true) {
packetHandler->write2ByteTxRx(portHandler, SER_ID, SER_REG_GOA_POS, 0, &dxl_error);
fmt::print("Goal position = {}\n ", Read2Byte(portHandler, packetHandler, SER_REG_GOA_POS, ""));
sleep(3);
packetHandler->write2ByteTxRx(portHandler, SER_ID, SER_REG_GOA_POS, 1023, &dxl_error);
fmt::print("Goal position = {}\n ", Read2Byte(portHandler, packetHandler, SER_REG_GOA_POS, ""));
sleep(3);
}
portHandler->closePort();
fmt::print("CLOSED PORT\n");
return 0;
}Для завершения программы необходимо нажать сочетание клавиш «CTRL+C». Пример выполнения программы показан на Рис 6.17.
Рис. 6.17. Пример выполнения проекта «example_17_servoj»