一、TS-DRIVE-NODE介绍
1. 简介
TS-DRIVE-NODE是一个面向多自由度机器人伺服通讯协议转换的模块，其兼容了市面上数款常用关节减速器电机，完成对其的位置控制和扭矩驱动，并将纷繁复杂的通讯协议转换为一个标准的CAN通讯链路，通过配套的上位机能快速完成电机类型的选择，最高支持3路电机的介入，目前仅支持统一类型电机的协议驱动，后续不断升级实现任意类型电机的组合使用。TS-DRIVE-NODE具有CAN和485两个硬件通讯链路，预留一路TTL串口实现传感器采集，兼容MIT、老虎、达妙、宇树电机，是DIY四足机器人或双足机器人的不二选择，TS-DRIVE-NODE功能接口如下图示：
TS-DRIVE-NODE采用CAN完成与主站的通讯，可以采用标准协议驱动不同的电机，无缝适配UDroid-C4通用控制单元，TS-DRIVE-NODE模块具体性能参数如下表所示：
2. 硬件引脚分配
TS-DRIVE-NODE主要接口如下图所示，采用XH2.0端子完成与主站的通讯，PH1.0端子完成与CAN协议电机通讯，并预留一路接地保证CAN通讯可靠；预留高速485完成与宇树电机的通讯；预留TTL串口可以完成对压力、距离或LED模块的驱动；预留USB实现上位机快速模式配置；预留5路DC分电实现1路供电为3个电机分电供电的目的：
TS-Node二合一版本IO定义
TS-DRIVE-NODE以上图的视角为例，各端子IO定义如下，使用时请查看丝印确定连线，特别是供电是否正确。
注：电路板采用CAN主站接口5V进行供电，电路板闭源固件通过STlink转接板由USB或SWD进行更新，DC口仅实现对电机供电的分电功能，拨码开关可以选择电机端CAN总线是否有120欧电阻，部分CAN通讯电机需要将接地端与电机GND端连接！
TS-DRIVE-NODE同时可以介入3路CAN协议电机或3路485协议电机，具体连线如下图所示：
3. SWD下载与固件更新
如上图所示，我们提供了Keil下的开发Demo，可以使用SWD转接模块连接主控使用KEIL5 IDE进行单片机开发，同时可用USB端口连接OCU实现参数在线监视和配置，对下载器和USB的连续需要使用对应的转接板或者可以自行按引脚定义连线：
对于项目中一些不开源的软件，我们推荐使用USB口对固件进行更新，首先需要将Hex固件文件转换为生成DFU文件打开Dfu file manager软件，默认选择生成DFU文件，点击OK。选择文件，我们这里选择S19 or Hex并在工程目录找到工程生成的Hex文件打开。
然后点击生成文件，选择存放目录及文件名，再确定保存。提示生成成功，确定，到这里DFU文件就生成好了。打开DfuSe Demo软件，按住载板控制器上端子⑩上方的Boot按键，在无外接供电的条件下插上USB线连上电脑就能识别出DFU设备了，如下图：
选择文件，在弹出窗口中找到之前软件生成的DFU文件或者下载的DFU文件，选择并打开。双击Target Id中的STM32 Flash一栏，点击Choose选择刚才生成到的Dfu文件后，点击Upgrade进行下载，当读条完毕无错误报出则可以连接OCU上位机查看固件版本。
注：部分USB驱动无法安装成功，可通过通过SWD口下载固件！
目前控制器固件需要通过STlink下载，首先使用转接线与STlink的SWD接口对接并从下载口提供5V供电，之后使用STM32 ST-LINK Utility 下载固件。首先连接单片机，正确连接后显示
之后选择需要下载的固件，点击File选择Open File功能，找到对应版本hex固件确认：
选择Target点击Program and Vertify下载固件：
点击Start开始下载当读条完毕，并显示如下说明则固件下载完成：
备注（USB转接模块的连接）：
下载器配套两个SWD下载转接模块，下载时连接主控SWD下载口：
下载程序可以直接使用STlink连接小转接板上排针引脚，另外可以通过Micro USB口连接延长线到转接模块：
4. TS-DRIVE-NODE上位机配置与使用教程
4.1 电机适配能力
在使用TS-DRIVE-NODE前首先需要了解其兼容的电机类型，目前模块主要兼容MIT协议的电机如老虎电机、达妙电机同时也兼容宇树的电机，具体明细如下表所示，注部分电机随固件版本更新协议出现失效如使用中出现问题请及时联系我们进行修正：
4.2 上位机配置
如果你使用了UDroid-C4主控则只需要连接好线，采用我们提供的软件就可以实现在Linux系统中操作电机的位置和扭矩，因此这里首先以此为例进行配置的介绍。
上位机下载解压后需要将图片文件夹放置在D盘更目录：
在第一次使用时首先断开模块与主控的CAN通讯线，使用USB转接线连接电路板打开上位机，请选择正确的串口（部分台式机可能有其他串口设备）：
点击执行器ICON，进入电机配置界面，开始电机配置：
1）首次配置时先选择电机的下拉菜单找到自己对应的电机类型（目前版本仅支持3路电机采用相同的电机类型），具体对应关系请查看上面的适配列表；
2）在将电机都完成配置后，电机红色保存参数，插拔USB打开上位机确认电机类型是否正确保存在Flash中：
3）配置模块ID，修改框里的ID，并点击保存，对于采用UDroid-C4主控则NODE的ID为0~3：
4）如电机通讯正常可以点击蓝色ICON标定电机零位；
5）对于目前兼容的关节电机可以修改整体刚度为：
6）点击使能，则电机将会出现反馈电流数据：
7）此时如果拖动电流托条，则电机进入电流开环模式，对于非DGM-FOC或ODrive的关节电机这里设定的是输出轴扭矩，单位为Nm，同时电流反馈也是输出轴扭矩：
同时电流限制也将变成扭矩的限制：
注：电流模式下如电机转速过快会进入故障模式，重新标定0位后即可复位故障！
如拖动位置托条，则电机进入位置模式，可以调整刚度提高闭环效果：
8）通过选择不同的轨迹模式可以测试电机的运动效果：
9）电机状态下发的勾选键可以实现ID1和ID2电机通道的快速调换：
同时勾选转向可以完成电机0~180度正方向的反向，T命令则可以改变当前电机控制的符号，T测量可以改变当前电机反馈数据的正负号，通常情况下命令和测量的符号应该一致，对于上述电机如果勾选了转向反向则一般情况下需要勾选命令反向保证测量和指令的同号！
注：在操作转向反向指令时最好先关闭电机使能！
10）综上，采用OCU上位机可以快速完成对电机转向和控制指令反馈符号的调节，在确认后需要点击保存，而电流限幅和标0角度等参数重新上电后复位，在正常使用中是通过CAN主站发送的配置进行修改；
注：上位机中所有输入框均不可以输入英文或符号，仅可以输入数字或小数点数字，如输入错误后面输入的数据不会生效，需要返回上一级菜单重新载入界面！
5. TS-DRIVE-NODE通讯协议
TS-DRIVE-NODE主站的通讯采用了2MBps CAN超频配置，如果是STM32F4其相应的寄存器配置参数如下，要使用CAN通讯驱动节点首先保证已经完成OCU中的配置并且电机工作正常：
定义如下的宏定义参数:
#define CAN_T_DIV 500.0
#define CAN_I_DIV 100.0
#define CAN_F_DIV 100.0
#define CAN_POS_DIV 50.0
#define CAN_DPOS_DIV 20.0
#define CAN_GAIN_DIV_P 500.0
#define CAN_GAIN_DIV_I 10000.0
#define CAN_GAIN_DIV_D 1000.0
#define CAN_SD_PARAM_SYS_HEAD 0
#define CAN_SD_PARAM_SYS_HEAD_DIV 10
#define CAN_SD_PARAM_T_HEAD 20
#define CAN_SD_PARAM_T_HEAD_GAIN 40
#define CAN_SD_PARAM_T_HEAD_SW 50
#define CAN_SD_PARAM_T_HEAD_ST 60
#define CAN_FB_STATE_HEAD 70
#define CAN_FB_POS_HEAD 80
#define CAN_FB_SPD_HEAD 90
#define CAN_FB_MIT_HEAD 100
#define CAN_SD_T_HEAD 120
#define CAN_SD_I_HEAD 140
#define CAN_SD_POS_HEAD 160
#define CAN_SD_SPD_HEAD 180
#define CAN_SD_MIT_HEAD 200
#define CAN_SD_MIT_HEAD_Q 210
#define CAN_SD_MIT_HEAD_T 220
#define CAN_SD_PARAM_T_HEAD_SW_D1 300
#define CAN_SD_PARAM_T_HEAD_SW_D2 310
#define CAN_SD_PARAM_T_HEAD_SW_D3 320
#define CAN_SD_PARAM_T_HEAD_ST_D1 330
#define CAN_SD_PARAM_T_HEAD_ST_D2 340
#define CAN_SD_PARAM_T_HEAD_ST_D3 350
#define CAN_FB_REMOTE_FB1 400
#define CAN_FB_REMOTE_FB2 401
#define CAN_FB_REMOTE_FB1_Q3 402
#define CAN_FB_REMOTE_FB1_I3 403
#define CAN_FB_REMOTE_RST_Q 412
#define CAN_FB_REMOTE_RST_ERR 413
#define CAN_FB_POS3_HEAD 430
#define CAN_FB_I3_HEAD 440
#define CAN_FB_STATE_HEAD_Q3 4505.1 反馈数据的获取
1）反馈三个电机的角度值：
标准帧，帧头：CAN_FB_POS3_HEAD+节点ID;
帧长度：8
说明：上述角度值首先单位是°，然后在上传时放大了CAN_POS_DIV，以STM32为例其数据读取代码如下，另外[0]和[1]的角度通道顺序会受到上位机电机ID交互功能的影响：
q_now[0]=(float)((int16_t)(*(canbufr1+0)<<8)|*(canbufr1+1))/CAN_POS_DIV;
q_now[1]=(float)((int16_t)(*(canbufr1+2)<<8)|*(canbufr1+3))/CAN_POS_DIV;
q_now[2]=(float)((int16_t)(*(canbufr1+4)<<8)|*(canbufr1+5))/CAN_POS_DIV;另外，节点板默认采用应答遥控帧的方式反馈数据，因此主站需要主动发送如下帧，保证伺服控制的高同步性，发送该帧后总线上所有节点模块都会反馈自己内部电机角度值：
标准帧，帧头：CAN_FB_REMOTE_FB1_Q3;
帧长度：0
CAN1_Send_Msg_Board(canbuft1,0,CAN_FB_REMOTE_FB1_Q3);
2）反馈三个电机的扭矩：
标准帧，帧头：CAN_FB_REMOTE_FB1_I3+节点ID;
帧长度：8
说明：对于MIT等电机上述参数的单位是Nm，然后在上传时放大了CAN_T_DIV，以STM32为例其数据读取代码如下，另外[0]和[1]的角度通道顺序会受到上位机电机ID交互功能的影响：
t_now[0]=(float)((int16_t)(*(canbufr1+0)<<8)|*(canbufr1+1))/CAN_T_DIV;
t_now[1]=(float)((int16_t)(*(canbufr1+2)<<8)|*(canbufr1+3))/CAN_T_DIV;
t_now[2]=(float)((int16_t)(*(canbufr1+4)<<8)|*(canbufr1+5))/CAN_T_DIV;另外，节点板默认同样采用应答遥控帧的方式反馈数据，因此主站需要主动发送如下帧，保证伺服控制的高同步性，发送该帧后总线上所有节点模块都会反馈自己内部电机扭矩反馈：
标准帧，帧头：CAN_FB_REMOTE_FB1_I3;
帧长度：0
CAN1_Send_Msg_Board(canbuft1,0,CAN_FB_REMOTE_FB1_I3);
3）反馈三个电机的状态：
标准帧，帧头：CAN_FB_STATE_HEAD_Q3+节点ID;
帧长度：8
说明：上述协议以0.1s的周期自动上传，首先反馈了各个电机是否连接、是否准备好、是否故障，另外[0]和[1]的通道顺序会受到上位机电机ID交互功能的影响，STM32下的读取方法如下：
int ge,shi,bai;
char temp;
temp=*(canbufr1+0);
bai = (int)temp/100%10;
shi = (int)temp/10%10;
ge = (int)temp%10;
connect_motor[0]=ge;
ready[0]=shi;
err_flag[0]=bai;
temp=*(canbufr1+1);
bai = (int)temp/100%10;
shi = (int)temp/10%10;
ge = (int)temp%10;
connect_motor[1]=ge;
ready[1]=shi;
err_flag[1]=bai;
temp=*(canbufr1+2);
bai = (int)temp/100%10;
shi = (int)temp/10%10;
ge = (int)temp%10;
connect_motor[2]=ge;
ready[2]=shi;
err_flag[2]=bai;
bat_v[0]=*(canbufr1+3);5.1 机的配置
1）设置三个电机的摆动参数：
为了方便四足机器人控制中摆动和支撑参数快速切换，NODE采用了一个标志位选择使用的参数，则在驱动电机前需要依次发送三个电机的摆动PD参数，以电机通道0的摆动参数设置为例，其协议为：
标准帧，帧头：CAN_SD_PARAM_T_HEAD_SW_D1+节点ID;
帧长度：8
说明：上述协议发送一次就刷新节点内部参数，首先扭矩系数即扭矩转化电流的比例，对于MIT等电机值为1，KP（0~100）和KD（0~10）为电机PD参数，扭矩限制为电机最大扭矩输出限幅，最后一位为电机模式控制，则以STM32为例发送函数为：
void CAN_set_torque_param_d(char node_id,char bldc_sel){
char res=0,i;
vs16 _temp;
u8 canbuft1[8]={0};
float kp=0,kd=0;
uint32 can_header=CAN_SD_PARAM_T_HEAD_SW_D1;
if(bldc_sel==0)
can_header=CAN_SD_PARAM_T_HEAD_SW_D1;
else if(bldc_sel==1)
can_header=CAN_SD_PARAM_T_HEAD_SW_D2;
else
can_header=CAN_SD_PARAM_T_HEAD_SW_D3;
kp=kp_d[bldc_sel];
kd=kd_d[bldc_sel];
_temp=leg_motor[id].t_to_i[0]*CAN_T_DIV;
canbuft1[0]=BYTE1(_temp);
canbuft1[1]=BYTE0(_temp);
_temp=kp*CAN_GAIN_DIV_P;
canbuft1[2]=BYTE1(_temp);
canbuft1[3]=BYTE0(_temp);
_temp=kd*CAN_GAIN_DIV_D;
canbuft1[4]=BYTE1(_temp);
canbuft1[5]=BYTE0(_temp);
canbuft1[6]=max_i[0];
canbuft1[7]=motor_en*100+reset_q*10+reset_err;
for(i=0;i<8;i++)
canbuft1_1[id][i]=canbuft1[i];
res=CAN1_Send_Msg(canbuft1,8,can_header+node_id);
}同理，对于支撑参数只需要将帧头换为CAN_SD_PARAM_T_HEAD_ST_D*即可！
2）设置三个电机的标0角度参数：
对于没有输出轴记忆的电机，每次使用前都需要标定0位，而0位角度需要主动触发的形式进行标定，即发送触发触发信号后节点会将电机当前的角度复位为本协议发送的数据，其协议为：
标准帧，帧头：CAN_SD_PARAM_SYS_HEAD_DIV+节点ID;
帧长度：8
说明：上述[0]和[1]通道顺序受OCU电机交换功能影响，零位角度发送时的单位为°，并且发送时数据扩大了CAN_POS_DIV，则以STM32为例发送函数为：
void CAN_set_zero_off(char node_id){
char res=0,i=0;
int16_t _temp;
uint8_t canbuft1[8]={0};
_temp=q_reset[0]*CAN_POS_DIV;
canbuft1[0]=BYTE1(_temp);
canbuft1[1]=BYTE0(_temp);
_temp=q_reset[1]*CAN_POS_DIV;
canbuft1[2]=BYTE1(_temp);
canbuft1[3]=BYTE0(_temp);
_temp=q_reset[2]*CAN_POS_DIV;
canbuft1[4]=BYTE1(_temp);
canbuft1[5]=BYTE0(_temp);
canbuft1[6]=max_i[0];
canbuft1[7]=motor_en*100+reset_q*10+reset_err;
res=CAN1_Send_Msg(canbuft1,8,CAN_SD_PARAM_SYS_HEAD_DIV+node_id);
}5.3 电机控制帧
1）设置三个电机的输出轴角度：
在完成标定后，既可以使能电机motor_en=1，并发送电机的期望角度，其协议为：
标准帧，帧头：CAN_SD_MIT_HEAD_Q+节点ID;
帧长度：8
说明：上述[0]和[1]通道顺序受OCU电机交换功能影响，期望角度为输出轴，角度发送时的单位为°，并且发送时数据扩大了CAN_POS_DIV，则以STM32为例发送函数为：
void CAN_set_pos_force_mit_div_q(char id){
char res=0;
int16_t _temp;
uint8_t canbuft1[8]={0};
_temp=tar_sita[0]*CAN_POS_DIV;
canbuft1[0]=BYTE1(_temp);
canbuft1[1]=BYTE0(_temp);
_temp=tar_sita[1]*CAN_POS_DIV;
canbuft1[2]=BYTE1(_temp);
canbuft1[3]=BYTE0(_temp);
_temp=tar_sita[2]*CAN_POS_DIV;
canbuft1[4]=BYTE1(_temp);
canbuft1[5]=BYTE0(_temp);
canbuft1[6]=motor_en*100reset_q*10+reset_err;
canbuft1[7]=q_pid_sel; res=CAN1_Send_Msg(canbuft1,8,CAN_SD_MIT_HEAD_Q+id);
}2）设置三个电机的输出轴扭矩：
在完成标定后，既可以使能电机motor_en=1，并发送电机的期望扭矩，其协议为：
标准帧，帧头：CAN_SD_MIT_HEAD_T+节点ID;
帧长度：8
说明：上述[0]和[1]通道顺序受OCU电机交换功能影响，期望扭矩为输出轴，角度发送时的单位为Nm，并且发送时数据扩大了CAN_T_DIV，则以STM32为例发送函数为：
void CAN_set_pos_force_mit_div_t(char id){
char res=0;
int16_t _temp;
uint8_t canbuft1[8]={0};
_temp=set_t[0]*CAN_T_DIV;
canbuft1[0]=BYTE1(_temp);
canbuft1[1]=BYTE0(_temp);
_temp=set_t[1]*CAN_T_DIV;
canbuft1[2]=BYTE1(_temp);
canbuft1[3]=BYTE0(_temp);
_temp=set_t[2]*CAN_T_DIV;
canbuft1[4]=BYTE1(_temp);
canbuft1[5]=BYTE0(_temp);
canbuft1[6]=motor_en*100+reset_q*10+reset_err;
canbuft1[7]=q_pid_sel;
res=CAN1_Send_Msg(canbuft1,8,CAN_SD_MIT_HEAD_T+id);
}5.4 电机控制流程
综上，TS-DRIVE-NODE为了保证高实时的控制，采用了多帧协议配置，因此相比MIT的方式会略显麻烦，但是其能保证较好的机器人运动控制同步性，因此对其的驱动过程主要分为如下几个步骤：
1）写入3个电机的摆动PD参数；
2）写入3个电机的支撑PD参数；
3）写入3个电机的0位角度；
4）确认电机反馈标志位正常；
5）标定电机（可选），先发送reset_q=2，演示200ms后将其复位为0；
6）复位当前期望角度，开使能；
7）周期发送期望角度，期望扭矩和参数切换Flag；
注：TS-DRIVE-NODE最高伺服通讯频率可达1Khz，同时由于NODE具有看门狗包括需要至少10ms接收到一帧电机控制帧，因此我们推荐如下的发送方式：
