stm32f4驱动数字舵机 XL430-W250-T

• 最近有一个项目需要用到数字舵机。和普通的pwm舵机直接用主机发送pwm波控制不同，数字舵机内部有一个小单片机，它直接控制数字舵机工作，主机通过向内部单片机发送指令间接控制舵机。
• 数字舵机功能强大，我用的这款XL430-W250-T舵机，其RAM区和ROM区一共有上百个参数可以自由读写，我们可以命令它在四种不同的运行模式间切换，甚至还能修改内部pid控制的参数。当然，这种舵机价格比较高，我这一款要三百多块
• 数字舵机一般使用半双工串口控制，给它编程，主要工作就是写一些数据帧收发、编码解码的函数

---

一、XL430-W250-T舵机

1. 此舵机是ROBOTIS在2018年推出的一块数字舵机
2. 舵机基本介绍
   • 重点关注一下这个：
     
     可见舵机使用TTL电平的半双工异步串行通信接口通信，而且是长8位，1停止位，无奇偶校验的
3. 舵机详细介绍：重点关注里面Control Table部分内容，我们给舵机发的各种指令本质就是读写这个控制表
4. 通信协议：这是此舵机使用的通信协议，我们要依据这个进行指令帧（Instruction Packet）编码和状态帧（Status Packet）解码
   • 注意里面Packet Process部分介绍了逐位接收解析状态帧的方法，不过我使用的是DMA接受方法，直接收到整帧，和这部分说的有所不同

二、半双工串口配置

1. stm32的串口支持半双工配置，可以参考我以前的这篇文章：stm32 USART串口半双工功能测试
2. 经过一些测试，我发现不能用DMA方法给这个舵机发送数据，其内置单片机应该是用逐位解析的方法来收主机数据的，不过我们可以用DMA方法接受舵机数据。
3. usart配置示例如下：

//usart.h部分内容---------------------------------------------------------------------------

//dma开关
//#define USART1_Send_DMA	
#define USART1_Rec_DMA
//#define USART2_Send_DMA	
#define USART2_Rec_DMA

#define readOnly(x)	x->CR1 |= 4;	x->CR1 &= 0xFFFFFFF7;		//串口x配置为只读，CR1->RE=1, CR1->TE=0
#define sendOnly(x)	x->CR1 |= 8;	x->CR1 &= 0xFFFFFFFB;		//串口x配置为只写，CR1->RE=0, CR1->TE=1

//usart.c部分内容-------------------------------------------------------------------------
//串口2配置为半双工
void USART2_Half_Configuration(u32 buad)  
{  
  GPIO_InitTypeDef GPIO_InitStructure;  
  USART_InitTypeDef USART_InitStructure;   
	NVIC_InitTypeDef NVIC_InitStructure;
	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD,ENABLE); //使能GPIOA时钟
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE);//使能USART1时钟
	
	GPIO_PinAFConfig(GPIOD,GPIO_PinSource5,GPIO_AF_USART2); //GPIOA9复用为USART1
	
  GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
	//GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;  
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;  
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;  
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;  
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;  
  GPIO_Init(GPIOD, &GPIO_InitStructure);  

	
  USART_InitStructure.USART_BaudRate = buad;  
  USART_InitStructure.USART_WordLength = USART_WordLength_8b;  
  USART_InitStructure.USART_StopBits = USART_StopBits_1;  
  USART_InitStructure.USART_Parity = USART_Parity_No;  
  USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;  
  USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;  
  USART_Init(USART2,&USART_InitStructure);  

  USART_HalfDuplexCmd(USART2, ENABLE);  	//配置为半双工模式

	NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;//串口1中断通道
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3
	NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;		//子优先级3
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;			//IRQ通道使能
	NVIC_Init(&NVIC_InitStructure);	

	USART_ClearFlag(USART2, USART_FLAG_TC);
#ifndef USART2_Rec_DMA
	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);//开启相关中断
#else  
	USART_ITConfig(USART2, USART_IT_IDLE, ENABLE);//开启空闲中断
	USART_DMACmd(USART2, USART_DMAReq_Rx, ENABLE);//开启DMA接收
	USART_DMACmd(USART2, USART_DMAReq_Tx, ENABLE);//开启DMA接收
	
	//DMA for rx
	DMA_Config(DMA1_Stream5,DMA_Channel_4, 
						 (uint32_t)&(USART2->DR),
						 (uint32_t)USART2_Rx_DMA_Buffer,
						 DMA_DIR_PeripheralToMemory,  
						 USART2_RX_BUFFER_SIZE);
#endif

#ifdef USART2_Send_DMA
	//DMA for tx					 
	DMA_Config(DMA1_Stream6,DMA_Channel_4, 
						 (uint32_t)&(USART2->DR),
						 (uint32_t)USART2_Tx_DMA_Buffer,
						 DMA_DIR_MemoryToPeripheral,
						 USART2_RX_BUFFER_SIZE);
#endif	

  USART_Cmd(USART2,ENABLE);  
}  

//串口空闲中断部分-----------------------------------------------------------------------------
#ifdef USART2_Rec_DMA
long long errorCodeCnt_2[12];	//存储解码结果
void USART2_IRQHandler(void)                	
{
	u8 errorCode;
	uint8_t rc_tmp;
	uint16_t rc_len;
	
	USART_ClearITPendingBit(USART2,USART_IT_RXNE);
  if(USART_GetITStatus(USART2,USART_IT_IDLE)!=RESET)
  {
			rc_tmp=USART2->SR;	
			rc_tmp=USART2->DR;
			DMA_Cmd(DMA1_Stream5, DISABLE);
			DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_TCIF5);	// Clear Transfer Complete flag
			DMA_ClearITPendingBit(DMA1_Stream5, DMA_IT_TEIF5);	// Clear Transfer error flag	
			rc_len = USART2_RX_BUFFER_SIZE - DMA_GetCurrDataCounter(DMA1_Stream5);
	
			errorCode=Inst_Decoding_Handler(USART2_Rx_DMA_Buffer,rc_len,&serverData[2]);
			errorCodeCnt_2[errorCode]++;
		
			DMA_Enable(DMA1_Stream5,USART2_RX_BUFFER_SIZE); 
	}
	serverData[2].busy=0;
} 
#else
u8 cnt=0;
u8 rec[200];
void USART2_IRQHandler(void)
{
	if(USART_GetITStatus(USART2,USART_IT_RXNE))//串口非空标志位为1，收到数据
	{
		rec[cnt++] = USART_ReceiveData(USART2);//读取最新一个收到的数据
	}
}
#endif

//dma.c部分内容-----------------------------------------------------------------------------
//DMA_Streamx:DMA数据流,DMA1_Stream0~7/DMA2_Stream0~7
//chx:DMA通道选择,@ref DMA_channel DMA_Channel_0~DMA_Channel_7
//par:外设地址
//mar:存储器地址
//ndtr:数据传输量  
void DMA_Config(DMA_Stream_TypeDef *DMA_Streamx,uint32_t chx,uint32_t par,uint32_t mar,uint32_t dir,u16 ndtr)
{ 
 
	DMA_InitTypeDef  DMA_InitStructure;
	
	if((u32)DMA_Streamx>(u32)DMA2)//得到当前stream是属于DMA2还是DMA1
	{
	  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2,ENABLE);//DMA2时钟使能 
		
	}else 
	{
	  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1,ENABLE);//DMA1时钟使能 
	}
  DMA_DeInit(DMA_Streamx);
	
	while (DMA_GetCmdStatus(DMA_Streamx) != DISABLE){}//等待DMA可配置 
	
  /* 配置 DMA Stream */
  DMA_InitStructure.DMA_Channel 					= chx;  							//通道选择
  DMA_InitStructure.DMA_PeripheralBaseAddr 			= par;								//DMA外设地址
  DMA_InitStructure.DMA_Memory0BaseAddr 			= mar;								//DMA 存储器0地址
  DMA_InitStructure.DMA_DIR 					    = dir;								//direction of transmit.
  DMA_InitStructure.DMA_BufferSize 				    = ndtr;								//数据传输量 
  DMA_InitStructure.DMA_PeripheralInc				= DMA_PeripheralInc_Disable;		//外设非增量模式
  DMA_InitStructure.DMA_MemoryInc 					= DMA_MemoryInc_Enable;				//存储器增量模式
  DMA_InitStructure.DMA_PeripheralDataSize 		    = DMA_PeripheralDataSize_Byte;		//外设数据长度:8位
  DMA_InitStructure.DMA_MemoryDataSize 				= DMA_MemoryDataSize_Byte;			//存储器数据长度:8位
  DMA_InitStructure.DMA_Mode 						= DMA_Mode_Normal;					// 使用普通模式 
  DMA_InitStructure.DMA_Priority 					= DMA_Priority_High;				//中等优先级
  DMA_InitStructure.DMA_FIFOMode 					= DMA_FIFOMode_Disable;         
  DMA_InitStructure.DMA_FIFOThreshold 			    = DMA_FIFOThreshold_Full;
  DMA_InitStructure.DMA_MemoryBurst 				= DMA_MemoryBurst_Single;			//存储器突发单次传输
  DMA_InitStructure.DMA_PeripheralBurst 		    = DMA_PeripheralBurst_Single;		//外设突发单次传输
  DMA_Init(DMA_Streamx, &DMA_InitStructure);
  DMA_Cmd(DMA_Streamx,ENABLE);
} 

//开启一次DMA传输
void DMA_Enable(DMA_Stream_TypeDef *DMA_Streamx,u16 ndtr)
{
 
	DMA_Cmd(DMA_Streamx, DISABLE);                      //先关闭DMA,才能设置它
	
	while (DMA_GetCmdStatus(DMA_Streamx) != DISABLE){}	//等待传输结束
		
	DMA_SetCurrDataCounter(DMA_Streamx,ndtr);          //设置传输数据长度 
 
	DMA_Cmd(DMA_Streamx, ENABLE);                      //开启DMA
}	

三、指令帧编码和数据帧解码

• server.c提供舵机通信数据编码和解码方法
• 主要就是按照通信协议来处理数据

1. server.h

#ifndef __SERVER_H
#define __SERVER_H
#include "sys.h"
#include "queue.h"

//广播ID
#define BroadcastID 0xFE

//read指令的缓存长度
#define readBufMax	100

//运行模式（Operating Mode），控制表地址11
#define Velocity_Control_Mode 					1
#define Position_Control_Mode 					3
#define Extended_Position_Control_Mode			4
#define PWM_Control_Mode						16

//指令参数
#define Inst_Ping 			0x01		//Instruction that checks whether the Packet has arrived to a device with the same ID as Packet ID
#define Inst_Read 			0x02		//Instruction to read data from the Device
#define Inst_Write 			0x03		//Instruction to write data on the Device
#define Inst_RegWrite 		0x04		//Instruction that registers the Instruction Packet to a standby status; Packet is later executed through the Action command
#define Inst_Action			0x05		//Instruction that executes the Packet that was registered beforehand using Reg Write
#define Inst_Reset 			0x06 		//Instruction that resets the Control Table to its initial factory default settings
#define Inst_Reboot 		0x08		//Instruction to reboot the Device
#define Inst_Clear 			0x10		//Instruction to reset certain information
#define Inst_Status 		0x55		//Return Instruction for the Instruction Packet
#define Inst_SyncRead 		0x82		//For multiple devices, Instruction to read data from the same Address with the same length at once
#define Inst_SyncWrite 		0x83		//For multiple devices, Instruction to write data on the same Address with the same length at once
#define Inst_BulkRead 		0x92		//For multiple devices, Instruction to read data from different Addresses with different lengths at once
#define Inst_BulkWrite		0x93		//For multiple devices, Instruction to write data on different Addresses with different lengths at once

//指令长度
#define Inst_Ping_Len 			10		
#define Inst_Read_Len 			14	
//#define Inst_Write_Len;		//write指令长度不定，相关变量在serverData结构体里
//#define Inst_RegWrite_Len 	
#define Inst_Action_Len			10			
//#define Inst_Reset_Len 		//以下指令待补充	
//#define Inst_Reboot_Len 		
//#define Inst_Clear_Len 			
//#define Inst_Status_Len 		
//#define Inst_SyncRead_Len 	
//#define Inst_SyncWrite_Len 	
//#define Inst_BulkRead_Len 	
//#define Inst_BulkWrite_Len	

//解码成功标志
#define DC_Success 				0x00	
//发送帧错误标志（舵机进行判断，写入返回状态帧的Err位）
#define DC_ResultFail			0x01	//Failed to process the sent Instruction Packet
#define DC_InstructionError		0x02	//Undefined Instruction has been used || Action has been used without Reg Write
#define DC_CRCError				0x03	//发送数据的帧的CRC校验出错
#define DC_DataRangeError		0x04	//要写入相应地址的数据超出了最小/最大值的范围
#define DC_DataLenError			0x05	//尝试写入比相应地址的数据长度短的数据
#define DC_DataLimitError		0x06	//要写入相应地址的数据超出了限值
#define DC_AccessError			0x07	//尝试在只读或未定义的地址中写入值 || 尝试读取仅写入或未定义的地址中的值 || 在转矩启用状态(ROM锁)时，尝试在ROM域中写入值
//接受帧错误标志（解码时进行判断）
#define DC_MyHeadError 			0x08	//接受数据帧的数据头出错
#define DC_MyLenError			0x09	//接受数据帧的长度出错
#define DC_MyIDError			0x0A	//接受数据帧的ID出错
#define DC_MyCRCError			0x0B	//接受数据帧的CRC校验出错			
		
typedef struct EEPROM
{
	u8 Model_Number[2];			//0
	u8 Model_Information[4];	//2
	u8 Firmware_Version;		//6
	u8 ID;						//7
	u8 Baud_Rate;				//8
	u8 Return_Delay_Time;		//9
	u8 Drive_Mode;				//10
	u8 Operating_Mode;			//11
	u8 Secondary_ID;			//12
	u8 Protocol_Type;			//13
	u8 RSRV1[6];				//14	保留段1
	u8 Homing_Offset[4];		//20
	u8 Moving_Threshold[4];		//24
	u8 RSRV2[3];				//28	保留段2
	u8 Temperature_Limit;		//31
	u8 Max_Voltage_Limit[2];	//32
	u8 Min_Voltage_Limit[2];	//34
	u8 PWM_Limit[2];			//36
	u8 RSRV3[6];				//38	保留段3
	u8 Velocity_Limit[4];		//44
	u8 Max_Position_Limit[4];	//48
	u8 Min_Position_Limit[4];	//52
	u8 RSRV4[7];				//56	保留段4
	u8 Shutdown;				//63
}EEPROM;

typedef struct RAM
{
	u8 Torque_Enable;			//64
	u8 LED;						//65
	u8 RSRV1[2];				//66 保留段1
	u8 Status_Return_Level;		//68
	u8 Registered_Instruction;	//69
	u8 Hardware_Error_Status;	//70
	u8 RSRV2[5];				//71 保留段2
	u8 Velocity_I_Gain[2];		//76
	u8 Velocity_P_Gain[2];		//78
	u8 Position_D_Gain[2];		//80
	u8 Position_I_Gain[2];		//82
	u8 Position_P_Gain[2];		//84
	u8 RSRV3[2];				//86 保留段3
	u8 Feedforward_2nd_Gain[2];	//88
	u8 Feedforward_1st_Gain[2];	//90
	u8 RSRV4[7];				//91 保留段4
	u8 Bus_Watchdog;			//98
	u8 Goal_PWM[2];				//100
	u8 RSRV5[2];				//102 保留段5
	u8 Goal_Velocity[4];		//104
	u8 Profile_Acceleration[4];	//108
	u8 Profile_Velocity[4];		//112
	u8 Goal_Position[4];		//116
	u8 Realtime_Tick[2];		//120		
	u8 Moving;					//122
	u8 Moving_Status;			//123
	u8 Present_PWM[2];			//124
	u8 Present_Load[2];			//126
	u8 Present_Velocity[4];		//128
	u8 Present_Position[4];		//132
	u8 Velocity_Trajectory[4];	//136
	u8 Position_Trajectory[4];	//140
	u8 Present_Input_Voltage[2];//144
	u8 Present_Temperature;		//146
}RAM;

//舵机控制表
typedef struct CONTROL_TABLE
{
	EEPROM rom;
	RAM ram;
}CONTROL_TABLE;

//read指令参数包
typedef struct RWINST
{
	u16 RWAddr;			//最近一次读写控制表的起始地址
	u16 RWLen;			//最近一次读写控制表的长度
	u16 frameLen;		//指令帧的长度
}RWINST;


//ping指令接受数据包
typedef struct PINGINFO
{
	u16 ModelNum;
	u8 FirmwareVersion;
}PINGINFO;


//舵机数据
typedef struct SERVERDATA
{
	u8 inst;							//最近一次发送的指令
	u8 ID;								//串口连接舵机的ID
	u8 recLen;							//最近一次接受的指令长度
	u8 IDInit;							//初始化ID标志
	u8 *writeBuf;						//写入数据buf的指针
	u8 busy;							//正在收发数据
	u8 lock;							//锁，lock=1时不能从缓冲队列取指令
	
	USART_TypeDef* USARTx;	//串口号
	RRQUEUE *rrQueue;					//舵机对应的指令缓冲队列
	
	RWINST RWInstPara;					//读写指令参数包
	PINGINFO PingInfo;					//ping接受数据包

	CONTROL_TABLE ctrlTable;			//控制表
}SERVERDATA;


extern u8 instBuf[INSTBUFLEN];
extern SERVERDATA serverData[5];
extern u8 write2CtrlTable[5][100];	//写入控制表的数据缓冲(注意低字节在前)

u16 dataAssmeble(u8 data_L,u8 data_H);
void dataSplit(u16 data,u8 *data_L,u8 *data_H);
u32 dataAssmeble_32(u8 *buf);
void dataSplit_32(u32 data,u8 *buf);

void serverDataInit(void);


void readPara(SERVERDATA *sd,u16 addr,u16 len);
void writePara(SERVERDATA *sd,u16 addr,u16 len,u8 *paraBuf);

void PingCode(SERVERDATA *sd);
u8 Inst_Decoding_Handler(u8 *buf,u8 reclen,SERVERDATA *sd);
void Inst_Sending_Handler(u8 inst,SERVERDATA *sd);
void deQueue(SERVERDATA *sd,u8 *buf,u8 len);

#endif

2. server.c

#include "server.h"
#include "crc.h"			//舵机厂商提供的crc函数
#include "usart.h"

SERVERDATA serverData[5];	//舵机基本信息,下标是串口号(0号不用)
u8 instBuf[INSTBUFLEN];		//指令帧Buf

u8 write2CtrlTable[5][100];	//写入控制表的数据buf(注意低字节在前)

/*------------------------------------------------数据处理函数-----------------------------------------------------*/
//两个8位数合成一个16位数
u16 dataAssmeble(u8 data_L,u8 data_H)
{
	u16 temp=(data_H<<8)|data_L;
	return temp;
}

//一个16位数拆两个8位
void dataSplit(u16 data,u8 *data_L,u8 *data_H)
{
	*data_L = data & 0xFF;
	*data_H = (data & 0xFF00)>>8;
}

//一个32位数据拆四个8位（buf首存最低位）
void dataSplit_32(u32 data,u8 *buf)
{
	u32 temp=0xFF;
	for(int i=0;i<4;i++)
		buf[i]=(data & temp<<8*i)>>8*i;
}

//四个8位数据拼一个32位（buf首存最低位）
u32 dataAssmeble_32(u8 *buf)
{
	u32 temp=buf[3];
	for(int i=2;i>=0;i--)
		temp=(temp<<8)|buf[i];
	
	return temp;
}

//清inst数组
void clearInstBuf()
{
	for(int i=0;i<INSTBUFLEN;i++)
		instBuf[i]=0;
}	
/*------------------------------------------------辅助函数-----------------------------------------------------*/
//舵机结构体初始化
void serverDataInit()
{
	initQueue();							//初始化指令缓冲队列
	
	for(int i=1;i<=4;i++)
	{
		serverData[i].IDInit=1;	//准备进行id设置
		serverData[i].writeBuf=write2CtrlTable[i];	
		serverData[i].busy=0;
		serverData[i].rrQueue = &queue[i];
	}
		
	serverData[1].USARTx=USART1;
	serverData[2].USARTx=USART2;
	serverData[3].USARTx=USART3;
	serverData[4].USARTx=UART4;
	
	//发送广播指令，初始化ID(这里的串口2必须接上舵机)
	serverData[2].ID=BroadcastID;
	sendOnly(USART2);
	PingCode(&serverData[2]);
	sendBuf(USART2,instBuf,10);
	readOnly(USART2);	
}

//设置帧头和保留位
void setHead()
{
	//帧头
	instBuf[0]=0xFF;
	instBuf[1]=0xFF;
	instBuf[2]=0xFD;
	instBuf[3]=0x00;
}

//帧头和保留位检查
u8 cheakHead(u8 *buf)
{
	if(buf[0]!=0xFF || buf[1]!=0xFF || buf[2]!=0xFD || buf[3]!=0)
		return 0;
	return 1;
}

//帧长度检查
u8 cheakLen(u8 *buf,u8 len)
{
	u16 L=dataAssmeble(buf[5],buf[6]);
	if(len!=L)
		return 0;
	return 1;
}

//CRC检查
u8 cheakCRC(u8 *buf,u8 size) 
{
	u16 crc=update_crc(0,buf,size);
	if(buf[size]!=(crc&0xFF)	|| buf[size+1]!=((crc>>8)&0xFF))
			return 0;
	return 1;
}

/*------------------------------------------------参数准备-----------------------------------------------------*/
//设置读控制表的参数
void readPara(SERVERDATA *sd,u16 addr,u16 len)
{
	sd->RWInstPara.RWAddr=addr;
	sd->RWInstPara.RWLen=len;
}

//设置写控制表参数
void writePara(SERVERDATA *sd,u16 addr,u16 len,u8 *paraBuf)
{
	sd->RWInstPara.RWAddr=addr;
	sd->RWInstPara.RWLen=len;
	copyBuf(paraBuf,sd->writeBuf,len);
}

/*-----------------------------------------------编码发送帧-----------------------------------------------------*/

//ping指令：要求指定ID的设备发回数据包，如果ID设为BroadcastID（0xFE），所有链接的设备根据其排列顺序发送状态包
void PingCode(SERVERDATA *sd)
{
	setHead();				//帧头
	instBuf[4]=sd->ID;		//ID
	instBuf[5]=0x03;		//LEN_L
	instBuf[6]=0x00;		//LEN_H
	instBuf[7]=Inst_Ping;	//Inst	
	setCrc(instBuf,8);		//CRC
	
	sd->RWInstPara.frameLen=Inst_Ping_Len;	//发送帧长度
}

//Read指令：从控制表RWAddr地址开始读RWLen长字节（地址<64是EEPROM区，否则是RAM区）
void ReadCode(SERVERDATA *sd)
{
	setHead();				//帧头
	instBuf[4]=sd->ID;		//ID
	instBuf[5]=0x07;		//LEN_L
	instBuf[6]=0x00;		//LEN_H
	instBuf[7]=Inst_Read;	//Inst
	dataSplit(sd->RWInstPara.RWAddr,&instBuf[8],&instBuf[9]);	//para_addr
	dataSplit(sd->RWInstPara.RWLen,&instBuf[10],&instBuf[11]);	//para_len
	setCrc(instBuf,12);		//CRC
	
	sd->RWInstPara.frameLen=Inst_Read_Len;	//发送帧长度
}

//write/reg write指令：立即向控制表RWAddr写入writeBuf中前RWLen长字节
void WriteCode(SERVERDATA *sd,u8 regFlag)
{
	setHead();							//帧头
	instBuf[4]=sd->ID;					//ID
	u16 len=sd->RWInstPara.RWLen+2+3;	//LEN
	dataSplit(len,&instBuf[5],&instBuf[6]);	
	
	if(regFlag)
		instBuf[7]=Inst_RegWrite;		//Inst
	else
		instBuf[7]=Inst_Write;						
	
	dataSplit(sd->RWInstPara.RWAddr,&instBuf[8],&instBuf[9]);	//para_addr
			
	copyBuf(sd->writeBuf,&instBuf[10],sd->RWInstPara.RWLen);	//写入控制表的数据	
	setCrc(instBuf,len+5);				//CRC
	
	sd->RWInstPara.frameLen=len+7;		//发送帧长度
}

//action指令，执行regWrite注册的指令
void ActionCode(SERVERDATA *sd)
{
	setHead();						//帧头
	instBuf[4]=sd->ID;				//ID
	instBuf[5]=0x03;				//len
	instBuf[6]=0x00;
	instBuf[7]=Inst_Action;			//inst
	setCrc(instBuf,8);				//CRC
	
	sd->RWInstPara.frameLen=Inst_Action_Len;	//发送帧长度
}

//发送处理函数
void Inst_Sending_Handler(u8 inst,SERVERDATA *sd)
{
		sd->inst=inst;
		
		switch(inst)
		{
			case 	Inst_Ping:			PingCode(sd);				break;	
														
			case 	Inst_Read:			ReadCode(sd);				break;

			case 	Inst_Write:			WriteCode(sd,0);			break;

			case 	Inst_RegWrite:		WriteCode(sd,1);			break;
			
			case 	Inst_Action:		ActionCode(sd);				break;
			case 	Inst_Reset:				break;					//以下指令待补充
			case 	Inst_Reboot:			break;
			case 	Inst_Clear:				break;
			case 	Inst_Status:			break;
			case 	Inst_SyncRead:			break;
			case 	Inst_SyncWrite:			break;
			case 	Inst_BulkRead:			break;
			case 	Inst_BulkWrite:			break;
		}
	
		enQueue(sd->rrQueue,instBuf,sd->RWInstPara.frameLen);	//加入指令帧循环队列
}

/*-----------------------------------------------解码接受帧-----------------------------------------------------*/
//ping
void PindDecode(u8 *buf,SERVERDATA *sd)
{
	sd->PingInfo.ModelNum=dataAssmeble(buf[9],buf[10]);
	sd->PingInfo.FirmwareVersion=buf[11];
}

//read
void ReadDecode(u8 *buf,SERVERDATA *sd)
{
	u16 len=dataAssmeble(buf[5],buf[6]);
	u8 *p=(u8 *)(&sd->ctrlTable)+sd->RWInstPara.RWAddr;
	
	for(int i=0;i<len-4;i++)
		p[i]=buf[9+i];
}

//write
void WriteDecode()
{
	;	//write返回的指令包没有数据，留个接口
}

//reg write
void RegWriteDecode()
{
	;	//red write返回的指令包没有数据，留个接口
}

//action
void ActionDecode()
{
	;	//action返回的指令包没有数据，留个接口
}

//解码处理函数
u8 Inst_Decoding_Handler(u8 *buf,u8 reclen,SERVERDATA *sd)
{
	if(!cheakHead(buf))			//检测帧头
		return DC_MyHeadError;
		
	if(!cheakLen(buf,reclen-7))	//检查帧长度
		return DC_MyLenError;

	if(!cheakCRC(buf,reclen-2))	//检查CRC
		return DC_MyCRCError;	
	
	if(buf[8]!=0)				//检查发送数据错误
		return buf[8];
	
	//ID初始化
	if(sd->IDInit)
	{
		sd->ID=buf[4];
		sd->IDInit=0;
		return DC_Success;
	}
	//正常通信
	else
	{
		if(buf[4]!=sd->ID)				//检查ID
			return DC_MyIDError;
			
		switch(sd->inst)
		{
			case 	Inst_Ping:			PindDecode(buf,sd);		break;	
			case 	Inst_Read:			ReadDecode(buf,sd);		break;
			case 	Inst_Write:			WriteDecode();			break;
			case 	Inst_RegWrite:		RegWriteDecode();		break;
			case 	Inst_Action:		ActionDecode();			break;
			case 	Inst_Reset:				break;				//以下指令待补充
			case 	Inst_Reboot:			break;
			case 	Inst_Clear:				break;
			case 	Inst_Status:			break;
			case 	Inst_SyncRead:			break;
			case 	Inst_SyncWrite:			break;
			case 	Inst_BulkRead:			break;
			case 	Inst_BulkWrite:			break;
		}
	
		return DC_Success;
	}
}

四、指令循环队列

1. 在测试中，我发现每一条指令从主机发送指令帧到主机接受到状态帧耗时挺长的，如果还没收到上一条指令返回数据就发送下一条指令，有可能出现问题。因此我把通信做成了阻塞式的。必须收到前一条指令的返回数据，才能发送下一条指令。
2. 为此我设置了一个循环队列。每一条指令帧编码完成后，先存入循环队列（上一部分在Inst_Sending_Handler函数最后就是指令入队位置），设置一个定时器中断循环检测循环队列和舵机通信情况，当条件满足时出队并发送指令
3. queue.h定义了循环队列的数据结构

#ifndef __QUEUE_H
#define __QUEUE_H

#include "sys.h"

#define queueLen 10 		//队列长度
#define INSTBUFLEN 30		//发送帧缓Buf长度	

//一条指令（队列元素）
typedef struct QINST 
{
	u8 buf[INSTBUFLEN];		//存储整个指令帧
	u16 readAddr;					//读指令的附加信息
	u16 readLen;
	u8 instLen;						//指令长度
}QINST;

//一个循环队列
typedef struct RRQUEUE
{
	u8 head;
	u8 tail;
	QINST qinst[queueLen];
}RRQUEUE;
	
extern RRQUEUE queue[5];

void copyBuf(u8 *from,u8 *to,u8 len);
void initQueue(void);
void enQueue(RRQUEUE *q,u8 *buf,u8 len);

u8 queueEmpty(RRQUEUE *q);

#endif 

4. queue.c提供了循环队列操作

/*
	指令缓冲循环队列
		keil对多维数组指针支持好像不太好，这里用结构体
		
	阻塞式通信说明：
		在舵机结构体SERVERDATA中有一个busy元素，在发送命令帧时置1，在解码状态帧时清0
		发送指令时，指令编码为指令帧后存入队列（如果是read指令，注意addr和len也要存）
		tim3定时器中断以10ms周期检查所有舵机的busy标志和指令循环队列，如果队列非空且busy=0，出队一个指令发送
*/
#include "queue.h"
#include "server.h"

RRQUEUE queue[5];	//指令缓冲队列

//复制数组
void copyBuf(u8 *from,u8 *to,u8 len)
{
	for(int i=0;i<len;i++)
		to[i]=from[i];
}

//初始化队列
void initQueue()
{	
	for(int i=1;i<=4;i++)
		queue[i].head=queue[i].tail=0;
}

//入队一条指令
void enQueue(RRQUEUE *q,u8 *buf,u8 len)
{
	u8 tailTemp=q->tail;
	
	tailTemp++;
	if(tailTemp == queueLen)
		tailTemp=0;
	
	if(tailTemp == q->head)	//满了,不能入队
		return;
	
	copyBuf(buf,q->qinst[q->tail].buf,len);	//复制指令帧
	
	switch(buf[7])
	{
		case Inst_Read:	
				q->qinst[q->tail].readAddr = dataAssmeble(buf[8],buf[9]);
				q->qinst[q->tail].readLen = dataAssmeble(buf[10],buf[11]);	break;
	}
	
	q->qinst[q->tail].instLen = len;				//记录帧长度						
	q->tail=tailTemp;
}

//出队一条指令
void deQueue(SERVERDATA *sd,u8 *buf,u8 len)
{
	RRQUEUE *q = sd->rrQueue;
	copyBuf(q->qinst[q->head].buf,buf,len);
	
	u8 headTemp=q->head;
	headTemp++;
	if(headTemp==queueLen)
		headTemp=0;
	
	q->head=headTemp;
	
	switch(buf[7])
	{
		case Inst_Read:		//如果是read指令，更新RWAddr和RWLen(否则解码时会出错)
				sd->RWInstPara.RWAddr = dataAssmeble(buf[8],buf[9]);
				sd->RWInstPara.RWLen = dataAssmeble(buf[10],buf[11]);	break;
	}
}

//判空
u8 queueEmpty(RRQUEUE *q)
{
	u8 res = q->head == q->tail ? 1:0;
	return res;
}

5. 定时器中断

void TIM3_IRQHandler(void)//10ms
{
	if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) //溢出中断
	{		
		for(int i=1;i<=4;i++)
		{
			//如果舵机not busy && not lock && 指令缓冲队列非空，出队一个指令发送
			if(!serverData[i].busy && !serverData[i].lock && !queueEmpty(serverData[i].rrQueue))
			{
				u8 headPos = serverData[i].rrQueue->head;
				len = serverData[i].rrQueue->qinst[headPos].instLen;
				deQueue(&serverData[i],instTemp,len);
				
				serverData[i].busy=1;
				USART_TypeDef* USARTx = serverData[i].USARTx;
				sendOnly(USARTx);	
				sendBuf(USARTx,instTemp,len);
				readOnly(USARTx);
			}
		}
	}
	TIM_ClearITPendingBit(TIM3,TIM_IT_Update);  //清除中断标志位
}

五、完整代码下载

下载完整代码