Elmo驱动器：CANopen协议电机PVT模式的PDO映射

0.前言

最近在做Rhex机器人，该机器人的六条腿部每条腿均只有一个电机，对步态的控制完全依赖于一个电机的控制，使用以前用的比较简单的电机位置PTP（Point to Point）模式时由于每两个点之间驱动器均要进行一次加速减速的过程，而实际的控制过程中对每条腿每个周期的规划是一个整体的加速减速过程，PTP导致电机运动噪音很大（每两个点之间从0加速到最大速度再减速到0，存在很大的冲击），而且在整个周期很短又有六条腿的情况下高频控制对通信带宽要求很高，因此决定采用PVT(Position Velocity Time)模式，即对一个周期内每个位置的速度和两个点直接的时间进行控制，这样就能实现整个周期的连续平稳运动。

1.开启PVT模式的PDO映射

Elmo驱动器预设了CANOPEN协议下的高速PVT模式的PDO映射，即直接通过PDO传输数据，不需要通过SDO访问对象字典的方法对数据进行读写，从而实现主机与从机直接的高效数据传递。
PDO的映射通过客户端（此时应为用户采用的CAN发送模块）向服务端（驱动器）发送SDO指令实现，SDO指令结构如下图所示



其中字节0为配置数据，字节123为对象字典index和sub-index，字节4567为数据
现在我们想将PDO映射为PVT模式参数，因此先看看使用PDO映射PVT时要在SDO指令中使用的接受PDO映射对象的ID和子ID


ID0x1600-0x1603分别对应R-PDO1~R-POD4，每一个ID同时具备两个子ID分别用于指明映射到该PDO的对象个数与映射的对象的ID、子ID和Date。
除此之外，对PDO进行映射时还需要对触发模式进行配置，




此处只需对传输模式进行配置，即sub-ID 2

由于我们要讲PVT模式参数映射到PDO，因此还需要知道PVT参数的对象ID

综合以上信息，此处我们将PVT模式的数据映射到RPDO3，将其配置为异步接收，即不需要同步信号，接收到数据后在驱动器空闲循环立即对数据进行处理，具体配置代码如下

/_*
 _ @brief SetPVTPDOMapping
 _      将PVT参数（0x2001）映射到RPDO2，用于快速传递PVT参数。
 _/

void Driver::SetPVTPDOMapping()
{
    can_frame frame;
    memset(&frame, 0, sizeof(can_frame));
    frame.can_id = mCANId - ELMO_CTRL_ID_BASE + 0x600;
    frame.can_dlc = 8;
    //0x1602(0)设置为0  Stop all emissions of RPDO2
    frame.data[0] = 0x23; //Initiate download, expedited, index valid, data valid
    frame.data[1] = 0x02; //Index (LO) to store at
    frame.data[2] = 0x16; //Index (HI) to store at
    frame.data[3] = 0x0;  //Sub-index to store at.
    frame.data[4] = 0x0;  //Data length
    frame.data[5] = 0x0;  //Sub-index
    frame.data[6] = 0x0;  //High data
    frame.data[7] = 0x0;  //Low data
    emit sendCANMsg_sig(frame);

    //0x1602(1)设置为0x20010040 将PVT映射到RPDO2
    memset(&frame, 0, sizeof(can_frame));
    frame.can_id = mCANId - ELMO_CTRL_ID_BASE + 0x600;
    frame.can_dlc = 8;
    frame.data[0] = 0x23;
    frame.data[1] = 0x02;
    frame.data[2] = 0x16;
    frame.data[3] = 0x1;
    frame.data[4] = 0x40;
    frame.data[5] = 0x0;
    frame.data[6] = 0x01;
    frame.data[7] = 0x20;
    emit sendCANMsg_sig(frame);

    //0x1401(2)设置为255 异步触发 接收到数据即处理
    memset(&frame, 0, sizeof(can_frame));
    frame.can_id = mCANId - ELMO_CTRL_ID_BASE + 0x600;
    frame.can_dlc = 8;
    frame.data[0] = 0x23;
    frame.data[1] = 0x02;
    frame.data[2] = 0x14;
    frame.data[3] = 0x02;
    frame.data[4] = 0xff;
    emit sendCANMsg_sig(frame);

    //0x1601(0)设置为1  启动PDO映射的对象（1个）
    memset(&frame, 0, sizeof(can_frame));
    frame.can_id = mCANId - ELMO_CTRL_ID_BASE + 0x600;
    frame.can_dlc = 8;
    frame.data[0] = 0x23;
    frame.data[1] = 0x02;
    frame.data[2] = 0x16;
    frame.data[3] = 0x0;
    frame.data[4] = 0x1;
    frame.data[5] = 0x0;
    frame.data[6] = 0x0;
    frame.data[7] = 0x0;
    emit sendCANMsg_sig(frame);
}

2.使用效果

经过上述配置之后，即可通过RPDO2向驱动器发送PVT参数，用于填充PVT队列，使用PVT队列在队列数据剩余N个时返回的紧急指令作为反馈，通过RPDO2(CANID 0X400+ID)向PVT队列填充数据，从而维持整个PVT模式的持续运动。