标签：STM32F103 __ RW 舵机 RAM uint8 Dynamixel response define
简介
本文主要目的是建立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);
}
在添加该代码之前，会出现舵机响应信号接收不完整的情况，这会造成通信失败！
标签：STM32F103,__,RW,舵机,RAM,uint8,Dynamixel,response,define 来源： https://blog.csdn.net/m0_47989004/article/details/115795939
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。