STM32

申根换

7年用户 1576经验值

私信关注

[问答]

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

回帖（1）

凌流浪

2021-12-14 10:09:07

简介

本文主要目的是建立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 "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
简介

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

举报

更多回帖

申根换

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

回帖（1）

凌流浪

相关问答

mbedTLS如何建立安全通信连接？

请问stca15系列和cpld的epm240系列怎么通信连接？

如何实现ESP8266 WIFI与ONENET通信连接？

nucleo l476rg和st25ru3993 hpev如何通过spi通信连接读取标签数据？

STM32驱动舵机时的问题？？？？？？？

MSP430F5529　HID 与linux主机通过借助libu***1.0.9进行通信，为什么有时通信连接会断？？

如何使用串口通信连接GY-33颜色传感器完成对颜色的识别？

STM32如何控制舵机？

串口通信建立连接问题

stm32控制舵机的代码

20万+工程师都在用，免费PCB检查工具

申根换

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

回帖（1）

凌流浪

相关问答

mbedTLS如何建立安全通信连接？

请问stca15系列和cpld的epm240系列怎么通信连接？

如何实现ESP8266 WIFI与ONENET通信连接？

nucleo l476rg和st25ru3993 hpev如何通过spi通信连接读取标签数据？

STM32驱动舵机时的问题？？？？？？？

MSP430F5529 HID 与linux主机通过借助libu***1.0.9进行通信，为什么有时通信连接会断？？

如何使用串口通信连接GY-33颜色传感器完成对颜色的识别？

STM32如何控制舵机？

串口通信建立连接问题

stm32控制舵机的代码

20万+工程师都在用，免费PCB检查工具

MSP430F5529　HID 与linux主机通过借助libu***1.0.9进行通信，为什么有时通信连接会断？？