如何建立STM32与Dynamixel舵机间的通信连接？

简介

本文主要目的是建立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协议

舵机控制协议实现主要参考官方手册，具体见：

• MX-64
  
• protocol1.0
  
  

链接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 "rn", ##__VA_ARGS__)
#define user_main_info(format, ...)                 USARTx_printf(USART1, "[INFO] [%s@%s,%d] " format "rn", __FILE__, __func__, __LINE__, ##__VA_ARGS__);
#define user_main_debug(format, ...)    USARTx_printf(USART1, "[DEBUG] [%s@%s,%d] " format "rn", __FILE__, __func__, __LINE__, ##__VA_ARGS__)
#define user_main_error(format, ...)                 USARTx_printf(USART1, "[ERROR] [%s@%s,%d] " format "rn", __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);  // parameters
        checksum += params;
    }
   
    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 = 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);
   
   
    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;
    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                         user_main_debug("%d", RS485_RX_BUF);
        }               
}


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);
}
在添加该代码之前，会出现舵机响应信号接收不完整的情况，这会造成通信失败！
修改备注
之所以之前产生中断嵌套的问题是由于两个串口之间的优先级设置相同，这里采用的更为有效率的方式是通过设置串口优先级的方式，优先级具体参见：STM32中断优先级彻底讲解。多说一句，优先级数值越大代表优先级越高，之前也尝试过设置优先级，但认为优先级数值越小优先级越高。
源代码地址：STM32ToDynamixel