【学习笔记】演示系统1
Dynamixel Read and Write Protocal 1.0
Dynamixel的协议族有两个版本
protocol1.0为最为常用版本，也是常用舵机AX和MX系列舵机的协议形式，
protocol2.0是新款舵机和教育套件的协议形式。
一些头文件
unistd.h unix类系统定义符号常量的头文件，包含许多系统服务函数原型
fcnl.h 定义很多宏和open，fcnl函数原型
示例代码
/*
* read_write.c
*
* Created on: 2016. 5. 16.
* Author: Leon Ryu Woon Jung
*/
//
// ********* Read and Write Example *********
//
//
// Available DXL model on this example : All models using Protocol 1.0
// This example is designed for using a Dynamixel MX-28, and an USB2DYNAMIXEL.
// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below "#define"d variables yourself.
// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000 [1M])
//
#ifdef __linux__
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#elif defined(_WIN32) || defined(_WIN64)
#include <conio.h>
#endif
//以上头文件可获取在运行过程中的关键输入中断。
//获取输入的实际功能
#include <stdlib.h>
#include <stdio.h>
#include "dynamixel_sdk.h" // Uses Dynamixel SDK library //所有的Dynamixel SDK库都通过dynamixel_sdk.h这个头文件连接起来
// 每一个控制表的地址以及比特长度
#define ADDR_MX_TORQUE_ENABLE 24 // Control table address is different in Dynamixel model
#define ADDR_MX_GOAL_POSITION 30
#define ADDR_MX_PRESENT_POSITION 36
// Protocol version
#define PROTOCOL_VERSION 1.0 // See which protocol version is used in the Dynamixel
// Default setting(默认设置)
//以下设置可以根据自己的需求自由改变
#define DXL_ID 1 // Dynamixel ID: 1
#define BAUDRATE 1000000
#define DEVICENAME "/dev/ttyUSB0" // Check which port is being used on your controller
// ex) Windows: "COM1" Linux: "/dev/ttyUSB0"
#define TORQUE_ENABLE 1 // Value for enabling the torque驱动扭轴的值
#define TORQUE_DISABLE 0 // Value for disabling the torque禁用扭轴的值
#define DXL_MINIMUM_POSITION_VALUE 100 // Dynamixel will rotate between this value
#define DXL_MAXIMUM_POSITION_VALUE 4000 // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)移动范围
#define DXL_MOVING_STATUS_THRESHOLD 10 // Dynamixel moving status threshold验证或轮换停止的标准
#define ESC_ASCII_VALUE 0x1b
//以下代码接收关键输入，主要用getch接收按键信息
int getch()
{
#ifdef __linux__
struct termios oldt, newt; //串口
int ch;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
return ch;
#elif defined(_WIN32) || defined(_WIN64)
return _getch();
#endif
}
int kbhit(void)
{
#ifdef __linux__
struct termios oldt, newt;
int ch;
int oldf;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); //改变已打开文件性质
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
fcntl(STDIN_FILENO, F_SETFL, oldf);
if (ch != EOF)
{
ungetc(ch, stdin);
return 1;
}
return 0;
#elif defined(_WIN32) || defined(_WIN64)
return _kbhit();
#endif
}
//main函数实施控制
int main()
{
// Initialize PortHandler Structs
// Set the port path设置端口路径
// Get methods and members of PortHandlerLinux or PortHandlerWindows获取它们的方法和成员
int port_num = portHandler(DEVICENAME);
// Initialize PacketHandler Structs
packetHandler();
int index = 0; //变量指向机械臂应该旋转的方向
int dxl_comm_result = COMM_TX_FAIL; // Communication result表示数据包通讯期间发生了错误
int dxl_goal_position[2] = { DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position存储它旋转的目标点
uint8_t dxl_error = 0; // Dynamixel error
uint16_t dxl_present_position = 0; // Present position当前指向
// Open port打开端口，如果没能打开，则终止
if (openPort(port_num))
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Set port baudrate 设置端口波特率
if (setBaudRate(port_num, BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
getch();
return 0;
}
// Enable DXL Torque启用DXL扭矩
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
else
{
printf("Dynamixel has been successfully connected \n");
}
//通过分组发送，获取机械臂当前位置
while (1)
{
printf("Press any key to continue! (or press ESC to quit!)\n");
if (getch() == ESC_ASCII_VALUE)
break;
// Write goal position记录目标位置
write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
do
{
// Read present position读取当前位置
dxl_present_position = read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_POSITION);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
printf("[ID:%03d] GoalPos:%03d PresPos:%03d\n", DXL_ID, dxl_goal_position[index], dxl_present_position);
} while ((abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD));
// Change goal position
if (index == 0)
{
index = 1;
}
else
{
index = 0;
}
}
// Disable Dynamixel Torque
write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE);
if ((dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
}
else if ((dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
{
printRxPacketError(PROTOCOL_VERSION, dxl_error);
}
// Close port
closePort(port_num);
return 0;
}