CANopen笔记3 -- DS402运动控制子协议
DS301就是一个通讯协议栈，DS402是建立在DS301基础之上的伺服类控制协议。协议中规定好每个对象字典值的作用，比如0x6040，是控制字。DS402把一个伺服控制系统应该具有的功能都定义好了，厂家和使用者按照协议定义即可开发和使用符合标准的设备。
- NMT
NMT是网络管理报文，用于实现一些管理操作，比如节点重启、进入运行状态等，网络管理状态转换图如下：
初始化：设备处于启动状态，不能进行通信
预运行：设备启动完毕，还未进入运行模式。设备仅回复SDO、NMT消息
运行：正常工作，可回复SDO、NMT、PDO
停止：仅能发送NMT（包括心跳消息）
NMT报文格式很简单，COB-ID固定为0x000，数据为：NMT命令 + 从设备节点ID（0x00表示广播）
- Boot-up Messages
设备开机启动完成初始化进入预运行状态时，会产生boot-up事件，发送一条boot-up消息。boot-up消息的COB-ID为：0x700 + Node ID。
假设节点ID为1，则该节点开机后会发送boot-up message（0x00 data, always 0）
- 设备控制
根据DS402协议（Chapter 6：Device Control Objects），设备的状态由下图描述。The device states and possible control sequence of the drive are described by the state machine, as depicted in the following figure:
如上图所示，状态机可以分成三部分：“ Power Disabled” (主电关闭)、“ Power Enabled”（主电打开）和“Fault”。所有状态在发生报警后均进 入“Fault”。在上电后 ，驱动器完成初始化， 然后进入SWITCH_ON_DISABLED状态。在该状态，可以进行CAN通讯，可以对驱动器进行配置（例如，将驱动器的工作模式设置成“ PP”模式）。此时，主电仍然关闭，电机没有被励磁。经过State Transition2、3、4 后，进入OPERATION ENABLE。此时，主电已开启，驱动器根据配置的工作模式控制电机。因此，在该状态之前必须先确认已经正确配置了驱动器的参数。 State Transition 9 完成关闭电路主电。一旦驱动器发生报警，驱动器的状态都进入FAULT。
设备上电自检和初始化完成后，会自动进入switch on disabled状态（The drive performs transitions 0 and 1 after initiation, either at power up or at NMT node reset）。状态之间的转变可以通过向对象字典0x6040处写入控制字实现（参考DS402-Chapter 6.5）；查询设备状态可以通过读取状态字实现（参考DS402-Chapter 6.6）。
下面进行一个测试，通过USB-CAN接口用PC机给elmo电机驱动器发送SDO数据帧（COB类型为：SDO-Receive，以驱动器的角度来看发送和接收），来查询设备状态。驱动器节点ID为1，SDO-Receive对应的功能码为0b1100，则COB-ID为：0b1100 0000 001->0x601。数据域中第一个字节为SDO命令，这里为0x40；接着3个字节为对象索引和对象子索引，由于没有子索引，该字节为0x00；最后四个字节为保留字节，全部为0。按照小端模式，数据域中的8个字节数据为0x40 41 60 00 00 00 00 00。设备返回数据帧ID为0x581(SDO-Transmit对应的功能码为0b1011，则COB-ID为：0b1011 0000 001->0x581)。数据域中第一个字节0x4B表示返回数据有16位，即0x0250。状态字中的Bits 0-3, 5 and 6这几位的组合表示驱动器所处的状态，根据描述表，可知0x0250表示驱动器处于Switch on disabled的状态。
从Switch on disabled状态到Ready to switch on状态发送： 0x2B 40 60 00 06 00 00 00
从Ready to switch on状态到Switch on状态发送： 0x2B 40 60 00 07 00 00 00
从Ready to switch on或Switch on到Operation enable状态发送：0x2B 40 60 00 0F 00 00 00
具体状态转变发送的指令参考下表：
- 控制模式
The drive behavior depends on the activated modes of operation. The modes-of-operation variables are initialized at reset to no mode (value 0) . If set to OPERATION ENABLE, the motor stands still until an explicit motion command is received via a control word. Modes of operation用于设置控制模式，Modes of operation display用于读取当前控制模式。
常见的一些模式如下：
发送0x40 61 60 00 00 00 00 00查询当前驱动器控制模式，返回数据为：0x4F 61 60 00 01 00 00 00. 根据上表可知，驱动器处于位置控制模式。发送0x2F 60 60 00 04 00 00 00 ，可以将其改为力矩/电流模式。
一般伺服都有三种控制方式：速度控制方式，转矩控制方式，位置控制方式 。之所以有这三中控制方式，是因为伺服一般为三个环控制。所谓三环就是3个闭环负反馈PID调节系统。由伺服系统的三个控制回路来实现。
第1环是电流环，它是最内环。此环完全在伺服驱动器内部进行，通过霍尔装置检测驱动器给电机的各相的输出电流，负反馈给电流的设定进行PID调节，从而达到输出电流尽量接近等于设定电流，电流环就是控制电机转矩的，所以在转矩模式下驱动器的运算最小，动态响应最快。
第2环是速度环，它是次外环，通过检测的电机编码器的信号来进行负反馈PID调节，它的环内PID输出直接就是电流环的设定，所以速度环控制时就包含了速度环和电流环，换句话说任何模式都必须使用电流环，电流环是控制的根本，在速度和位置控制的同时系统实际也在进行电流（转矩）的控制以达到对速度和位置的相应控制。
第3环是位置环，它是最外环，可以在驱动器和电机编码器间构建也可以在外部控制器和电机编码器或最终负载间构建，要根据实际情况来定。由于位置控制环内部输出就是速度环的设定，位置控制模式下系统进行了所有3个环的运算，此时的系统运算量最大，动态响应速度也最慢。
- 位置模式运动参数
target position 是给定目标位置，该位置可以是相对值，也可以是绝对值，取决于controlword的bit6
profile velocity 是指电机启动完成加速阶段后达到的速度
max profile velocity 是位置模式下运动过程中允许的最大速度
end velocity 是到达给定位置时的速度。通常为了在到达给定位置时停止驱动器，该参数设置为 0；但在连续多点位置时，该值可以设置成非零值
maximal acceleration/deceleration 是指最大加减速度，用来限制加减速度范围，防止电机和运动机构遭受损坏
profile acceleration 是到达给定位置期间的加速度
profile deceleration 是到达给定位置期间的减速度
quick stop deceleration 是急停命令（Quick Stop command）发出时的减速度
motion profile type 被用来选择何种速度曲线
- 位置控制模式
位置模式的控制字（0x6040）：
其中第4,5,6位所代表的信息与具体运行模式有关，位置模式下的控制字如下：
位置模式的状态字（0x6041）：
按照下表写入相应的控制字，可以设定期望的位置运动模式：
If the change set immediately bit of the Controlword is set to 1, one set-point is still in progress and a new one is activated, the previous motion is discarded without setting the target reached bit and a new one is started with its target position and speed. The target reached bit is set on the end of the new profile.
给定目标位置有两种方法：
单步设定：电机到达目标位置后，驱动器通知主机“目标位置到达”，然后获取新的目标位置并开始运动。在获取新的目标位置前，电机速度减速到零。
连续设定：电机到达目标位置后，立即继续朝下一个事先设置好的目标位置运动。这样可达到无停顿的连续运动效果，在两个目标位置之间，电机可以无需减速到零。
======================================================================================
单步设定方法的步骤：
1、首先设置节点NMT状态为Operational(0x01 start remote node;0x00 means "for all nodes")
0x00 0x01 0x00
2、设置运行模式为位置模式
0x601 0x22 0x60 0x60 0x00 0x01 0x00 0x00 0x00
3、设置位置模式的运动参数(如速度、加速度等)
0x601 0x22 0x81 0x60 0x00 XX XX XX XX
0x601 0x22 0x83 0x60 0x00 XX XX XX XX
4、设备进入Ready to switch on状态
0x601 0x22 0x40 0x60 0x00 0x06 0x00 0x00 0x00
5、设备进入Switch on状态
0x601 0x22 0x40 0x60 0x00 0x07 0x00 0x00 0x00
6、设备进入Operation enable状态
0x601 0x22 0x40 0x60 0x00 0x0F 0x00 0x00 0x00
7、设置目标位置(10000)
0x601 0x22 0x7A 0x60 0x00 0x10 0x27 0x00 0x00
8、设置位置模式控制字(absolute,immediate,not blended)
0x601 0x22 0x40 0x60 0x00 0x3F 0x00 0x00 0x00
或设置为：relative,immediate,not blended
0x601 0x22 0x40 0x60 0x00 0x7F 0x00 0x00 0x00
9、 发送指令？ Set-point acknowledge位？
0x601 0x22 0x40 0x60 0x00 0x0F 0x00 0x00 0x00
10、设置新目标位置(假设为0)
0x601 0x22 0x7A 0x60 0x00 0x00 0x00 0x00 0x00
11、设置位置模式控制字，开始运动
0x601 0x22 0x40 0x60 0x00 0x3F 0x00 0x00 0x00
========================================================================================
- 单位换算
伺服驱动器被广泛应用在各种场合。为了在各类应用时都能方便地设置参数，驱动器内部的单位换算单元可以将任意用户单位参数值自动与驱动器内部单位值自动互换。
position factor对象将用户端的所有长度单位转换成驱动器内部单位（脉冲），同时也将驱动器输出量的长度单位（脉冲）转换成用户端的长度单位（position units）。Position value in user units can be calculated from position in internal units by the formula:
- Binary Interpreter Commands
The Binary Interpreter is designed to simplify the way by which CAN master can get and set Elmo commands. By default, PDO2 is mapped to the Binary Interpreter objects. In that way, the CAN host can address the Elmo commands immediately after power up.The Binary Interpreter uses the 2-letters format where the first two bytes are the explicit command for example in order to get the AC command, the first two transmitted bytes will be ASCII value of ‘A’ & ‘C’ which is 0x41 & 0x43 respectively. For detailed information about the Elmo commands, please refer to the Gold Command Reference manual.
注意：由于Binary Interpreter使用了RPTO2和TPDO2，因此驱动器需要处于Operational NMT状态，否则不能进行PDO通信(NMTstart node command must be sent before）。通过CAN发送2进制指令和通过RS-232串口发送ASCII指令的区别如下：
RS-232 operation is fast and simple, requiring no detailed understanding of communication processes. CANopen communication achieves higher rates and is able to support advanced functions.
Set and Query Commands
The host (client) sends commands (RPDO2) for setting variables in eight bytes (DLC=8). The drive (server) transmits the reply (TPDO2) as an asynchronous event of the received object.
RPDO2用来设置驱动器参数和读取相关值，命令结构如下：
字节0—字节1： 指令的ASCII码（注意字母都是大写），比如"PX"指令，相应的ASCII码为0x50 0x58，分别代表大写字母P和X
字节4—字节7： 4字节的数据，可以为整数或浮点数，以小端模式存储
字节2—字节3： command index(when needed) and data type(float or integer)
字节2的0-7位和字节3的0-5位：In array commands in which the index is used (as in ET[100]), the lowest significant bits are in byte 2 (bits 0 to 7) and the most significant bits are in byte 3.
字节3的第6位： When this bit is set to 1, the drive treats the command as a query and not as a setting. In this case, the rest of the data bytes are discarded. （For compatibility reasons, bytes 4 to 7 should be 0.）
字节3的第7位：Always use the bit 7 of byte 3 to indicate the data type (float or integer) in the transmitted message.
驱动器通过TPDO2回复客户端的设置或查询指令。The server (drive) replies (TPDO2) to query and set requests in eight bytes (DLC=8)：
Bytes 0 to 3 are the header, which includes the responding command, command index (when needed) and data type (float or integer). It also indicates whether the response data is true data or an error code. When this byte 3,bit 6 is 1 for TPDO, the data in bytes 4 to 7 should be interpreted as an error code. Bytes 4 to 7 are data or an error code.
驱动器的回复格式如下（Byte3的第6位如果为1，则数据为错误代码）：
Example：
1. 设置CL[1] = 1.0，浮点数1.0表示为3F800000h(参考IEEE 754标准)，则通过RPDO2发送的数据如下：
2. 设置AC=150000（0249F0h），发送的数据如下：
3. 获取AC的值，发送数据如下（数据长度为4字节）：
或者发送如下数据（数据长度为8字节）：
4. 获取ET[992](992 = 3E0h)的值，假设该值为32121（7D79h），则发送如下数据：
5. 获取编码器位置，通过RS232发送字符串指令"PX"或通过CAN接口发送二进制指令：
TPDO2返回数据为0x2710 —> 10000，即当前编码器在10000个脉冲位置。
设置编码器位置为0（需要先关闭电机，发送"MO=0"，或通过RPDO2发送:0x4D 4F 00 00 00 00 00 00），发送如下图所示的数据帧后，编码器位置被清零，文本框中的10000变为了0。
6. 发送"PA/PR=10000"和"BG"指令，控制电机运动到绝对/相对位置10000，实现相同的功能可通过RPDO2发送如下指令：
PA=10000：0x50 41 00 00 10 27 00 00 PR=10000：0x50 52 00 00 10 27 00 00
BG：0x42 47 00 00
发送"PR=-10000"让电机朝相反方向运动(-10000的补码为：0xFF FF D8 F0)，则发送如下指令设置相对运动位置：
0x50 52 00 00 F0 D8 FF FF
7. 发送TC指令设置电流大小，并切换到力矩/电流模式，电机即可运动。比如通过RS232发送"TC=1.5"，电机即以指定的电流大小运动。
8. 发送JV和BG指令，电机将以指定的速度运行。
9. 通过KP[1]，KI[1]调整力矩/电流模式下的比例和积分系数。
Execute Command
Execute指令不带操作数，用于控制驱动器完成特定的功能。The reply to these commands is only an acknowledgement or an error code.
Example： 发送BG指令（BG – Begin Motion：The BG command is used to activate the next profiled motion），发送数据如下：
返回数据长度为8字节，并表明操作成功或失败（失败时数据为错误代码）：
参考：
http://blog.csdn.net/gw569453350game/article/details/51025425
