简介
本文主要目的是建立STM32与Dynamixel舵机间的通信连接，开发上位机——下位机——舵机的控制框架，在上位机下发指令，下位机执行舵机力控外环，舵机实现位置控制内环。其中上位机与下位机、下位机与舵机之间均采用串口通信（上位机与下位机间通过USART1通信，下位机与舵机间通过RS485(USART2)通信）。
硬件平台
本文中涉及硬件为：
- STM32F1精英版
- Dynamixel MX64 AR（协议1.0）
- 飞特总线舵机USB转RS485/TTL转接板（代替U2D2）
硬件连接
注意
- RS485连线不能接反，即：A<–>A，B<–>B，MX 64AR的A端口为Data+,B端口为Data-
- RS485仅仅定义了物理接口及电气特性，并没有规定具体的协议
MX64协议
舵机控制协议实现主要参考官方手册，具体见：
链接1中主要关注Control table，其中定义了舵机各项寄存器所处位置（EEPROM, RAM），寄存器地址及长度，典型的属性如下：
[control table]
# addr | item name | length | access | memory | min value | max value | signed
0 | model_number | 2 | R | EEPROM | 0 | 65535 | N
2 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N
3 | ID | 1 | RW | EEPROM | 0 | 252 | N
4 | baudrate | 1 | RW | EEPROM | 0 | 252 | N
5 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N
6 | CW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N
8 | CCW_angle_limit | 2 | RW | EEPROM | 0 | 4095 | N
10 | drive_mode | 1 | RW | EEPROM | 0 | 3 | N
11 | max_temperature_limit | 1 | RW | EEPROM | 0 | 99 | N
12 | min_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N
13 | max_voltage_limit | 1 | RW | EEPROM | 0 | 250 | N
14 | max_torque | 2 | RW | EEPROM | 0 | 1023 | N
16 | status_return_level | 1 | RW | EEPROM | 0 | 2 | N
17 | alarm_LED | 1 | RW | EEPROM | 0 | 127 | N
18 | alarm_shutdown | 1 | RW | EEPROM | 0 | 127 | N
20 | multi_turn_offset | 2 | RW | EEPROM | -26624 | 26624 | Y
22 | resolution_dividor | 1 | RW | EEPROM | 1 | 255 | N
24 | torque_enable | 1 | RW | RAM | 0 | 1 | N
25 | LED | 1 | RW | RAM | 0 | 1 | N
26 | position_d_gain | 1 | RW | RAM | 0 | 254 | N
27 | position_i_gain | 1 | RW | RAM | 0 | 254 | N
28 | position_p_gain | 1 | RW | RAM | 0 | 254 | N
30 | goal_position | 2 | RW | RAM | -28672 | 28672 | Y
32 | goal_velocity | 2 | RW | RAM | 0 | 1023 | N
34 | goal_torque | 2 | RW | RAM | 0 | 1023 | N
36 | present_position | 2 | R | RAM | -32768 | 32767 | Y
38 | present_velocity | 2 | R | RAM | 0 | 2048 | N
40 | present_load | 2 | R | RAM | 0 | 2048 | N
42 | present_voltage | 1 | R | RAM | 50 | 250 | N
43 | present_temperature | 1 | R | RAM | 0 | 99 | N
44 | registered_instruction | 1 | R | RAM | 0 | 1 | N
46 | is_moving | 1 | R | RAM | 0 | 1 | N
47 | EEPROM_lock | 1 | RW | RAM | 0 | 1 | N
48 | punch | 2 | RW | RAM | 0 | 1023 | N
68 | current_consumption | 2 | RW | RAM | 0 | 4095 | N
70 | torque_control_mode | 1 | RW | RAM | 0 | 1 | N
71 | torque_control_goal | 2 | RW | RAM | 0 | 2047 | N
73 | goal_acceleration | 1 | RW | RAM | 0 | 254 | N
链接2中是Dynamixel舵机的具体通信协议
- 指令格式
Header1 Header2 Packet ID Length Instruction Param 1 … Param N Checksum
0xFF 0xFF Packet ID Length Instruction Param 1 … Param N CHKSUM
- 状态格式
Header1 Header2 Packet ID Length Error Param 1 … Param N Checksum
0xFF 0xFF ID Length Error Param 1 … Param N CHKSUM
本次主要实现三种通信功能实例
- ping
- read temperature
- write goal position
开发前的准备
基于C++开发STM32程序
开发IDE为Keil V5，该开发环境支持C++编译，为简化开发难度，本程序代码基于C++编写，具体如何基于C++开发STM32程序见：STM32 C++ 串口通信
串口打印便于Debug
嵌入式开发一大难点便是Debug难度高，一般会通过串口打印获得当前硬件运行状态进而判断代码执行情况，本次用到的串口打印代码文件mLog.h如下：
#ifndef MLOG_H_
#define MLOG_H_
#include "usart.h"
#ifndef DEBUG_INFO
#define DEBUG_INFO
#endif
#ifdef DEBUG_INFO
#define user_main_printf(format, ...) USARTx_printf(USART1, format "\r\n", ##__VA_ARGS__)
#define user_main_info(format, ...) USARTx_printf(USART1, "[INFO] [%s@%s,%d] " format "\r\n", __FILE__, __func__, __LINE__, ##__VA_ARGS__);
#define user_main_debug(format, ...) USARTx_printf(USART1, "[DEBUG] [%s@%s,%d] " format "\r\n", __FILE__, __func__, __LINE__, ##__VA_ARGS__)
#define user_main_error(format, ...) USARTx_printf(USART1, "[ERROR] [%s@%s,%d] " format "\r\n", __FILE__, __func__, __LINE__, ##__VA_ARGS__)
#else
#define user_main_printf(format, ...)
#define user_main_info(format, ...)
#define user_main_debug(format, ...)
#define user_main_error(format, ...)
#endif
#endif
预定义的宏可在IDE中添加，具体见：
代码实现
USART与RS485通信
usart.h, usart.c, rs485.h, rs485.c来源于正点原子通信代码
舵机通信协议封装
将舵机通信协议封装在Servo类中，具体见下：
// servo.h
#include "stdio.h"
#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "mLog.h"
#include "rs485.h"
#define REC_BUFFER_LEN 32
#define SERVO_MAX_PARAMS (REC_BUFFER_LEN - 5)
#define REC_WAIT_START_US 75
#define REC_WAIT_PARAMS_US (SERVO_MAX_PARAMS * 5)
#define REC_WAIT_MAX_RETRIES 200
#define SERVO_INSTRUCTION_ERROR (1 << 6)
#define SERVO_OVERLOAD_ERROR (1 << 5)
#define SERVO_CHECKSUM_ERROR (1 << 4)
#define SERVO_RANGE_ERROR (1 << 3)
#define SERVO_OVERHEAT_ERROR (1 << 2)
#define SERVO_ANGLE_LIMIT_ERROR (1 << 1)
#define SERVO_INPUT_VOLTAGE_ERROR (1)
enum ServoCommand
{
PING = 1,
READ = 2,
WRITE = 3
};
typedef struct ServoResponse
{
uint8_t id;
uint8_t length;
uint8_t error;
uint8_t params[SERVO_MAX_PARAMS];
uint8_t checksum;
} ServoResponse;
class Servo{
public:
Servo(u8 servoID=1, u32 baudrate=57600){
m_baudrate=baudrate;
m_servoID=servoID;
delay_init();
}
void OpenPort(){
RS485_Init(m_baudrate);
}
bool pingServo ();
bool setServoAngle (const int angle);
bool getServoAngle (int *angle);
int getTemperature();
private:
void sendServoCommand (const ServoCommand commandByte,
const uint8_t numParams,
const uint8_t *params);
bool getServoResponse ();
bool getAndCheckResponse ();
int getServoBytesAvailable ();
void sendServoByte(uint8_t byte);
private:
u32 m_baudrate;
u8 m_servoID;
ServoResponse m_response;
};
// servo.cpp
// from control table
#define RETURN_DELAY 0x05
#define BLINK_CONDITIONS 0x11
#define SHUTDOWN_CONDITIONS 0x12
#define TORQUE 0x22
#define MAX_SPEED 0x20
#define CURRENT_SPEED 0x26
#define GOAL_ANGLE 0x1e
#define CURRENT_ANGLE 0x24
#define TEMPRETURE 0x2b
// response location
#define SERVO_ID_POS 2
#define SERVO_LEN_POS 3
#define SERVO_ERROR_POS 4
#define SERVO_PARAM_POS 5
// public
// ping a servo, returns true if we get back the expected values
bool Servo::pingServo ()
{
sendServoCommand (PING, 0, 0);
if (!getAndCheckResponse ())
return false;
return true;
}
bool Servo::setServoAngle (const int angle)
{
if (angle < 0 || angle > 0xfff)
return false;
const uint8_t highByte = (uint8_t)((angle >> 8) & 0xff);
const uint8_t lowByte = (uint8_t)(angle & 0xff);
const uint8_t params[3] = {GOAL_ANGLE,
lowByte,
highByte};
sendServoCommand (WRITE, 3, params);
if (!getAndCheckResponse ())
return false;
return true;
}
bool Servo::getServoAngle (int *angle)
{
const uint8_t params[2] = {CURRENT_ANGLE,
2};
sendServoCommand (READ, 2, params);
if (!getAndCheckResponse ())
return false;
uint16_t angleValue = m_response.params[1];
angleValue <<= 8;
angleValue |= m_response.params[0];
*angle = angleValue;
return true;
}
int Servo::getTemperature()
{
const uint8_t params[2] = {TEMPRETURE,
0x01};
sendServoCommand(READ, 2, params);
if (!getAndCheckResponse ())
return -1;
int tempreture=m_response.params[0];
return tempreture;
}
// private
void sendServoCommand (const ServoCommand commandByte,
const uint8_t numParams,
const uint8_t *params);
{
sendServoByte (0xff);
sendServoByte (0xff); // command header
sendServoByte (m_servoId); // servo ID
uint8_t checksum = m_servoId;
sendServoByte (numParams + 2); // number of following bytes
sendServoByte ((uint8_t)commandByte); // command
checksum += numParams + 2 + commandByte;
for (uint8_t i = 0; i < numParams; i++)
{
sendServoByte (params[i]); // parameters
checksum += params[i];
}
sendServoByte (~checksum); // checksum
RS485_RX_CNT=0; // 清空接收缓存
// **import** 避免两个串口中断干涉 打开USART2串口接收中断 关闭USART1串口接收中断
DisableUsart1RXIT();
}
bool Servo::getServoResponse ()
{
uint8_t retries = 0;
uint8_t res[REC_BUFFER_LEN];
uint8_t len;
while (getServoBytesAvailable() < 4)
{
retries++;
if (retries > REC_WAIT_MAX_RETRIES)
{
user_main_error("Too many retries at start");
return false;
}
delay_ms (REC_WAIT_START_US); // delay_us
}
retries = 0;
RS485_Receive_Data(res, &len);
m_response.id = res[SERVO_ID_POS];
m_response.length = res[SERVO_LEN_POS];
if (m_response.length > SERVO_MAX_PARAMS)
{
user_main_error("Response length too big: %d", (int)m_response.length);
return false;
}
if(len-SERVO_LEN_POS < m_response.length-1) // -1 or 0
{
user_main_error("Too many retries waiting for params, got %d of %d params", getServoBytesAvailable(), m_response.length);
return false;
}
m_response.error = res[SERVO_ERROR_POS];
for (uint8_t i = 0; i < m_response.length - 2; i++)
m_response.params[i] = res[SERVO_PARAM_POS+i];
user_main_debug("Response %d, %d, %d", (int)m_response.id, (int)m_response.length, (int)m_response.error);
for (uint8_t i = 0; i < m_response.length - 2; i++)
user_main_debug("%d", m_response.params[i]);
uint8_t calcChecksum = m_response.id + m_response.length + m_response.error;
for (uint8_t i = 0; i < m_response.length - 2; i++)
calcChecksum += m_response.params[i];
calcChecksum = ~calcChecksum;
const uint8_t recChecksum = res[len-1];
if (calcChecksum != recChecksum)
{
user_main_error("Checksum mismatch: %d calculated, %d received", calcChecksum, recChecksum);
return false;
}
return true;
}
bool Servo::getAndCheckResponse ()
{
if (!getServoResponse())
{
user_main_error("Servo error: Servo %d did not respond correctly or at all", (int)m_servoId);
return false;
}
if (m_response.id != m_servoId)
{
user_main_error("Servo error: Response ID %d does not match command ID %d", (int)m_response.id, m_servoId);
return false;
}
if (m_response.error != 0)
{
user_main_error("Servo error: Response error code was nonzero (%d)", (int)m_response.error);
return false;
}
return true;
}
int Servo::getServoBytesAvailable ()
{
return RS485_RX_CNT;
}
void Servo::sendServoByte (uint8_t byte)
{
RS485_Send_Data(&byte, 1);
}
// main.cpp
void ShowResponse(){
for(int i=0;i<RS485_RX_CNT;i++){
user_main_debug("%d", RS485_RX_BUF[i]);
}
}
int main(void)
{
delay_init();
NVIC_Configuration();
uart_init(9600); // USART1
DisableUsart1RXIT();
Servo servo;
servo.OpenPort();
bool bflag=servo.pingServo();
if(bflag){
DisableUsart2RXIT();
ShowResponse();
delay_ms(1000);
DisableUsart1RXIT();
while(1)
{
// TODO
}
}
结果验证
重要
由于涉及同时开启两个USART接收中断，所以可能出现中断嵌套的问题，及一个中断处理函数被令一个中断处理函数打断，造成数据接收不全的BUG。为了避免这种情况，添加了两个处理函数如下：
// Disable Usart1RXIT Enable Usart2RXIT
void DisableUsart1RXIT()
{
USART_ITConfig(USART1,USART_IT_RXNE,DISABLE);
USART_ITConfig(USART2,USART_IT_RXNE,ENABLE);
USART_Cmd(USART2,ENABLE);
}
// Disable Usart2RXIT Enable Usart1RXIT
void DisableUsart2RXIT()
{
USART_ITConfig(USART2,USART_IT_RXNE,DISABLE);
USART_ITConfig(USART1,USART_IT_RXNE,ENABLE);
USART_Cmd(USART1,ENABLE);
}
在添加该代码之前，会出现舵机响应信号接收不完整的情况，这会造成通信失败！