说明：
用于CANopen总线伺服的调试
目前主要用于：
雷赛DMA882-CAN
设计：
伺服 x1轴 = new 伺服（站号1+）；
x1轴.写参数（【参数地址】，【值】）；
//=================================
x1轴.电流（1）；//【1A】
x1轴.齿轮比（分子，分母）；//【圈脉冲】
x1轴.模式（定位）；//【定位】【力矩】【速度】【回零】
x1轴.使能
x1轴.回零
x1轴.停止
x1轴.复位
x1轴.绝对
x1轴.相对
x1轴.力矩
x1轴.速度
x1轴.绝对（坐标，0）；// 不立即执行
加速度
减速度
雷赛DMA882-CAN
更新24.5.6
增加oop概念。
旧版本：
#define debug
using CAN_namespace;
using help;
using Models;
using System;
using System.Collections.Generic;
using System.IO;
using System.IO.Ports;
using System.Linq;
using System.Net.Sockets;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Xml.Linq;
using static BLL.CANopen;
using static System.Net.Mime.MediaTypeNames;
//===============
#region Cia定义
//指数(十六进制) 目标
//0000 未使用
//0001 - 025F 数据类型
//0260 - 0FFF 保留供进一步使用
//1000-1FFF 》通信参数 区域 CiA301（DS301）
//2000-5FFF 》硬件参数 区域（生产制造商特定）
//6000-9FFF 》标准化，运动控制 区域 CiA402（DSP402）
//A000-AFFF 网络可变区域
//B000-BFFF 系统可变区
//C000-CFFF 保留供进一步使用
//————————————————
//版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
//原文链接：https://blog.csdn.net/cfqq1989/article/details/128308808
#endregion
#region Cia301_通讯层
//1000h 装置类型(硬件识别码0x40192) 04020192 h : A2 Series 06020192h : M Series
//1001h 错误寄存器4组
//1003h 预定义错误4组 （-01：58120）
//1005h COB-ID SYNC码 0x80（同步码）
//1006h 通信周期0
//100Ch 保护时间0
//100Dh 寿命因数0
//1010h 参数保存4组（0x1010_0x04=0x65766173;回帧大于2秒）
//1011h 恢复默认参数4组（0x1011_4=0x64616F6C;不要保存，只要断电重启）
//1014h COB-ID EMCY（0xFF）急停码
//1016h 消费者心跳时间-1
//1017h 发生器心跳时间 2000ms
//1018h 标识对象4
//1029h 阵列错误行为-1
//1200h 第一台服务器SDO参数SDO参数RO（2）
//1400h~03h 接收PDO参数（开关配置）bit31总开关（0开）bit30RTR（0开）bit29标准帧（0标准帧1扩展帧）bit28~0 COB-ID码
//1600h~03h 接收PDO映射（寄存器指针）
//1800h~03h 发送PDO参数（开关配置）
//1A00h~03h 发送PDO映射（寄存器指针）
#endregion
#region Cia402_运动层
//603Fh 最新错误代码-1
//6040h 控制字==========0
//6041h 状态字==========
//605Ah 快速停止代码
//605Bh 停机选项代码-1
//605Eh 故障反应选项代码-1
//6060h 工作模式（ 1位置 3速度 4扭矩 6回零 7补间）================
//6061h 显示驱动器的工作模式
//6062h 显示电机命令位置
//6063h 位置实际值-1
//6064h 显示电机实际位置0
//6065h 以下错误窗口-1
//6067h 位置窗口-1
//6068h 位置窗口时间-1
//606Bh 显示电机命令速度0
//606Ch 显示电机的实际速度0
//606Dh 速度窗口-1
//606Eh 速度窗口时间-1
//606Fh 速度阈值-1
//6071h 目标扭矩-1
//6074h 扭矩需求值-1
//6075h 电机额定电流-1
//6076h 电机额定扭矩-1
//6077h 扭矩实际值-1
//6078h 当前实际值-1
//607Ah 目标位置0
//607Ch 原点偏移0
//607Dh 软件位置限制-1
//607Fh 最大剖面速度-1
//6080h 最大电机转速-1
//6081h 位置模式的最大速度（每秒脉冲量）除以每圈脉冲 （每圈默认5万）
//6082h 起跳速度
//6083h 剖面加速度（默认50 0000
//6084h 剖面减速度（默认500 0000
//6085h 急停减速度（默认5000 0000
//6086h 运动剖面类型-1
//6087h 扭矩斜率-1
//6093h位置系数2
//6098h回零方式0
//6099h-01原点快速
//6099h-02原点慢速
//609Ah 回零加减速 5000000
//60B0h位置偏移-1
//60B1h速度偏移-1
//60B2h扭矩偏移-1
//60C0h插值子模式选择-1
//60C1h插值数据记录-1
//60C2h插值时间段-1
//60C5h最大加速度-1
//60C6h最大减速度-1
//60F2h定位选项代码-1
//60F4h跟随误差实际值0
//60FCh位置需求值-1
//set_region(0x2154, 0, 1);// 输入io电平逻辑 0正逻辑1反
//60FDh 输入 IO 状态0（bit0：负限位 bit1：正限位 bit2：原点信号 bit16：快速停止
//60FEh-01 物理输出开启1
//60FEh-02 物理输出开启2
//60FFh目标速度0
//6502h支持的驱动模式-1
#endregion
namespace BLL
{
public class CANopen//被调用版本必须低（调用者，只需要属性里有接口）
{
#region 全局
//1申明委托》委托命令
public delegate void WT_GET_Data(byte[] butes, string data);//↑ui显示发送命令
public delegate void WT_SET_Data(byte[] butes, string data);//↓下执行对象
#region 参数类型
//public unsafe struct VCI_CAN_OBJ //CAN帧参数，
//{
// public uint ID;// 帧ID。 32位变量，数据格式为靠右对齐。 详情请参照： 《8.附件1： ID对齐方式.pdf》说明文档。
// public uint TimeStamp; //设备接收到某一帧的时间标识。 时间标示从CAN卡上电开始计时，计时单位为0.1ms。
// public byte TimeFlag; //是否使用时间标识，为1时TimeStamp有效
// public byte SendType; //发送帧类型。=0时为正常发送（发送失败会自动重发，重发超时时间为4秒， 4秒内没有发出则取消）；=1时为单次发送
// public byte RemoteFlag; //是否是远程帧。 =0时为为数据帧， =1时为远程帧（数据段空）。
// public byte ExternFlag; //是否是扩展帧。 =0时为标准帧（11位ID）， =1时为扩展帧（29位ID）。
// public byte DataLen; //数据长度 DLC (<=8)，即CAN帧Data有几个字节。约束了后面Data[8]中的有效字节。
// public fixed byte Data[8]; //如DataLen定义为3，即Data[0]、 Data[1]、 Data[2]是有效的。
// public fixed byte Reserved[3];//系统保留。
//}
#endregion
#endregion
#region 字段
Help_String helpstring = new Help_String();// 文本转换
/// <summary>
/// 设备类型
/// </summary>
uint CAN_DeviceType = (uint)Help_USB_CAN.VCI_USBCAN2;// 设备类型
/// <summary>
/// 硬件编号
/// </summary>
uint CAN_ind = 0; // 硬件编号
/// <summary>
/// 通道编号
/// </summary>
const uint CAN1 = 0;// 通道编号
const uint CAN2 = 1;
Help_EightDiagrams diagrams;
Help_USB_CAN can = new Help_USB_CAN();
Help_USR_socket rj45 = new Help_USR_socket();
int s剩余帧数;
#region 内部
static Int64 byte8 = 0;// 64bit 数据帧 包
static volatile byte Region_Type;// 寄存器标记（读40h，写，成功60h，失败80h）
bool debug = false;
#endregion
//private CancellationTokenSource cts;//线程令牌
//private ManualResetEvent resetEvent = new ManualResetEvent(true);// 暂停业务
#endregion
#region 属性
/// <summary>
/// ↑：byte包，文本
/// </summary>
static public WT_GET_Data Wt_get;//↑event委托=》呼叫上ui层 让上层显示：发送命令
/// <summary>
/// ↓：byte包，文本
/// </summary>
static public WT_SET_Data Wt_set;//↓委托=》呼叫下位机执行
public byte address { get; set; } = 127; // 从站1~127（不能为0，因为0站是广播）
public Int32[] AXIS;// 轴坐标
public Help_USR_socket RJ45 { set => rj45 = value; }// 以太网设备转CAN
public Iface_RTX help_rtx { get; set; }// 接口扩展（用于通讯的收，发）
public bool timeOutFun { get; set; } = false;// 通讯超时功能
public int get排队帧 { get; set; } = 0;
#endregion
#region 构造
public CANopen(Help_USR_socket rj45)
{
help_rtx = rj45;
RJ45 = rj45;
//diagrams = new Help_EightDiagrams();// 八卦炉
//diagrams.令牌1 = true;// 总开关
//diagrams.镇坛木1 = true;// 循环
//====================
debug = true;//调试
}
public CANopen(Iface_RTX iface)
{
help_rtx = iface;// 接口 Help_RTX
//diagrams = new Help_EightDiagrams();// 八卦炉
//diagrams.令牌1 = true;// 总开关
//diagrams.镇坛木1 = true;// 循环
//====================
debug = true;//调试
}
#endregion
#region 方法_APP
#region 核心_延时
/// <summary>
/// Help_Delay.delayTime(0.123);//0.1秒
/// </summary>
/// <param name="secend"></param>
void delaySecond(double secend)// 延时，秒
{
DateTime tempTime = DateTime.Now;
while (tempTime.AddSeconds(secend).CompareTo(DateTime.Now) > 0)
{
//Application.DoEvents();
}
}
#endregion
#region 收发GET_SET
public int Send(int hand8, int cobid, Int64 byte8)// 1 4 8
{
//int hand = 0x88;// 设备：USR-CANET200
//int cobid = 0x12345678;// 11bit或29bit
//Int64 byte8 = 0x0011223344556677;
//wait_dataRX数据池();
//can.send(hand8, cobid, byte8);
//wait_dataRX数据池();
string cmd = madeCode(hand8, cobid, byte8);// Code编码
int sta = (int)help_rtx.TXs(cmd);// 接口功能
if (sta < 1) throw new Exception("发送失败。检查主站状态！");
return sta;
}
/// <summary>
/// 伺服参数操作 0x40读 0x20+写
/// </summary>
/// <param name="byte8"></param>
void SDO(Int64 byte8)// 600h+
{// SDO操作 600h+
Send(0x08, 0x600 + address, byte8);// 40 2000 00
//===============================================
code码（发送）===============================
//byte by0 = (byte)regionGET.读;// 读 0x40
//Int16 by1 = reg;// 寄存器
//byte by2 = (byte)(Son);// 子索引
b0 b1b2 b3 b4b5b6b7 // 编码
//byte8 = by0 + (by1 << 8) + (by2 << 24); // 40 2000 00
3 准备发送==================================
Send(0x04, 0x600 + address, byte8);//40 2000 00
help_rtx.TXs(madeCode(0x04, 0x600 + address, byte8));
//SDO(byte8);
}
public void SDO_set(regionSET regtype, short reg, int Son, int data)// 1操作类型，2寄存器编号，3子索引，4参数值
{// SDO操作 600h+
//========================================================
// 准备发送
byte by0 = (byte)regtype;//1 写 操作码
Int16 by1 = reg;//2 寄存器
byte by2 = (byte)(Son);//3 子索引
Int64 by3 = data;//4设定值
// b0 b1b2 b3 b4b5b6b7
byte8 = by0 + (by1 << 8) + (by2 << 24) + (by3 << 32); // 23 2000 00
Send(0x08, 0x600 + address, byte8);// 40 2000 00
}
/// <summary>
/// 可能返回null
/// </summary>
/// <returns></returns>
public byte[] Receive()
{// rj45.socket
#if RJ45
lock (this) //加锁，防止冲突
{
byte[] buffer = new byte[13]; //定义一个接收的数组
MemoryStream ms = new MemoryStream();//创建一个内存流，用来保存数据
if (rj45.socket.Available > 0)
{
//读取缓冲区的字节数
int count = rj45.socket.Receive(buffer, 13, SocketFlags.None);
//把缓冲区的数据写入内存流
ms.Write(buffer, 0, count);
//读取完毕，返回读取的数据
return ms.ToArray();
}
else
{
}
return buffer;
#else
byte[] rx13 = (byte[])help_rtx.RXs();// 提取数据帧
return rx13;
#endif
}
/// <summary>
/// rx13[5] = 0x80;// 故障码
/// </summary>
/// <param name="ms"></param>
/// <param name="WaitCount"></param>
/// <returns></returns>
byte[] wait_Receive(int ms, int WaitCount)
{
DateTime startTime = DateTime.Now;//记录开始时间
byte[] rx13;// 提取数据帧
while (true)
{
if (timeOutFun)
{
if ((DateTime.Now - startTime).TotalMilliseconds > ms)// 超时
{
//Region_Type = 0x80;// 标记 读取失败
rx13 = new byte[13];
rx13[5] = 0x80;// 故障码
break;
}
}// 超时
rx13 = Receive();//回帧报文
if (rx13 == null)
{
if (WaitCount <= 0)// 超次数
{
rx13 = new byte[13];
rx13[5] = 0x80;// 故障码
break;
}
WaitCount--;
Help_Delay.delayTime(0.003);
continue;
}// 超次
else
{// 非空
return rx13;
}
}
return rx13;
}
#endregion
#region 读写寄存器
public unsafe int get_region(Int16 reg, int Son)//寄存器，子索引 //0x0000~0xFFFF
{
#region 清空缓存_---使发/收对应
//#if debug
// s剩余帧数 = can.VCI_get缓存区帧数(Help_USB_CAN.VCI_USBCAN2, 0, CAN1);
//#endif
// can.set_ClearBuffer(Help_USB_CAN.VCI_USBCAN2, 0, CAN1);// CAN1清空接收
//#if debug
// s剩余帧数 = can.VCI_get缓存区帧数(Help_USB_CAN.VCI_USBCAN2, 0, CAN1);
//#endif
#endregion
#region set_只为获取寄存器类型
SDO_set(regionSET.读, reg, Son, 0);
#endregion
#region get_Value
VCI_CAN_OBJ[] obj = new VCI_CAN_OBJ[2500];// 创新科技CAN分析仪最大支持2000帧接收
byte[] rx13 = new byte[13];// 提取数据帧
Help_Delay.delayTime(0.005);//5ms // USB速度3~5ms
int num = can.get_Receive(CAN_DeviceType, CAN_ind, CAN1, ref obj[0], 2000, 50);
if (num > 0)
{
for (int i = num - 1; i >= 0; i--)
{
if (obj[i].ID > 0x80 && obj[i].ID < (0x80 + 127))// 从站故障 80h+ 从站
{
// 故障呼叫
//COB - ID Byte 0:1 Byte 2 Byte 3:7
//0x080 + Node - ID 应急错误代码 错误寄存器(1001H) 厂商指定区域
//byte0~1：
// 应急错误代码
// 0000H 无错误
// 8110H CAN 溢出
// 8120H 错误被动模式（0411）
// 8130H 寿命保护/ 心跳错误
// 8140H 被迫离线恢复故障
// 8141H 被迫离线
// 8150H 发送 COB - ID 冲突
// 8210H PDO 长度错误未处理
// 8220H PDO 超过长度
//byte2：
// (1001H)位定义
// Bit0：generic error 一般错误
// Bit1：current 电流
// Bit2：voltage 电压 （04）
// Bit3：temperature 温度
//————————————————
//版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。
//原文链接：https://blog.csdn.net/cfqq1989/article/details/128308808
}// 故障（从站故障）80h + 从站
if (obj[i].ID > 0x580 && obj[i].ID < 0x600 && obj[i].Data[0] != 0x60)// SDO 600h+
{
rx13[5] = obj[i].Data[0];
rx13[6] = obj[i].Data[1];
rx13[7] = obj[i].Data[2];
rx13[8] = obj[i].Data[3];
rx13[9] = obj[i].Data[4];
rx13[10] = obj[i].Data[5];
rx13[11] = obj[i].Data[6];
rx13[12] = obj[i].Data[7];
break;
}// SDO应答帧
else rx13[5] = 0x80;// 故障
}
}
else
{
rx13[5] = 0x80;// 故障
}
// 回帧结果
Region_Type = rx13[5];// 标记： 0x4b读
int value = get_result(rx13);// 解析
// 日志====================================
if (Region_Type < 0x43 | Region_Type > 0x4F)// 读取失败
{
string error = getError(value, reg, Son, -1); // 故障码，寄存器，子，值
if (Wt_get != null)
{
Wt_get(new byte[] { 0x00 }, "读取错误：" + error);
}
throw new Exception("读取错误：" + error);// 提前终止
}
return value;
#endregion
}
/// <summary>
/// SET 寄存器
/// </summary>
/// <param name="reg">寄存器</param>
/// <param name="Son">子索引</param>
/// <param name="data">数值</param>
/// <returns></returns>
/// <exception cref="Exception">结果</exception>
public unsafe int set_region(Int16 reg, byte Son, Int32 data, int waitTimeMs)
{
VCI_CAN_OBJ[] obj = new VCI_CAN_OBJ[2500];// 创新科技CAN分析仪最大支持2000帧接收
byte[] rx13 = new byte[13];// 提取数据帧
//=======================================
int valus = get_region(reg, Son);//用于 获知 寄存器类型
//========================================================
// 准备发送
SDO_set((regionSET)Region_Type - 0x20, reg, Son, data);// 全局 Region_Type
// 等待应答帧
Help_Delay.delayTime(0.005 + (waitTimeMs / 1000));//5ms // USB速度3~5ms
int num = can.get_Receive(CAN_DeviceType, CAN_ind, CAN1, ref obj[0], 2000, 50);
if (num > 0)
{
for (int i = num - 1; i >= 0; i--)
{
if (obj[i].ID > 0x80 && obj[i].ID < (0x80 + 127))// 从站故障 80h+ 从站
{
// 故障呼叫
//COB - ID Byte 0:1 Byte 2 Byte 3:7
//0x080 + Node - ID 应急错误代码 错误寄存器(1001H) 厂商指定区域
//byte0~1：
// 应急错误代码
// 0000H 无错误
// 8110H CAN 溢出
// 8120H 错误被动模式（0411）
// 8130H 寿命保护/ 心跳错误
// 8140H 被迫离线恢复故障
// 8141H 被迫离线
// 8150H 发送 COB - ID 冲突
// 8210H PDO 长度错误未处理
// 8220H PDO 超过长度
//byte2：
// (1001H)位定义
// Bit0：generic error 一般错误
// Bit1：current 电流
// Bit2：voltage 电压 （04）
// Bit3：temperature 温度
//————————————————
//版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。
//原文链接：https://blog.csdn.net/cfqq1989/article/details/128308808
}// 故障（从站故障）
if (obj[i].ID > 0x580 && obj[i].ID < 0x600 && (obj[i].Data[0] == 0x60 | obj[i].Data[0] == 0x80))// SDO 600h+
{
rx13[5] = obj[i].Data[0];
rx13[6] = obj[i].Data[1];
rx13[7] = obj[i].Data[2];
rx13[8] = obj[i].Data[3];
rx13[9] = obj[i].Data[4];
rx13[10] = obj[i].Data[5];
rx13[11] = obj[i].Data[6];
rx13[12] = obj[i].Data[7];
break;
}// SDO应答帧
else rx13[5] = 0x80;// 故障
}
}// 提取数据
else
{
rx13[5] = 0x80;// 故障
}// 报错
// 回帧结果
Region_Type = rx13[5];// 标记： 结果
int end = get_result(rx13);// 解析数值
// 日志====================================
if (Region_Type != 0x60)// 写入失败
{
string error = getError(end, reg, Son, data); // 故障码，寄存器，子，值
if (Wt_get != null)
{
Wt_get(new byte[] { 0x00 }, "写失败：" + error);// 错误 上传
}
throw new Exception("寄存器写入失败！");
}
if (Region_Type == 0x60) return 1;//成功
return end;
}//长时间set寄存器
public unsafe int set_region(Int16 reg, byte Son, Int32 data)
{
return set_region(reg, Son, data, 0);
}
/// <summary>
/// 故障码，寄存器，子，值
/// </summary>
/// <param name="errcode"></param>
/// <param name="reg"></param>
/// <param name="Son"></param>
/// <param name="data"></param>
/// <returns></returns>
string getError(Int32 errcode, Int16 reg, Int32 Son, Int32 data)
{
string errorString = string.Empty;
string str1 = $"Reg{reg.ToString("X4")}h-{Son.ToString("X2")}={data.ToString()};";
switch (errcode)
{
case 0x05030000: errorString = str1 + "触发位没有交替变化"; break;
case 0x05040000: errorString = str1 + "SDO 协议超时"; break;
case 0x05040001: errorString = str1 + "非法/ 未知的命令字"; break;
case 0x05040002: errorString = str1 + "无效的块大小(仅块传输模式)"; break;
case 0x05040003: errorString = str1 + "无效的序号(仅块传输模式)"; break;
case 0x05040004: errorString = str1 + "CRC 错误(仅块传输模式)"; break;
case 0x05040005: errorString = str1 + "内存溢出"; break;
case 0x06010000: errorString = str1 + "对象不支持访问"; break;
case 0x06010001: errorString = str1 + "试图读只写对象"; break;
case 0x06010002: errorString = str1 + "试图写只读对象"; break;
case 0x06020000: errorString = str1 + "对象不存在"; break;
case 0x06040041: errorString = str1 + "对象不能映射到PDO内"; break;
case 0x06040042: errorString = str1 + "映射的对象的数目和长度超出 PDO 长度"; break;
case 0x06040043: errorString = str1 + "一般性参数不兼容"; break;
case 0x06040047: errorString = str1 + "一般性设备内部不兼容"; break;
case 0x06060000: errorString = str1 + "硬件错误导致对象访问失败"; break;
case 0x06060010: errorString = str1 + "数据类型不匹配，服务参数长度不匹配"; break;
case 0x06060012: errorString = str1 + "数据类型不匹配，服务参数长度太长"; break;
case 0x06060013: errorString = str1 + "数据类型不匹配，服务参数长度太短"; break;
case 0x06070010: errorString = str1 + "数据型态不符；参数长度不符"; break;
case 0x06090011: errorString = str1 + "子索引不存在"; break;
case 0x06090030: errorString = str1 + "超出参数的值范围(写访问时)"; break;
case 0x06090031: errorString = str1 + "写入参数数值太大"; break;
case 0x06090032: errorString = str1 + "写入参数数值太小"; break;
case 0x06090036: errorString = str1 + "最大值小于最小值"; break;
case 0x08000000: errorString = str1 + "一般性错误"; break;
case 0x08000020: errorString = str1 + "数据不能传送或保存到应用"; break;
case 0x08000021: errorString = str1 + "由于本地控制导致数据不能传送或保存到应用"; break;
case 0x08000022: errorString = str1 + "对象使用中"; break;
case 0x08000023: errorString = str1 + "对象字典不存在"; break;
//0800 0023H
//对象字典动态产生错误或对象字典不存在(例如通过文件生成对象字典，但由于文件
//损坏导致错误产生)
default:
errorString = str1 + "未知错误。检查网线？";
break;
}
return errorString;
}
#endregion
#region 协议can
/// <summary>
/// 心跳包0x1017（16bit）
/// </summary>
/// <param name="address"></param>
public void SYNC_close(byte address)
{//关闭 心跳 0x1017=2000;ms
Send(0x08, 0x600 + address, 0x00000000010172b);// 寄存器 0x1017,操作码 0x2b （16bit）
//delaySecond(2.0);//延时2秒
Help_Delay.delayTime(0.123);//0.1秒
}
public void SYNC_open(byte address, int time)
{//心跳 0x1017 = 2000; ms
set_region(0x1017, 0x00, 2000);// 0x7d0 2000ms
Help_Delay.delayTime(2.123);//2.1秒
}
/// <summary>
/// 用于同步信号（广播帧）留20ms电机响应
/// </summary>
public void SYNC()
{// SYNC同步心跳 80h+
Send(0x08, 0x80, 0x00);// id=0x80
// 等待同步完成
Help_Delay.delayTime(0.020);//0.02秒
}
/// <summary>
/// 总线管理（命令，从站对象）；cob_id=0x00
/// 启动从站 = 0x01,
/// 停止从站 = 0x02,
/// 预运行从站 = 0x80,
/// 重启从站 = 0x81,
/// 通讯重连 = 0x82
/// </summary>
/// <param name="mode"></param>
/// <param name="address"></param>
public void NMT(NMTe modecmd, byte address)
{// NMT总线管理操作 00h+
byte by0 = (byte)modecmd;
byte by1 = address;
// b0 b1b2 b3 b4b5b6b7
byte8 = by0 + (by1 << 8);
//接收负载=0
Send(0x08, 0x00, byte8);
//延时 等待生效75ms
DateTime startTime = DateTime.Now;//记录开始时间
do
{
if ((DateTime.Now - startTime).TotalMilliseconds > 75)// 75ms
{
break; // 超时 毫秒
}
} while (true);
}
/// <summary>
/// 时间广播
/// </summary>
/// <param name="byte8"></param>
public void TIME(Int64 byte8)
{// TIME操作 100h+
Send(0x08, 0x100, byte8);
}
public void set_PDO(PDOtype pdo, int reg1, int bita, int reg2)
{
Int64 a = reg1;// 6040h 控制字
Int64 b = reg2;// 607Ah 目标位置
//int bits = 16;//reg1长度
Int64 byte8;
//a &= 0xFF;// bit4（准备上升沿） 1111 1111
b = b << bita;
byte8 = a + b;// 6040绑定在（数据帧低字节）
// 200h+
Send(0x08, 0x100 + (int)pdo * 0x100 + address, byte8);// 生效了
}
public void set_PDO(PDOtype pdo, int reg1, int bita, int reg2, int bitb, int reg3)
{
Int64 a = reg1;// 6040h 控制字 16bit
Int64 b = reg2;// 607Ah 目标位置 32bit
Int64 c = reg3;// 6081h 速度 16bit
int bits = 16;//reg1长度
Int64 byte8;
a &= 0xFF;// bit4（准备上升沿） 1111 1111
b = b << bits;// 16bit
c = c << 48;// 16bit + 32bit
byte8 = a + b + c;// 6040绑定在（数据帧低字节）
Send(0x08, 0x100 + (int)pdo * 0x100 + address, byte8);// 生效了
}
public void PDO(CMDtype cmd, Int32[] AXIS)// PDO 快捷命令
{// PDO1操作 200h+
// 6040 607a
// 16 32
// 1122-33445566-7788 （数据帧：低字节先出）
// a<<6字节
// b<<2字节
byte at = address;//标记
//6040
//0 Switch on 转换开
//1 Enable voltage电压开
//2 Quick Stop快停
//3 Enable operation 启用操作
//4 hm回零操作开始（正触发） pp新设定点（正触发）
//5 pp立即更改集合
//6 pp绝对（0）/相对（1）
//7 Fault reset 故障复位 ==
//8 Halt 暂停 ==
//9 Change of Set Point
if (cmd == CMDtype.回零)// bit4
{
for (int i = 127; i >= 1; i--)
{
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x0F, 16, 0);// 命令
}
SYNC();
for (int i = 127; i >= 1; i--)
{
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x1F, 16, 0);// 命令
}
SYNC();
address = at;// 还原
return;
}
if (cmd == CMDtype.绝对定位)
{
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x2F, 16, 0);// 6040命令
set_PDO(PDOtype.PDO2, AXIS[i], 32, AXIS[0]);// 轴坐标 + 速度
}
SYNC();// 同步帧：准备
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x3F, 16, AXIS[i]);
}
SYNC();// 同步帧：开始
address = at;// 还原
return;
}
if (cmd == CMDtype.相对定位)
{
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x6F, 16, 0);// 6040命令
set_PDO(PDOtype.PDO2, AXIS[i], 32, AXIS[0]);// 轴坐标 + 速度
}
SYNC();// 同步帧
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x7F, 16, 0);
}
SYNC();
address = at;// 还原
return;
}
if (cmd == CMDtype.补间)
{
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x6F, 16, 0);// 命令
set_PDO(PDOtype.PDO2, AXIS[i], 32, AXIS[0]);// 轴坐标 + 速度
}
SYNC();// 同步帧
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x7F, 16, AXIS[i]);
}
SYNC();
address = at;// 还原
return;
}
if (cmd == CMDtype.速度 | cmd == CMDtype.力矩)
{
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x07, 16, 0);// 6040命令
set_PDO(PDOtype.PDO2, AXIS[i], 32, AXIS[0]);// 速度 + 起步速度
}
SYNC();// 同步帧
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x0F, 16, 0);
}
SYNC();
address = at;// 还原
return;
}
if (cmd == CMDtype.复位)// bit7
{
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x0F, 16, 0);// 命令
}
SYNC();// 同步帧
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x8F, 16, 0);
}
SYNC();// 同步帧
address = at;// 还原
return;
}
if (cmd == CMDtype.暂停)// bit8
{
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x0F, 16, 0);// 命令
}
SYNC();// 同步帧
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x10F, 16, 0);
}
SYNC();// 同步帧
address = at;// 还原
return;
}
//Int64 a = reg1;// 6040h 控制字
//Int64 b = reg2;// 607Ah 目标位置
//int bits = 16;//reg1长度
//Int64 byte8;
//if (reg1 == 0x10F)//停止运行
//{
// b = b << bits;
// byte8 = a + b;// 6040绑定在（数据帧低字节）
// Send(0x08, 0x200 + address, byte8);
// SYNC();// 同步信号
// return;
//}
//a &= 0xEF;// bit4（准备上升沿）
//b = b << bits;
//byte8 = a + b;// 6040绑定在（数据帧低字节）
//Send(0x08, 0x200 + address, byte8);// 生效了
//SYNC();// 同步信号，用于寄存器生效
===============================
Help_Delay.delayTime(0.040); // 超过 命令滤波 时间 1400h-02
===============================
下一帧（上升沿）
//a |= 1 << 4;// 上升沿
//byte8 = a + b;// 6040绑定在（数据帧低字节）
//Send(0x08, 0x200 + address, byte8);
//SYNC();// 同步信号，用于激活旋转
}
public unsafe void gCode(CMDtype cmd, Int32[] AXIS)// G0 X123.123 F1000
{
VCI_CAN_OBJ[] obj = new VCI_CAN_OBJ[2500];// 创新科技CAN分析仪最大支持2000帧接收
byte[] rx13 = new byte[13];// 提取数据帧
can.set_ClearBuffer(4, 0, 0);
PDO(cmd, AXIS);
do
{
set_PDO(PDOtype.PDO1, 0, 0, 0);//
Help_Delay.delayTime(0.005);//5ms // USB速度3~5ms
int num = can.get_Receive(CAN_DeviceType, CAN_ind, CAN1, ref obj[0], 2000, 50);
if (num > 0)
{
for (int i = num - 1; i >= 0; i--)
{
if (obj[i].ID > 0x80 && obj[i].ID < (0x80 + 127))// 从站故障 80h+ 从站
{
// 故障呼叫
//COB - ID Byte 0:1 Byte 2 Byte 3:7
//0x080 + Node - ID 应急错误代码 错误寄存器(1001H) 厂商指定区域
//byte0~1：
// 应急错误代码
// 0000H 无错误
// 8110H CAN 溢出
// 8120H 错误被动模式（0411）
// 8130H 寿命保护/ 心跳错误
// 8140H 被迫离线恢复故障
// 8141H 被迫离线
// 8150H 发送 COB - ID 冲突
// 8210H PDO 长度错误未处理
// 8220H PDO 超过长度
//byte2：
// (1001H)位定义
// Bit0：generic error 一般错误
// Bit1：current 电流
// Bit2：voltage 电压 （04）
// Bit3：temperature 温度
//————————————————
//版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。
//原文链接：https://blog.csdn.net/cfqq1989/article/details/128308808
}// 故障（从站故障）80h + 从站
// 等待轴定位完成
if (obj[i].ID > 0x180 && obj[i].ID <= 0x180+127)// PDO1 200h+
{
rx13[5] = obj[i].Data[0];
rx13[6] = obj[i].Data[1];
rx13[7] = obj[i].Data[2];
rx13[8] = obj[i].Data[3];
rx13[9] = obj[i].Data[4];
rx13[10] = obj[i].Data[5];
rx13[11] = obj[i].Data[6];
rx13[12] = obj[i].Data[7];
if ((rx13[6]&0x16) == 0x16)// bit10
{
return;
}
}// SDO应答帧
}
}
else
{
Help_Delay.delayTime(0.205);//
}
} while (true);
}
/// <summary>
/// 移动控制6040h(控制字)，607A(目标位置)
/// </summary>
/// <param name="byte8"></param>
public void PDO1(bool en, Int32[] AXIS)// 6040 607A 目标位置
{// PDO1操作 200h+
// 6040 607a
// 16 32
// 1122-33445566-7788 （数据帧：低字节先出）
// a<<6字节
// b<<2字节
byte at = address;//标记
if (!en)
{
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x0F, 16, 0);// 命令
set_PDO(PDOtype.PDO2, AXIS[i], 32, AXIS[0]);// 轴坐标 + 速度
}
SYNC();// 同步帧
for (int i = 127; i >= 1; i--)
{
if (AXIS[i] == -1) continue;
address = (byte)i;// 指向从站
set_PDO(PDOtype.PDO1, 0x1F, 16, AXIS[i]);
}
}
else
{
SYNC();
}
address = at;// 还原
//Int64 a = reg1;// 6040h 控制字
//Int64 b = reg2;// 607Ah 目标位置
//int bits = 16;//reg1长度
//Int64 byte8;
//if (reg1 == 0x10F)//停止运行
//{
// b = b << bits;
// byte8 = a + b;// 6040绑定在（数据帧低字节）
// Send(0x08, 0x200 + address, byte8);
// SYNC();// 同步信号
// return;
//}
//a &= 0xEF;// bit4（准备上升沿）
//b = b << bits;
//byte8 = a + b;// 6040绑定在（数据帧低字节）
//Send(0x08, 0x200 + address, byte8);// 生效了
//SYNC();// 同步信号，用于寄存器生效
===============================
Help_Delay.delayTime(0.040); // 超过 命令滤波 时间 1400h-02
===============================
下一帧（上升沿）
//a |= 1 << 4;// 上升沿
//byte8 = a + b;// 6040绑定在（数据帧低字节）
//Send(0x08, 0x200 + address, byte8);
//SYNC();// 同步信号，用于激活旋转
}
public void PDO2(int reg1, int bits, int reg2)//相对运动
{// PDO2操作 300h+
// 6040 607a
// 16 32
// 1122-33445566-7788 （数据帧：低字节先出）
// a<<6字节
// b<<2字节
Int64 a = reg1;
Int64 b = reg2;
Int64 byte8;
if (reg1 == 0x10F)//停止运行
{
b = b << bits;
byte8 = a + b;// 6040绑定在（数据帧低字节）
Send(0x08, 0x200 + address, byte8);
SYNC();// 同步信号
return;
}
a |= 0x4F;
a &= 0xEF;// （准备上升沿） 4F 》 5F
b = b << bits;
byte8 = a + b;// 6040绑定在（数据帧低字节）
Send(0x08, 0x300 + address, byte8);
SYNC();// 同步信号
//===============================
Help_Delay.delayTime(0.040); // 超过 命令滤波 时间 1400h-02
//下一帧（上升沿）
//===============================
a |= 0x5F;// 上升沿
byte8 = a + b;// 6040绑定在（数据帧低字节）
Send(0x08, 0x300 + address, byte8);
SYNC();// 同步信号
}
public void PDO3(Int64 byte8)
{// PDO3操作 400h+
Send(0x08, 0x400 + address, byte8);
}
public void PDO4(Int64 byte8)
{// PDO4操作 500h+
Send(0x08, 0x500 + address, byte8);
}
/// <summary>
/// SET到伺服8字节，这是寄存器快捷键1400h，1600h
/// </summary>
/// <param name="zu">组#</param>
/// <param name="reg1">指针1</param>
/// <param name="bita">位数</param>
/// <param name="reg2">指针2</param>
/// <param name="bitb">位数</param>
/// <exception cref="Exception"></exception>
public void PDOr_Config(PDOtype PDOn, PDOregion regs)// 1400,1600
{// 1400-00=5
// 1400-01=4000027F （0x4000 0200h+从站地址127） // 200h+7F bit31是PDO总开关（0开）
// bit30 0：PDO上允许RTR
// bit29 0:标准11bit帧(CAN 2.0A) 1：扩展帧29bit(CAN 2.0B)
// bit28~0 ：COB-ID码
//
// 1400-02=0xFF // 0~240us 时，只要接收到一个同步帧则将该 RPDO 最新的数据更新到应用，255新数据立即生效
// 1400=03=0x00 // 禁止时间： rPDO1命令过滤间隔，单位0.1ms
// 1400-04=不存在
// 1400-05=0 //事件计时器
// 1600-00=2 //寄存器指针数
// 1600-01=60400010 // 6060h=1默认定位模式
// 1600-02=607A0020 // 607A是目标位置
// 1600-03=0
Int32 regValue;
//1400-01：总开关
//1600-00：绑定数量
//1600-01：第一绑定寄存器 6040油门离合
//1600-02：第二绑定寄存器 607A目标地址
// 2个寄存器，6040h，607Ah
// 0x6040 b7 b3b2b1b0
// b7 ↑故障复位
// 0x06 关闭
// 0x07 打开
// 0x0F 打开+启用操作
// 0<<1 快速停止
// 0x07 禁用操作
// 0x0F 启用操作
//===============================
short zu = (short)(PDOn);
short h1400 = (short)((zu - 1) + 0x1400);
short h1600 = (short)((zu - 1) + 0x1600);
try
{
//解锁 //开始设置 ， 从机响应配置
set_region(h1400, 0x01, 1 << 31); // 关闭 rPDO 功能(原值： 0x4000 0200h + 从站地址)
set_region(h1400, 0x02, 234); //接收类型： 生效rPDO数据的延迟 us（0~240us） （原值：0x000000FF）255为事件
set_region(h1400, 0x03, 0);// 接收PDO1命令过滤间隔，单位0.1ms 雷赛40ms（原值：0x00000000）
set_region(h1600, 0x00, 0x00); // 寄存器指针:关闭 （原值：0x00000002）
for (int i = 0; i < regs.num; i++)
{//绑定地址
regValue = regs.regName[i];
regValue <<= 16;//固定
regValue += regs.regBit[i];// 寄存器bit
set_region(h1600, (byte)(1 + i), regValue);// 指针1》绑定6040 0x60400010 （原值：0x60400010）16bit
}
// 上锁
set_region(h1600, 0x00, regs.num); // 绑定的寄存器 生效 （原值：0x00000002）
int pdo = 0x100 + (zu * 0x100) + address; //bit31(0开PDO功能)bit30(0开RTR)bit29(0标准帧)bit28~0(COB-ID码)
// 4000 027F
set_region(h1400, 0x01, pdo); // 开启PDO1功能 （原值：0x4000 0201）
// bit31是PDO总开关（0开）
// bit30 0：PDO上允许RTR(RTR是强制显性1bit)
// bit29 0:标准11bit帧(CAN 2.0A) 1：扩展帧29bit(CAN 2.0B)
// bit28~0 ：COB-ID码
}
catch (Exception ex)
{
throw ex;
}
//regs.num = 1;
//regs.regName[0] = 0x6040;// 控制字
//regs.regBit[0] = 16;
}
/// <summary>
/// 控制字，定位，速度
/// </summary>
/// <param name="PDOn"></param>
/// <param name="reg1"></param>
/// <param name="bita"></param>
/// <param name="reg2"></param>
/// <param name="bitb"></param>
/// <param name="reg3"></param>
/// <param name="bitc"></param>
public void PDOr_Config(PDOtype PDOn, Int16 reg1, int bita, Int16 reg2, int bitb, Int16 reg3, int bitc)// 1400,1600
{// 1400-00=5
// 1400-01=4000027F （0x4000 0200h+从站地址127） // 200h+7F bit31是PDO总开关（0开）
// bit30 0：PDO上允许RTR
// bit29 0:标准11bit帧(CAN 2.0A) 1：扩展帧29bit(CAN 2.0B)
// bit28~0 ：COB-ID码
//
// 1400-02=0xFF // 0~240us 时，只要接收到一个同步帧则将该 RPDO 最新的数据更新到应用，255新数据立即生效
// 1400=03=0x00 // 禁止时间： rPDO1命令过滤间隔，单位0.1ms
// 1400-04=不存在
// 1400-05=0 //事件计时器
// 1600-00=2 //寄存器指针数
// 1600-01=60400010 // 6060h=1默认定位模式
// 1600-02=607A0020 // 607A是目标位置
// 1600-03=0
Int32 regValue;
int regNum = 1;//绑定寄存器数量
//1400-01：总开关
//1600-00：绑定数量
//1600-01：第一绑定寄存器 6040油门离合
//1600-02：第二绑定寄存器 607A目标地址
// 2个寄存器，6040h，607Ah
// 0x6040 b7 b3b2b1b0
// b7 ↑故障复位
// 0x06 关闭
// 0x07 打开
// 0x0F 打开+启用操作
// 0<<1 快速停止
// 0x07 禁用操作
// 0x0F 启用操作
//===============================
short zu = (short)(PDOn);
short h1400 = (short)((zu - 1) + 0x1400);
short h1600 = (short)((zu - 1) + 0x1600);
try
{
//解锁 //开始设置 ， 从机响应配置
set_region(h1400, 0x01, 1 << 31); // 关闭 rPDO 功能(原值： 0x4000 0200h + 从站地址)
set_region(h1400, 0x02, 234); //接收类型： 生效rPDO数据的延迟 us（0~240us） （原值：0x000000FF）255为事件
set_region(h1400, 0x03, 0);// 接收PDO1命令过滤间隔，单位0.1ms 雷赛40ms（原值：0x00000000）
set_region(h1600, 0x00, 0x00); // 寄存器指针 关闭 （原值：0x00000002）
regValue = reg1;
regValue <<= 16;//固定
regValue += bita;// 寄存器bit
set_region(h1600, 0x01, regValue);// 指针1》绑定6040 0x60400010 （原值：0x60400010）16bit
if (reg2 != 0x00)
{
regNum = 2;// 又加一个
regValue = reg2;
regValue <<= 16; //固定
regValue += bitb;//寄存器bit 32bit(0x20)
set_region(h1600, 0x02, regValue);// 绑定607A 32bit 0x607A0020
}
if (reg3 != 0x00)
{
regNum = 3;// 又加一个
regValue = reg3;
regValue <<= 16; //固定
regValue += bitc;//寄存器bit 32bit(0x20)
set_region(h1600, 0x03, regValue);// 速度
}
// 上锁
set_region(h1600, 0x00, regNum); // 绑定的寄存器 生效 （原值：0x00000002）
int pdo = 0x100 + (zu * 0x100) + address; //bit31(0开PDO功能)bit30(0开RTR)bit29(0标准帧)bit28~0(COB-ID码)
// 4000 0200h + 7F
set_region(h1400, 0x01, pdo); // 开启PDO1功能 （原值：0x4000 0201）
// bit31是PDO总开关（0开）
// bit30 0：PDO上允许RTR(RTR是强制显性1bit)
// bit29 0:标准11bit帧(CAN 2.0A) 1：扩展帧29bit(CAN 2.0B)
// bit28~0 ：COB-ID码
}
catch (Exception ex)
{
throw ex;
}
}
public void PDOr_Config(PDOtype PDOn, Int16 reg1, int bita, Int16 reg2, int bitb)// 1400,1600
{
PDOr_Config(PDOn, reg1, bita, reg2, bitb, 0, 0);
// 1400-00=5
// 1400-01=4000027F （0x4000 0200h+从站地址127） // 200h+7F bit31是PDO总开关（0开）
// bit30 0：PDO上允许RTR
// bit29 0:标准11bit帧(CAN 2.0A) 1：扩展帧29bit(CAN 2.0B)
// bit28~0 ：COB-ID码
//
// 1400-02=0xFF // 0~240us 时，只要接收到一个同步帧则将该 RPDO 最新的数据更新到应用，255新数据立即生效
// 1400=03=0x00 // 禁止时间： rPDO1命令过滤间隔，单位0.1ms
// 1400-04=不存在
// 1400-05=0 //事件计时器
// 1600-00=2 //寄存器指针数
// 1600-01=60400010 // 6060h=1默认定位模式
// 1600-02=607A0020 // 607A是目标位置
// 1600-03=0
//1400-01：总开关
//1600-00：绑定数量
//1600-01：第一绑定寄存器 6040油门离合
//1600-02：第二绑定寄存器 607A目标地址
// 2个寄存器，6040h，607Ah
// 0x6040 b7 b3b2b1b0
// b7 ↑故障复位
// 0x06 关闭
// 0x07 打开
// 0x0F 打开+启用操作
// 0<<1 快速停止
// 0x07 禁用操作
// 0x0F 启用操作
//===============================
}
/// <summary>
/// GET上传 1800h，1A00h
/// </summary>
/// <param name="reg1"></param>
/// <param name="bita"></param>
/// <param name="reg2"></param>
/// <param name="bitb"></param>
public void PDOt_Config(PDOtype PDOn, Int16 reg1, int bita, Int16 reg2, int bitb)//1800,1A00
{// 1800-00=5 子索引数量
// 1800-01=400001FF 回帧功能码 0x4000 0180h+从站号 7F（127） // 最高bit位是总开关（1<<31是关)
// 1800-02=0xFF 当 TPDO 传输类型为 0 时，如果映射对象的数据发生改变，且接收到一个同步帧，则发送该 TPDO；
// 收到1~240个同步帧后，发送tPDO帧
// 0xFF异步是指数据一旦发生变化马上，发送该 TPDO
// 实时性要求不高的参数用同步 PDO 的方式，实时性高的参数用异步 PDO 的传输方式，但
// 要注意配置禁止时间，以保护网络负荷不受冲击
// 1800-03=0 抑制时间，发送失败间隔，单位0.1ms
// 1800-04=不存在
// 1800-05=0 事件计时器，如果在计时器运行周期内出现了数据改变等其它事件，TPDO 也会触发，且事件计数器会被立即复位。
// 0是关闭， bit15~0：周期性触发时间，单位毫秒ms
// 1A00-00=1 寄存器指针数
// 1A00-01=60410010 // 0x6041是状态字
// 1A00-02=0
Int32 reg;
int regs = 1;
//===============================
short zu = (short)(PDOn);
short h1800 = (short)((zu - 1) + 0x1800);
short h1A00 = (short)((zu - 1) + 0x1A00);
try
{
//解锁 //开始设置 上传
set_region(h1800, 0x01, 1 << 31); // 关闭tPDO功能 (原值： 0x4000 0180h+从站地址)
set_region(h1800, 0x02, 0xFF); // 收到1~240个同步帧就发送tPDO，（原值：0x000000FF）255数据变化就立即发送tPDO
set_region(h1800, 0x03, 400);// 抑制时间// 发送失败间隔，tPDO 间隔 单位0.1ms （40ms）设置400
set_region(h1A00, 0x00, 0x00); // 关闭 PDO1 寄存器指针 （原值：0x00000002）
reg = reg1;
reg <<= 16;//固定
reg += bita;// 寄存器bit
set_region(h1A00, 0x01, reg);// 指针1》绑定6041 0x60410010 （原值：0x60410010）16bit
if (reg2 != 0x00)
{
regs = 2;// 又加一个
reg = reg2;
reg <<= 16; //固定
reg += bitb;//寄存器bit 32bit(0x20)
set_region(h1A00, 0x02, reg);// 绑定607A 32bit 0x607A0020
}
// 上锁
set_region(h1A00, 0x00, regs); // 寄存器指针 生效 （原值：0x00000001）
int pdo = (0 << 30) + 0x80 + (zu * 0x100) + address; //b31总开关（0开）b30强制RTR显性开关（0开）b29~0帧id码
// 0x 0000 0180h + 从站地址
set_region(h1800, 0x01, pdo); // 开启 tPDO1 回帧 08(8字节) 0000 0181 寄存器A 寄存器B
}
catch (Exception ex)
{
throw ex;
}
}
#endregion
#region 伺服配置
/// <summary>
/// 初始化，从站
/// </summary>
/// <param name="id"></param>
public void setMode_Init(int id)// 初始化
{// 初始化从站
try
{
this.address = (byte)id;// 从站地址
SYNC();// 0x80帧，网络同步 ( 回帧 281,381,481)
NMT(NMTe.重启从站, address);
Help_Delay.delayTime(0.123);//0.1秒
NMT(NMTe.启动从站, 0x00);// 所有，从站
//===================
//SYNC_close(address);//关闭从站广播
//===================
#region 设备电机参数
if (address <= 127 | address >= 122) // 轴127 — 122
{
setMode_Init雷赛();// 电机初始化
}
#endregion
//使能电机
PowerEN();// 6>7>F
}
catch (Exception ex)
{
throw ex;
// 批量修改寄存器，必须用try，遇到错误能及时退出操作！！！！！！！
}
//=====rPDO1配置
//0x80 网络同步信号
//0x00 81 01 NMT 重启01站
//0x00 01 00 NMT 启动从站 0所有
//=========ACK 700h+
// 1400-01=0x8000000 rPDO1 bit31:1关闭PDO1,bit28:0是11bit，+CANcobid功能码 0x200+本站地址
// 1400.1=0x40000201 (1400=5,,1400.2=0xff,1400.3=0,1400.4=0,1400.5=0)
// 1600-00=0 rPDO1绑定 0失效 (2个)
// 1400-02=0x01 同步的延迟1us（1~240us）
// 1400-03=0x190 rPDO1 40ms
// 1600-01=0x60ff 00 20 32bit （ 6040 00 10）
// 1600-02=0x6040 00 10 16bit （ 607A 00 20）
// 1600-00=0x02 绑定2个寄存器生效
// 1400-01=0x4000 0201 rPDO1 开 （ 4000 0201）
//======tPDO1配置
// 0x00 81 01 NMT 重启01站
// 1800-01=0x80000000 tPDO1关 （ 4000 0181 ）
// 1A00-00=0x00 0绑定失效 （1）
// 1800-02=0xff 延迟255（0x01同步） （0xff事件）
// 1800-03=0x0190 上传间隔40ms
//
// 1A00-01=0x6041 00 10 16bit （6040 00 10）
// 1A00-02=0x3010 00 10 （0）
// 1A00-00=0x02 生效 6041,3010
// 1800-01=0x4000 0181 tPDO1开 （ 4000 0181）
//保存参数
// 1010-01=0x6576 6173
//PDOr_Config(1, 0x6040, 16, 0x607A, 32);// PDO1 // 控制字，目标位置
//PDOt_Config(1, 0x6041, 16, 0x6064, 32);// 第2寄存器绑 （状态字，位置反馈）
}
void setMode_Init雷赛()//0x2001一圈脉冲200，0x60ff 一秒脉冲量200 这样一秒一圈
{// 只要电流保护
// ===============设备厂家参数==============
set_region(0x2000, 0, 3000); // 电流 （默认3200mA） 最大 8200mA
// 2005-01 输出口1功能设置 bit0：报警输出 Bit2：到位输出 Bit4：主站控制输出
// 2005-02 输出口1功能设置 bit0：报警输出 Bit2：到位输出 Bit4：主站控制输出
// 2008-00 输出口逻辑方向 位定义：bit0 对应 out1 以此类推
// 2051-00 电机运行方向
// 2152-01 输入数字 IO 口1 功能选择 1：原点信号； 2：正限位；4：负限位； 8：快速停止；16：自定义；32：Probe1 功能（暂无该功能）；64：Probe2功能（暂无该功能）
set_region(0x2005, 1, 1);// io输出1 故障灯 （ bit0：报警输出 Bit2：到位输出 Bit4：主站控制输出
set_region(0x2005, 2, 2);// io输出2 刹车阀
//set_region(0x2154, 0, 1);// 输入io电平逻辑 0正逻辑1反
set_region(0x2152, 1, 1);// io输入1 原点（1原点信号 2正限位 4负限位 8快速停止 16自定义 32Probe1 功能（暂无该功能）；64：Probe2功能（暂无该功能）
set_region(0x2152, 2, 2);// io输入2 正限
set_region(0x2152, 3, 4);// io输入3 负限
set_region(0x2152, 4, 8);// io输入4 //急停
//60FEh-01 物理输出开启1
//60FEh-02 物理输出开启2 （刹车阀线圈
//60FDh 输入 IO状态 （bit0：负限位 bit1：正限位 bit2：原点信号 bit16：快速停止
}
/// <summary>
/// 回零模式：Home
/// </summary>
/// <param name="id"></param>
/// <param name="Puls">一圈脉冲数</param>
/// <param name="homeMode"></param>
/// <param name="Hspeed"></param>
/// <param name="Lspeed"></param>
/// <param name="AThome"></param>
public void setMode_HM(byte id, GoHome homeMode, int Hspeed, int Lspeed, int AThome)// Home
{// 回零模式：Home
//6040h b4:Homing operation start (positive trigger)
// 回零操作开始 （正触发）
//60FDh数字输入0
address = id;// 重新绑定从站
try
{
// HM回零模式
set_region(0x6060, 0, 6); //设置为回零模式 （ 1位置 3速度 4扭矩 6回零 7补间）
if (get_region(0x6061, 0) != 6) throw new Exception("回零模式：设置失败！");
// 速度基准 （步进电机57 速度250r/mm
set_region(0x2001, 0, 200); // 一圈脉冲量，（200~51200）， 》》》》》这个影响转速
//60ff 一秒脉冲量：一秒一圈（一秒脉冲量等于一圈脉冲量）
set_region(0x6081, 0, 200 * 250 / 60); // 最高速度（每秒脉冲量
set_region(0x6082, 0, 200 * 100 / 60); //起跳速度
set_region(0x6083, 0, 200 * 250 / 1); // 加速度 1秒
set_region(0x6084, 0, 200 * 250 / 1); // 减速度
set_region(0x6085, 0, 200 * 2500); // 急停减速度 （额定转速除以0.1秒
// 回零配置
set_region(0x607C, 0, AThome); // 原点偏移
set_region(0x6098, 0, (byte)homeMode); // 回零方式选择（1~35） 35当前位置强制零点
set_region(0x6099, 1, Hspeed); // 回零 高速
set_region(0x6099, 2, Lspeed); // 回零 低速（找Z点速度）
set_region(0x609A, 0, 3000); // 回零加减速
PDOr_Config(PDOtype.PDO1, 0x6040, 16, 0, 0);
//执行回零 bit4
set_PDO(PDOtype.PDO1, 0x0f, 16, 0);// 准备回零
SYNC();
set_PDO(PDOtype.PDO1, 0x1f, 16, 0);
SYNC(); // 触发回零
}
catch (Exception ex)
{
throw ex;
}
}
/// <summary>
/// 定位模式（齿轮分子，齿轮分母）
/// </summary>
public void setMode_PP(int zi, int mu)// 定位
{
// 目标位置607A-00
// 目标速度6081-00
// 实际位置6064-00
// 实际速度606C-00
// 数字输入60FD-00
// 最新错误603F-00
// 定位模式：6060
// 加速 6083
// 减速 6084
// 电机位置6064
//控制字6040
//状态字6041
//读取当前模式6061
// ，读实际位置6063，读电机位置6064，位置容忍误差6065，读追随误差60F4，到位范围值6067，到位时间值6068
try
{
// PP定位模式
set_region(0x6060, 0, 1); //设置为轮廓位置模式 （ 1位置 3速度 4扭矩 6回零 7补间）
if (get_region(0x6061, 0) != 1) throw new Exception("定位模式：设置失败！");
//==========受限于单帧8字节===========
//齿轮比
set_region(0x6093, 1, zi); //乘以N ，齿轮 分母(编码器最大值)
set_region(0x6093, 2, mu); //除以N ，齿轮 分子（一圈脉冲数）200
// 第1组 绝对定位
PDOr_Config(PDOtype.PDO1, 0x6040, 16, 0, 0);//set 控制字
PDOt_Config(PDOtype.PDO1, 0x6041, 16, 0x6064, 32);//get 位置反馈（32bit）、、注意6041的bit10
// 第2组
PDOr_Config(PDOtype.PDO2, 0x607A, 32, 0x6081, 32);// 目标位置,目标速度
//PDOt_Config(PDO.PDO2, 0x6041, 16, 0x6064, 32);//位置反馈（32bit）
}
catch (Exception ex)
{
throw ex;
//PDOr_Config(PDO.PDO1, 0x6040, 16, 0x6081, 32);//目标位置
//PDOr_Config(PDO.PDO1, 0x6040, 16, 0x607A, 32, 0x6081, 32);//目标位置,速度
}
//6061读电机当前模式
//6062读命令位置
//6063读位置
//6064当前电机位置
//set_region(0x6065, 0, 12345);// 位置容忍误差
//set_region(0x6067, 0, 12345);// 到位确认，范围值
//set_region(0x6068, 0, 12345);// 到位确认，时间范围值
//607A设置目标位置
// 速度
//set_region(0x6081, 0, 100); //定位速度；（受齿轮比影响）
//set_region(0x6082, 0, 10); // 默认0， 启停速度
//set_region(0x6083, 0, 3000);// 加速
//set_region(0x6084, 0, 3000);// 减速
//set_region(0x6085, 0, 50000000); // 停止减速度 50w（2FAF080）
//60F4h跟随误差实际值
//60FCh位置需求值
//set_region(0x6060, 0, 0x01); //
//set_region(0x6060, 0, 0x01); //
// 设置运动轨迹参数
//603 8 23 81 60 00 20 4E 00 00 ‘设置速度为20000 step/ s
//603 8 23 83 60 00 40 9C 00 00 ‘设置加速度为40000 step/ s2
//603 8 23 84 60 00 40 9C 00 00 ‘设置减速度为40000 step/ s2
//绝对运动
//603 8 23 7A 60 00 40 0D 03 00 ‘设置目标位置为200000 steps
//603 8 2B 40 60 00 1F 00 00 00 ‘设置New Set Point位为1(电机开始绝对运动)
//603 8 2B 40 60 00 0F 00 00 00 ‘清除New Set Point
//相对运动
//603 8 23 7A 60 00 40 0D 03 00 ‘设置目标位置为200000 steps
//603 8 2B 40 60 00 5F 00 00 00 ‘设置New Set Point位为1(电机开始相对运动)
//603 8 2B 40 60 00 4F 00 00 00 ‘清除New Set Point
// 607A 目标位置
// 607E 数据取反
// 6081 轮廓速度
// 607F 最大速度
// 6083 轮廓加速度
// 6084 轮廓减速度
// 6085 快速停止速度
// 605A 快速停止模式
// 6062 取得內部位置命令。 (unit: PUU)
// 6063h 取得實際位置。 (unit: increments)
// 6065h】設定為實際位置和位置命令的容忍誤差值。
// 60F4h】，取得追隨誤差的實際數值。
// 6067h】，定義距離目標位置的允許正負誤差範圍
//6068h】，設置當停留於 Position window 內多少時 間時 ，可判定為位置到達(unit: millisecond)。
// 0x6040控制字
//0x6041状态值
//0x6060操作模式
//0x6061操作模式显示（ 1位置 3速度 6回零 ）
//0x607A目标位置
//0x6081剖面速度
//0x6083配置文件加速
//0x6084轮廓减速
//0x6085急停减速
//0x605A快速停止代码
//===========================
}
/// <summary>
/// 补间模式
/// </summary>
public void setMode_IP()// 补间
{//60C0h插值子模式选择INTEGER16 RW
//60C1h插值数据记录ARRAY RW
try
{
set_region(0x6060, 0, 7); //设置为轮廓位置模式 （ 1位置 3速度 4扭矩 6回零 7补间）
if (get_region(0x6061, 0) != 7) throw new Exception("补间模式：设置失败！");
//齿轮比
set_region(0x6093, 1, 1);//乘以N ， 齿轮 分子1
set_region(0x6093, 2, 1);//除以N ， 齿轮 分母1
}
catch (Exception ex)
{
throw ex;
}
}
/// <summary>
/// 速度模式：目标速度0x60FF，速度反馈0x606C
/// </summary>
public void setMode_PV()// 速度Speed 200转（200*100）填 2万
{// 目标速度 60FF-00
try
{
// 速度
set_region(0x6060, 0, 3); //设置为轮廓位置模式 （ 1位置 3速度 4扭矩 6回零 7补间）
if (get_region(0x6061, 0) != 3) throw new Exception("速度模式：设置失败！");
// 第一组
PDOr_Config(PDOtype.PDO1, 0x6040, 16, 0, 0);//6040控制字
PDOt_Config(PDOtype.PDO1, 0x6041, 16, 0x606C, 32);// 状态字 + 速度反馈
// 第二组
PDOr_Config(PDOtype.PDO2, 0x60ff, 32, 0, 0);// 目标速度
}
catch (Exception ex)
{
throw ex;
//2001 一圈脉冲 200
//60ff 一秒脉冲量200 这样一秒一圈
}
}
public void setMode_PT(byte id)// 扭矩t 模式
{
try
{
// 扭矩
set_region(0x6060, 0, 4); //设置为轮廓位置模式 （ 1位置 3速度 4扭矩 6回零 7补间）
if (get_region(0x6061, 0) != 4) throw new Exception("扭矩控制模式：设置失败！");
PDOr_Config(PDOtype.PDO1, 0x6040, 16, 0, 0);//6040控制字
PDOt_Config(PDOtype.PDO1, 0x6041, 16, 0x6077, 32);//扭矩反馈
PDOr_Config(PDOtype.PDO2, 0x6071, 16, 0, 0);//目标扭矩
}
catch (Exception ex)
{
throw ex;
}
}
//===============================================
/// <summary>
/// 0x1010_0x04=0x65766173;回帧大于2秒
/// </summary>
/// <returns></returns>
public int set_Save()// 1010h_4个子项
{
int sta = 1;
//byte by0 = (byte)regionSET.写b32;// 写32bit
//Int16 by1 = 0x1010;// 寄存器 0x1010
//byte by2 = (byte)(0x01);// 子索引 0x01
//Int64 by3 = 0x65766173;
b0 b1b2 b3 b4b5b6b7
//byte8 = by0 + (by1 << 8) + (by2 << 24) + (by3 << 32);
//SDO(byte8);
sta &= set_region(0x1010, 0x01, 0x65766173, 2000);// 保存
return sta;
}
/// <summary>
/// 0x1011_4=0x64616F6C;不要保存，只要断电重启
/// </summary>
/// <returns></returns>
public int set_Rest恢复出厂()//4个子项 // 回帧大于1秒
{
int sta = 1;
//byte by0 = (byte)regionSET.写b32;// 写32bit
//Int16 by1 = 0x1011;// 寄存器 0x1011
//byte by2 = (byte)(0x01);// 子索引 0x01
//Int64 by3 = 0x64616F6C;
b0 b1b2 b3 b4b5b6b7
//byte8 = by0 + (by1 << 8) + (by2 << 24) + (by3 << 32);
//SDO(byte8);
//set_Save();// 保存参数，需要断电生效
sta &= set_region(0x1011, 0x04, 0x64616F6C, 1000);// 恢复出厂，回帧大于1秒 （雷赛只能子索引4）
return sta;
}
/// <summary>
/// 高位地址（N*32+SW=127）32的倍数
/// SW为off时，逻辑为1
/// </summary>
/// <param name="address"></param>
public void set_address_Hight(addressH address)
{//恢复出厂，保存后，高地址为空
byte add = (byte)address;
set_region(0x2150, 0, add);//高位地址
set_Save();
}
public void PowerEN()// 使能电机
{
//address = id;// 重新绑定从站
try
{
set_region(0x6040, 0x00, 0x06);// 脱机
set_region(0x6040, 0x00, 0x07);// 准备使能
//Help_Delay.delayTime(5);
set_region(0x6040, 0x00, 0x0F);// 进入使能（上升沿 触发）
}
catch (Exception ex)
{
throw ex;
}
}
public void PowerOFF()// 使能电机
{
set_region(0x6040, 0x00, 0x06);// 脱机
}
#endregion
#endregion
#region 后台方法
/// <summary>
/// 制作Code编码 1 4 8
/// </summary>
/// <param name="hend"></param>
/// <param name="id"></param>
/// <param name="byte8"></param>
/// <returns></returns>
string madeCode(int hend, int id, Int64 byte8)// 1表示帧类型，4字节是帧id，8表示帧数据包
{
string data8 = string.Empty;
for (int i = 0; i < 8; i++)
{
data8 += ((byte8 >> (i * 8)) & 0xFF).ToString("X2");
}
// 单帧
string cmd = hend.ToString("X2") + id.ToString("X8") + data8;// 1 4 8 配置数据单帧
byte[] buffer = helpstring.StringsToHexBytes(cmd);// 单帧 13字节
string data = helpstring.BytesToHexStrings(buffer);// 等效编码
return data;
}
/// <summary>
/// 6041h状态
/// </summary>
/// <param name="state"></param>
/// <returns></returns>
public string get_6041h(byte byte0, byte byte1)
{
string str0 = Convert.ToString(byte0, 16);
string str1 = Convert.ToString(byte1, 16);
int state = Convert.ToInt32(str1 + str0, 16);
string end = string.Empty;
switch (state & 0x6F)
{
case 0x00: end = "未准备启动 "; break;
case 0x40: end = "取消启动 "; break;
case 0x21: end = "准备启动 "; break;
case 0x23: end = "启动 "; break;
case 0x27: end = "允许操作 "; break;
case 0x07: end = "快速停止激活 "; break;
case 0x0F: end = "故障效应激活 "; break;
case 0x08: end = "故障 "; break;
default:
break;
}
return end;
}
//private void delayTime(double secend)
//{
// DateTime tempTime = DateTime.Now;
// while (tempTime.AddSeconds(secend).CompareTo(DateTime.Now) > 0)
// Application.DoEvents();
//}
Int32 get_result(List<string> getend)// "46s8821：08000005814B002000E8030000"
{//回帧结果
string str = string.Empty;
string obj = string.Empty;//寄存器标记
for (int i = 0; i < getend.Count; i++)
{
string str1 = getend[i];// 提取
if (str1.Length < 26) throw new Exception("长度不够 ");//长度不够
string str2 = str1.Substring(str1.Length - 16); // 截取 8字节
obj = str2.Substring(0, 2);// 寄存器 类型
// 1111 1011 0111 0011
if (obj == "4F" | obj == "4B" | obj == "47" | obj == "43" | obj == "80")// 1字节，2字节，3字节，4字节，错误码
{
Region_Type = Convert.ToByte(obj, 16);// 标记寄存器 类型
//if (obj == "4F") Region = 8;
//if (obj == "4B") Region = 16;
//if (obj == "47") Region = 24;
//if (obj == "43") Region = 32;
string str3 = str1.Substring(str1.Length - 8);
for (int j = 0; j < 4; j++)
{
string num = str3.Substring(str3.Length - (2 * j) - 2, 2);
str += num;
}
return Convert.ToInt32(str, 16);// 成功
}
//下一轮数据帧
}
if (obj == "60") Region_Type = 0x60; return 0;// 写成功
//return -1;// 读取失败
}
Int32 get_result(byte[] getend)// "46s8821：08000005814B002000E8030000"
{//回帧结果
return getend[9] + (getend[10] << 8) + (getend[11] << 16) + (getend[12] << 24);
}
void Log_ol(byte errcode, short reg, int Son)
{
// 日志====================================
if (Region_Type < 0x43 | Region_Type > 0x4F)// 读取失败
{
string error = getError(errcode, reg, Son, 0x40); // 故障码，寄存器，子，值
if (Wt_get != null)
{
Wt_get(new byte[] { 0x00 }, error);
//throw new Exception(error);
}
}
}
//void wait_dataRX数据池()
//{
// do
// {
// if (rj45.rx数据池字节 != 0)
// {
// int i = 0;// 调试 断点
// i++;
// }
// if (rj45.rx数据池字节 == 0)
// {
// break;
// }
// } while (true);
//}
void throw_bytes()
{
Thread.Sleep(20);
if (rj45.socket.Available > 0)
{
byte[] bytes = new byte[rj45.socket.Available];
int count = rj45.socket.Receive(bytes, bytes.Length, SocketFlags.None);
}
}
#endregion
#region 模块方法
public enum regionSET : byte
{
写b8 = 0x2F,
写b16 = 0x2B,
写b24 = 0x27,
写b32 = 0x23,
读 = 0x40, // 成功0x60
}
enum regionGET : byte
{
读 = 0x40,
b8 = 0x4f,
b16 = 0x4b,
b24 = 0x47,
b32 = 0x43
}
public enum NMTe : byte
{
启动从站 = 0x01,
停止从站 = 0x02,
预运行从站 = 0x80,
重启从站 = 0x81,
通讯重连 = 0x82
}
public enum NMTsalve : byte
{
心跳 = 0x00,
未连接 = 0x01,
连接 = 0x02,
预备 = 0x03,
从站停止 = 0x04,
从站运行 = 0x05,
从站预操作 = 0x7F // PDO禁用状态
}
public enum POWER : byte
{
// b7 ↑故障复位
// 0x06 关闭
// 0x07 打开
// 0x0F 打开+启用操作
// 0<<1 快速停止
// 0x07 禁用操作
// 0x0F 启用操作
完全关闭 = 0x06,
快速停止 = 0x00,
禁用操作 = 0x07,
启用操作 = 0x0F //要先0x06完全关闭
}
public enum PDOtype : byte
{
PDO1 = 1,
PDO2,
PDO3,
PDO4,
}
public enum GoHome : byte
{//6098
方法17 = 17,
方法18 = 18,
方法19 = 19,
方法20 = 20,
方法21 = 21,
方法22 = 22,
方法23 = 23,
方法24 = 24,
方法25 = 25,
方法26 = 26,
方法27 = 27,
方法28 = 28,
方法29 = 29,
方法30 = 30,
方法31 = 31,
方法32 = 32,
方法33 = 33,
方法34 = 34,
方法35 = 35,
零点直设 = 37
}
public enum cia402 : ushort// 16bit
{
故障码 = 0x603F,
控制字 = 0x6040,// 油门，刹车 06h关闭》07h伺服使能（命令关）》0Fh命令生效 00h急停
// 0 启动 1伺服开，0伺服关
// 1 电压输出 1电压输出，0关
// 2 急停 1不报警
// 3 命令（上升沿） 01命令上升沿生效 07h》0Fh
//======================================
// 4 运动指令（上升沿） 该位由0跳变为1将触发新的位置指令
// 5 0等待旧命令位置完成 1立刻更新并执行新的位置指令动作
// 6 0绝对位置 1相对位置
// 7 复位 01上升沿，故障复位
// 8 1暂停 0继续运行
// 9 速度 1使用上次的旧速度
// 10~15 空
状态字 = 0x6041,// 汽车仪表（状态）
// 0
// 1
// 2
// 3
// 4
// 5
// 6
// 7
// 8 0： 目标位置到达 （bit10为1）
// 9
// 10 1
// 11
// 12 当前位置正在缓存， 新的位置将被接受
// 13 跟随错误
// 14
// 15
快停 = 0x605A,
//定位模式
工作模式 = 0x6060, // 01h定位模式
目标位置 = 0x607A,
额定速度 = 0x6081,// 2w
加速度 = 0x6083,// 5w
减速度 = 0x6084,// 50000
// 绝对0x6040=1Fh 停 0Fh
// 相对0x6040=5Fh 停 4Fh
//速度模式
目标速度 = 0x60FF,
速度反馈 = 0x606C,
//力矩模式
//回零模式
回零方式 = 0x6098,
回零速度 = 0x6099,//-01回零高速，-02回零低速
回零加速 = 0x609A,
零点偏置 = 0x607C,
//0x603F-错误代码UINT16 RO YES
//0x6040-控制字UINT16 WO是
//0x6041-状态剑UINT16 RO是
//0x605A-快速停止选项代码INT16 RW NO
//0x6060-操作模式INT8 WO是
//0x6061-操作模式显示INT8 RO YES
//0x6064-位置实际值INT32 RO是
//0x6065-以下错误窗口UINT32 RW NO
//0x606C-速度实际值INT32 RO是
//0x6071-目标扭矩INT16 RW是
//0x6073-最大电流UINT16 RW是
//0x6074-扭矩需求INT16 RO是
//0x6078-当前实际值INT16 RO是
//0x607A-目标位置INT32 RW是
//0x607C-起始偏移INT32 RW是
//0x607E-极性UINT8 RW是
//0x607F-最大剖面速度UINT32 RW是
//0x6081-剖面速度UINT32 RW是
//0x6083-配置文件加速UINT32 RW是
//0x6084-轮廓减速UINT32 RW是
//0x6085-快速停止减速UINT32 RW是
//0x6087-扭矩斜率UINT32 RW是
//0x6098-归位方法INT8 RW是
//0x6099 2归位速度阵列-否
//0x609A-归位加速度UINT32 RW是
//0x60F4-跟随错误实际值INT32 RO YES
//0x60FD-数字输入UINT32 RW是
//0x60FE 2数字输出阵列-否
//0x60FF-目标速度INT32 RW是
//0x6502-支持的驱动器模式UINT32 RO N
//0x603F - Error code UINT16 RO YES
//0x6040 - Controlword UINT16 WO YES
//0x6041 - Statusword UINT16 RO YES
//0x605A - Quick stop option code INT16 RW NO
//0x6060 - Modes of operation INT8 WO YES
//0x6061 - Modes of operation display INT8 RO YES
//0x6064 - Position actual value INT32 RO YES
//0x6065 - Following error window UINT32 RW NO
//0x606C - Velocity actual value INT32 RO YES
//0x6071 - Target torque INT16 RW YES
//0x6073 - Max current UINT16 RW YES
//0x6074 - Torque demand INT16 RO YES
//0x6078 - Current actual value INT16 RO YES
//0x607A - Target position INT32 RW YES
//0x607C - Home offset INT32 RW YES
//0x607E - Polarity UINT8 RW YES
//0x607F - Max profile velocity UINT32 RW YES
//0x6081 - Profile velocity UINT32 RW YES
//0x6083 - Profile acceleration UINT32 RW YES
//0x6084 - Profile deceleration UINT32 RW YES
//0x6085 - Quick stop deceleration UINT32 RW YES
//0x6087 - Torque slope UINT32 RW YES
//0x6098 - Homing method INT8 RW YES
//0x6099 2 Homing speed ARRAY - NO
//0x609A - Homing acceleration UINT32 RW YES
//0x60F4 - Following error actual value INT32 RO YES
//0x60FD - Digital inputs UINT32 RW YES
//0x60FE 2 Digital outputs ARRAY - NO
//0x60FF - Target velocity INT32 RW YES
//0x6502 - Supported drive modes UINT32 RO N
}
public enum addressH : byte
{
add_0 = 0,
add_32 = 1,
add_64 = 2,
add_96 = 3,// 96+31=127
}
public class PDOregion
{
public int num;
public int[] regName { get; set; }//PDO待绑定寄存器
public byte[] regBit { get; set; }// 寄存器bit长度
}
public enum CMDtype
{
回零,
绝对定位,
相对定位,
补间,
速度,
力矩,
复位,// bit7
暂停,// bit8
}
#endregion
#region 委托
#endregion
#region 窗体移动
//private Point mouseOff;//鼠标移动位置变量
//private bool leftFlag;//标签是否为左键
//private void Frm_MouseDown(object sender, MouseEventArgs e)//鼠标按下
//{
// if (e.Button == MouseButtons.Left)
// {
// mouseOff = new Point(-e.X, -e.Y); //得到变量的值
// leftFlag = true; //点击左键按下时标注为true;
// }
//}
//private void Frm_MouseMove(object sender, MouseEventArgs e)//鼠标移动
//{
// if (leftFlag)
// {
// Point mouseSet = Control.MousePosition;
// mouseSet.Offset(mouseOff.X, mouseOff.Y); //设置移动后的位置
// //Location = mouseSet;//Form 窗体父类字段
// }
//}
//private void Frm_MouseUp(object sender, MouseEventArgs e)//鼠标松开
//{
// if (leftFlag)
// {
// leftFlag = false;//释放鼠标后标注为false;
// }
//}
#endregion
#region 错误
//try
//{
// byte[] buffer = Encoding.Default.GetBytes(data);//以计算机的编码发送
// serialPort.Write(buffer, 0, buffer.Length);// 开始发送
//}
//catch (Exception ex) { MessageBox.Show(ex.Message); }//显示错误
//================================================
// 读取回帧-------0x40 结果
//int WaitCount = 0;//接收超次数
//DateTime startTime = DateTime.Now;//记录开始时间
// 4 等待接收
#region MyRegion
//while (true)
//{// 08 00000081 20811104 00000000
// // 超时
// if (timeOutFun)
// {
// if ((DateTime.Now - startTime).TotalMilliseconds > 300)// 超时
// {
// Region_Type = 0x80;// 标记 读取失败
// throw new Exception("(超时)读寄存器失败！"); // 超时 毫秒
// }
// }// 超时
// rx13 = Receive();//回帧报文
// if (rx13 == null)
// {
// ++WaitCount;
// if (WaitCount > 20)// 超次数
// {
// rx13 = new byte[13];
// rx13[5] = 0x80;// 故障码
// break;
// }
// continue;
// }// 超次
#endregion
#endregion
#region 接口方法
#endregion
#region 参考
//6040
//0 Switch on 转换开
//1 Enable voltage电压开
//2 Quick Stop快停
//3 Enable operation 启用操作
//4 pp新设定点（正触发） hm回零操作开始（正触发）
//5 pp立即更改集合
//6 pp绝对（0）/相对（1）
//7 Fault reset 故障复位
//8 Halt 暂停 hm回零停止运行（0x10F）
//6041 上电50》启动250》06码231》07码233》0F码c237
//0准备打开=========================7
//1打开
//2操作已启用（伺服开启状态）
//3故障（驱动器将伺服关闭）
// 4电压==========================3
// 5快速停止
// 6关闭
// 7警告（驱动器仍然打开伺服）
//8非正常停止===========================2
//9远程控制
//10位置到达（到位置1）
//11内部位置超限（不支持）
//12 pp设定点 承认 HM回原点完成 ipIP模式激活 pv零速
//13 pp以下错误 HM归零错误
//
//
//定位中 1237
//定位完成 1637
//
#endregion
}
}
更新24.3.13
using System;
using System.CodeDom.Compiler;
using System.Collections.Generic;
using System.ComponentModel;
using System.Diagnostics;
using System.IO;
using System.IO.Ports;
using System.Linq;
using System.Net.Sockets;
using System.Runtime.InteropServices;
using System.Security.Policy;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.Xml.Linq;
//====================================
using help; //【文本转换】
using static CAN_namespace.Help_CAN_ZHCX;// 创芯CAN 【缓冲区2000帧】 珠海创芯科技
//using static CAN_namespace.Help_CAN_SYGC;// 广成CAN 【5帧，一坨屎】 沈阳广成
using static System.Net.Mime.MediaTypeNames;
//===============【使用方法】标准PLCopen方法
//CTRL + M, CTRL + O折叠代码定义
//CTRL + M, CTRL + L展开代码定义
#region Cia定义
// canopen协议： https://blog.csdn.net/cfqq1989/article/details/128707894
//指数(十六进制) 目标
//0000 未使用
//0001 - 025F 数据类型
//0260 - 0FFF 保留供进一步使用
//1000-1FFF 》通信参数 区域 CiA301（DS301）总线命令HMT，SDO，PDO配置
//2000-5FFF 》设备参数（按伺服品牌厂家生产制造商特定）
//6000-9FFF 》标准化，运动控制 区域 CiA402（DSP402）
//A000-AFFF 网络可变区域
//B000-BFFF 系统可变区
//C000-CFFF 保留供进一步使用
//————————————————
//版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
//原文链接：https://blog.csdn.net/cfqq1989/article/details/128308808
#endregion
#region Cia301_通讯层
//1000h 硬件类型(硬件识别码0x40192) 04020192 h : A2 Series 06020192h : M Series
//1001h 错误寄存器4组
//1003h 预定义错误4组 （-01：58120）
//1005h COB-ID SYNC码 0x80（同步码）
//1006h 通信周期0
//100Ch 保护时间0
//100Dh 寿命因数0
//1010h 参数保存4组（0x1010_0x04=0x65766173;回帧大于2秒）
//1011h 恢复默认参数4组（0x1011_4=0x64616F6C;不要保存，只要断电重启）
//1014h COB-ID EMCY（0xFF）急停码
//1016h 消费者心跳时间-1
//1017h 发生器心跳时间 2000ms
//1018h 标识对象4
//1029h 阵列错误行为-1
//1200h 第一台服务器SDO参数SDO参数RO（2）
//1400h~03h 接收rPDO控制参数（开关配置）bit31总开关（0开）bit30RTR（0开）bit29标准帧（0标准帧1扩展帧）bit10~0 COB-ID码（帧id：11bit）
//1600h~03h 接收rPDO映射（寄存器指针）
//1800h~03h 发送tPDO控制参数（开关配置）
//1A00h~03h 发送tPDO映射（寄存器指针）
#endregion
#region Cia402_运动层
//603Fh 最后错误代码-1
//6040h 控制字==========【0】【6】【7】【F】
//6041h 状态字==========【0x250】
//605Ah 快速停止代码
//605Bh 停机选项代码-1
//605Eh 故障反应选项代码-1
//6060h 工作模式 【1位置 3速度 4扭矩 6回零 7补间 8Ecat:CSP】================
//6061h 驱动器当前工作模式【1位置 3速度 4扭矩 6回零 7补间 8Ecat:CSP】
//6062h 显示电机命令位置
//6063h 位置实际值-1
//6064h 显示电机实际位置0
//6065h 以下错误窗口-1
//6067h 位置窗口-1
//6068h 位置窗口时间-1
//606Bh 显示电机命令速度0
//606Ch 显示电机的实际速度0
//606Dh 速度窗口-1
//606Eh 速度窗口时间-1
//606Fh 速度阈值-1
//6071h 目标扭矩-1
//6074h 扭矩需求值-1
//6075h 电机额定电流-1
//6076h 电机额定扭矩-1
//6077h 扭矩实际值-1
//6078h 当前实际值-1
//607Ah 目标位置0
//607Ch 原点偏移0
//607Dh 软件位置限制-1
//607Fh 最大剖面速度-1
//6080h 最大电机转速-1
//6081h 位置模式的最大速度（每秒脉冲量）除以每圈脉冲 （每圈默认5万）
//6082h 起跳速度
//6083h 加速度【单圈脉冲量】乘以【速度/秒】
//6084h 减速度【单圈脉冲量】乘以【速度/秒】
//6085h 急停减速度（默认5000 0000
//6086h 运动剖面类型-1
//6087h 扭矩斜率-1
//6093h位置系数2
//6098h回零方式-------35和37是当前位置设零点
//6099h-01原点快速
//6099h-02原点慢速
//609Ah 回零加减速 5000000
//60B0h位置偏移-1
//60B1h速度偏移-1
//60B2h扭矩偏移-1
//60C0h插值子模式选择-1
//60C1h插值数据记录-1
//60C2h插值时间段-1
//60C5h最大加速度-1
//60C6h最大减速度-1
//60F2h定位选项代码-1
//60F4h跟随误差实际值0
//60FCh位置需求值-1
//set_region(0x2154, 0, 1);// 输入io电平逻辑 0正逻辑1反
//60FDh 输入 IO 状态0（bit0：负限位 bit1：正限位 bit2：原点信号 bit16：快速停止
//60FEh-01 物理输出开启1
//60FEh-02 物理输出开启2
//60FFh目标速度0
//6502h支持的驱动模式-1
#endregion
#region 类型
// AXIS_REF_HC_CO 伺服can轴类型 【禾川CAN轴】
#endregion
namespace CAN_namespace
{
public class Help_CANopen
{//作者qq750273008 祁成 更新日期：24/3/13 2023.11.28
// CANopen【指令说明】
//NMT总线命令、SYNC同步、SDO读写、PDO批量读写、EMCY故障
//NMT(NMT总线命令，从站id)； //控制从站失效和上线
//SYNC（）；同步命令// 用于PDO命令生效
//SDO（）；读写寄存器// 每次读写单个寄存器
//PDO（）；批量读写寄存器//一次可读写64bit长度寄存器组
#region 全局
//1申明委托》委托命令
public delegate void WT_GET_Data(byte[] butes, string data);//↑ui显示发送命令
public delegate void WT_SET_Data(byte[] butes, string data);//↓下执行对象
#region CanOpen【数据类型】:全局
//public unsafe struct VCI_CAN_OBJ //CAN数据帧 【单帧对象】
//{
// public uint ID;// 帧ID。 【11bit】数据右对齐。 详情请参照： 《8.附件1： ID对齐方式.pdf》说明文档。
// public uint TimeStamp; //设备接收到某一帧的时间标识。 时间标示从CAN卡上电开始计时，计时单位为0.1ms。
// public byte TimeFlag; //时间标识，为1时TimeStamp有效
// public byte SendType; //发送帧类型。=0时为正常发送（发送失败会自动重发，重发超时时间为4秒， 4秒内没有发出则取消）；=1时为单次发送
// public byte RemoteFlag; //是否是远程帧【RTR】。 =0时为数据帧， =1时为远程帧（数据段空）。
// public byte ExternFlag; //是否是扩展帧。 =0时为标准帧（11位ID）， =1时为扩展帧（29位ID）。
// public byte DataLen; //数据长度 DLC (<=8)，即CAN帧Data有几个字节。约束了后面Data[8]中的有效字节。
// public fixed byte Data[8]; //如DataLen定义为3，即Data[0]、 Data[1]、 Data[2]是有效的。
// public fixed byte Reserved[3];//系统保留。
//}
public enum Region_Type : byte
{
读 = 0x40, // 回帧0x60成功
成功 = 0x60,
失败 = 0x80,
写b8 = 0x2F,
写b16 = 0x2B,
写b24 = 0x27,
写b32 = 0x23,
//寄存器bit
b8 = 0x4F,//回帧：4F 寄存器有效长度 8bit
b16 = 0x4B,
b24 = 0x47,
b32 = 0x43
}//【寄存器bit】
public enum NMT_CMD : byte
{//NMT总线命令；重连，重启，停止，预备，启动
/// <summary>
/// 【只有工作模式后，才能定位，回零，速度】
/// </summary>
从站工作 = 0x01,//回帧 05 PDO有效（可以运动控制）
从站停止 = 0x02,//回帧 04 运动命令失效【PDO命令失效】
/// <summary>
/// 【伺服待机，配参】
/// </summary>
从站预操作 = 0x80, //回帧 0F SDO命令有效，PDO命令失效（用于伺服配置参数阶段）【伺服配参】
从站重启 = 0x81, //回帧 0F SDO命令有效，PDO命令失效
从站通讯重连 = 0x82,//回帧 0F SDO命令有效，PDO命令失效
}//【总线，命令】
public enum Status_Slave : byte
{//从站状态
正在初始化 = 0x00,
未连接 = 0x01,
连接 = 0x02,
预备 = 0x03,
停止 = 0x04, //停止状态，NMT有效
运行 = 0x05, //PDO有效
预操作 = 0x7F //SDO有效， PDO失效 // 伺服配置参数状态
}//【从站，状态】
public enum PDO_Type
{
PDO1cmd = 1, // 寄存器呼叫
PDO2cmd,
PDO3cmd,
PDO4cmd,
配置rPDO1,// 上位机到终端 数据帧
配置rPDO2,
配置rPDO3,
配置rPDO4,
配置tPDO1,// 回帧 （终端到上位机）
配置tPDO2,
配置tPDO3,
配置tPDO4,
}
public struct PDO_Slave
{
public byte regUSE;
public Int32[] regNO;
public Int32[] regValue;
}
public struct AXIS_OBJ
{
public bool isError;
public Int64 Error;
public byte h700;
public PDO_Slave PDO1;
public PDO_Slave PDO2;
public PDO_Slave PDO3;
public PDO_Slave PDO4;
public Int64 PDO1_Int64;
public Int64 PDO2_Int64;
public Int64 PDO3_Int64;
public Int64 PDO4_Int64;
}
public struct AXIS_OBJ3
{
public byte h700;
public Int64 PDO1_Int64;
public int[][] reg;// PDO1_reg1;
public int[] PDO_reg_num;//
public int PDO1_num;
public int PDO1_reg1;
public int PDO1_reg2;
public int PDO1_reg3;
public int PDO1_reg4;
public int PDO1_reg5;
public int PDO1_reg6;
public int PDO1_reg7;
public int PDO1_reg8;
public int PDO1_reg1_value;
public int PDO1_reg2_value;
public int PDO1_reg3_value;
public int PDO1_reg4_value;
public int PDO1_reg5_value;
public int PDO1_reg6_value;
public int PDO1_reg7_value;
public int PDO1_reg8_value;
public Int64 PDO2_Int64;
public int PDO2_num;
public int PDO2_reg1;
public int PDO2_reg2;
public int PDO2_reg3;
public int PDO2_reg4;
public int PDO2_reg5;
public int PDO2_reg6;
public int PDO2_reg7;
public int PDO2_reg8;
public int PDO2_reg1_value;
public int PDO2_reg2_value;
public int PDO2_reg3_value;
public int PDO2_reg4_value;
public int PDO2_reg5_value;
public int PDO2_reg6_value;
public int PDO2_reg7_value;
public int PDO2_reg8_value;
public Int64 PDO3_Int64;
public int PDO3_num;
public int PDO3_reg1;
public int PDO3_reg2;
public int PDO3_reg3;
public int PDO3_reg4;
public int PDO3_reg5;
public int PDO3_reg6;
public int PDO3_reg7;
public int PDO3_reg8;
public int PDO3_reg1_value;
public int PDO3_reg2_value;
public int PDO3_reg3_value;
public int PDO3_reg4_value;
public int PDO3_reg5_value;
public int PDO3_reg6_value;
public int PDO3_reg7_value;
public int PDO3_reg8_value;
public Int64 PDO4_Int64;
public int PDO4_num;
public int PDO4_reg1;
public int PDO4_reg2;
public int PDO4_reg3;
public int PDO4_reg4;
public int PDO4_reg5;
public int PDO4_reg6;
public int PDO4_reg7;
public int PDO4_reg8;
public int PDO4_reg1_value;
public int PDO4_reg2_value;
public int PDO4_reg3_value;
public int PDO4_reg4_value;
public int PDO4_reg5_value;
public int PDO4_reg6_value;
public int PDO4_reg7_value;
public int PDO4_reg8_value;
}
public struct PDO_Config_Struct // PDO的配置表
{ //从站id，PDO配置，指针表
//public Int32[] Regs;//=new UInt16[8];//= 0x60400010; //8张表
public Byte UseRegNum; // 寄存器指针使用数量
public Int32 Reg1; // 寄存器指针1
public Int32 Reg2; // 寄存器指针2
public Int32 Reg3;
public Int32 Reg4;
public Int32 Reg5;
public Int32 Reg6;
public Int32 Reg7;
public Int32 Reg8;
//======【写信】===================================================================
/// <summary>
/// 【0~240】在1个SYNC信号后，rPDO值才生效。【254或255】rPDO值立即生效。（不需要SYNC信号）
/// 【241~251】保留。【252~253】不受rPDO命令控制
/// </summary>
public Byte rPDO命令触发器; //【伺服接收命令】
/// <summary>
/// 命令死区时间【x0.1ms倍数】2个命令最小间隔
/// </summary>
public UInt16 rPDO禁止时间_100us;
//======【回信】===================================================================
/// <summary>
/// 【0】值有变化，1次sync后上传tPDO【1~240】1~240次SYNC后上传，【254或255】值变或定时到就上传
/// </summary>
public Byte tPDO上传触发器;//【伺服上传状态】
/// <summary>
/// 上传间隔时间【x0.1ms倍数】
/// </summary>
public UInt16 tPDO禁止时间_100us;
/// <summary>
/// 只在tPDO异步时生效【x1ms】（254，255）异步，【u16】定时上传tPDO帧，65535ms
/// </summary>
public UInt16 tPDO定时触发器_1ms;
}
#endregion 【数据类型】:全局
#endregion 全局
#region 字段
//private CancellationTokenSource cts;//线程令牌
//private ManualResetEvent resetEvent = new ManualResetEvent(true);// 暂停业务
Help_String helpstring;// 文本转换
static UInt32 address;//标记从站地址【1~127】
Int32 error = 0;// CAN总线故障标记
/// <summary>
/// 【1 4 8】字节
/// </summary>
static byte[] Rx13;// 1 4 8 //1：DLC 4：帧id 8：byte8
static Int64 Byte8;// 64bit 数据帧 包
static volatile byte Region_Obj;// 寄存器标记（读40h，写，成功60h，失败80h）参考 enum Region_Type
/// <summary>
/// 【忙碌状态】
/// </summary>
int busy;
volatile static AXIS_OBJ[] axis_obj;//【单例】 static 实例化不会创建副本，不会加速读写性能
#endregion 字段
#region 属性
/// <summary>
/// ↑：byte包，文本
/// </summary>
static public WT_GET_Data Wt_get;//↑event委托=》呼叫上ui层 让上层显示：发送命令
/// <summary>
/// ↓：byte包，文本
/// </summary>
static public WT_SET_Data Wt_set;//↓委托=》呼叫下位机执行
//===============================================================
public UInt32 Address { get => address; set => address = value; } // 从站1~127（不能为0，因为帧id=0是NMT广播）
public Int32 IsError { get => error; }
/// <summary>
/// 接口【收/发】
/// </summary>
public IFace_RTX Help_rtx { get; set; }// 接口【收/发】
/// <summary>
/// 忙碌状态
/// </summary>
public Int32 Busy { get => busy; }// 【/ˈbɪzi/】忙碌
/// <summary>
/// 现场总线刷新的对象
/// </summary>
public AXIS_OBJ[] GetAxis { get => axis_obj; }
#endregion 属性
#region 构造
/// <summary>
/// 【CAN】接口
/// </summary>
/// <param name="iface"></param>
public Help_CANopen(CAN_namespace.IFace_RTX iface) // usb转CAN
{
Help_rtx = iface;// 接口 Help_RTX
//================================
Rx13 = new byte[13];// 提取数据帧
Byte8 = 0; // 64bit，数据帧的【数据包】
Region_Obj = 0x80; // 寄存器标记（读40h，写，成功60h，失败80h）参考 enum Region_Type
busy = 0;
helpstring = new Help_String();// 文本转换
axis_obj = new AXIS_OBJ[128]; //127轴
for (int i = 0; i < 127; i++) // PDO4组 ，reg指针数量
{
axis_obj[i].PDO1 = new PDO_Slave();
axis_obj[i].PDO1.regNO=new int[9];
axis_obj[i].PDO1.regValue=new int[9];
axis_obj[i].PDO2 = new PDO_Slave();
axis_obj[i].PDO2.regNO = new int[9];
axis_obj[i].PDO2.regValue = new int[9];
axis_obj[i].PDO3 = new PDO_Slave();
axis_obj[i].PDO3.regNO = new int[9];
axis_obj[i].PDO3.regValue = new int[9];
axis_obj[i].PDO4 = new PDO_Slave();
axis_obj[i].PDO4.regNO = new int[9];
axis_obj[i].PDO4.regValue = new int[9];
}// axis_obj[address].PDO1.regNO[1] = obj.Reg1;
//===【调试】===================================
address = 3;// 默认 从站id
}
#endregion 构造
//NMT、SYNC、EMCY、TIME、PDO、SDO。
#region 方法_API 【 NMT总线命令、SYNC同步、SDO读写、EMCY故障、TIME时间广播、PDO批量读写。
#region 标准CAN总线指令
#region NMT
/// <summary>
/// NMT【can总线指令，从站id】0x00指所有从站
/// </summary>
/// <param name="Slave_id">从站id【0x00全部】 </param>
/// <param name="nMT_Type">通讯复位，重启，停止，配参0x80，工作0x01</param>
/// <returns></returns>
public void NMT(NMT_CMD nMT_CMD, int Slave_id)// 【总线命令】
{// 从站id，NMT总线命令
NMT(nMT_CMD, Slave_id, 0.075);// 【延时，秒】
}
public void NMT(NMT_CMD nMT_Type)// 【总线命令】
{// 从站id，NMT总线命令
NMT(nMT_Type, (byte)Address, 0.075);// 【延时，秒】
}
#endregion
#region SYNC
/// <summary>
/// 主站同步帧
/// </summary>
public void SYNC_Set_OutOne()//【单个同步帧】
{
SYNC(0.020);
}
/// <summary>
/// Set从站心跳时间ms【0x1017 = 0ms为关】
/// </summary>
/// <param name="Slave_id"></param>
/// <param name="type"></param>
/// <param name="time_ms"></param>
public void SYNC_Set_UpTime(int Slave_id, int time_ms)// 0ms为关
{// 0x1017
// SDO(0x1017,0x00,time_ms);
SYNC(Slave_id, time_ms);
}
#endregion
#region SDO【伺服参数配置】
/// <summary>
/// 【SDO】读_寄存器
/// </summary>
/// <param name="reg"></param>
/// <param name="son"></param>
/// <returns></returns>
public Int32 SDO(int reg, int son)//【读】
{
Int32 i32 = SDO(reg, son, 0.0);
return i32;
}
/// <summary>
/// 【SDO】写_寄存器
/// </summary>
/// <param name="reg"></param>
/// <param name="son"></param>
/// <param name="value"></param>
/// <returns></returns>
public bool SDO(int reg, int son, int value)//【写】
{// 重启伺服操作需要延时
bool sta = SDO(reg, son, value, 0.0);
return sta;
}
#endregion
#region PDO【批量命令】
/// <summary>
/// 从站，PDO组，配置表
/// </summary>
/// <param name="Slave_id"></param>
/// <param name="type"></param>
/// <param name="pdo"></param>
/// <returns></returns>
public bool PDO_Set_Config(PDO_Type type, PDO_Config_Struct pdo)
{
return PDO_Config(type, pdo);
}
public bool PDO_Set默认配置()
{
bool sta;
//===================
// 2批量PDO配置=========================
#region 写信
PDO_Config_Struct rPDO = new PDO_Config_Struct();
rPDO.rPDO命令触发器 = 240; //【0~240】收到1个SYNC信号，rPDO值才生效【255】rPDO值立即生效（不需要SYNC帧）
rPDO.rPDO禁止时间_100us = 10;// 1ms
rPDO.UseRegNum = 2;
rPDO.Reg1 = 0x60400010; //控制字u16 //bit64分配
rPDO.Reg2 = 0x607A0020; //目标位置u32
rPDO.Reg3 = 0;
rPDO.Reg4 = 0;
rPDO.Reg5 = 0;
rPDO.Reg6 = 0;
rPDO.Reg7 = 0;
rPDO.Reg8 = 0;
// 0x60400010, 0x607A0020
//PDO1
sta = PDO_Config(PDO_Type.配置rPDO1, rPDO);// 批量：命令帧
//PDO2
rPDO.UseRegNum = 0;
sta &= PDO_Config(PDO_Type.配置rPDO2, rPDO);// 关
//PDO3
sta &= PDO_Config(PDO_Type.配置rPDO3, rPDO);// 关
//PDO4
rPDO.UseRegNum = 1;
sta &= PDO_Config(PDO_Type.配置rPDO4, rPDO);// //调试用 PDO4 【6040】
#endregion
#region 回信
PDO_Config_Struct tPDO = new PDO_Config_Struct();
tPDO.tPDO上传触发器 = 7;
tPDO.tPDO禁止时间_100us = 5000;//500ms
tPDO.tPDO定时触发器_1ms = 0;
tPDO.UseRegNum = 2;
tPDO.Reg1 = 0x60410010; //状态字u16 //bit64分配
tPDO.Reg2 = 0x60640020; //电机位置u32
tPDO.Reg3 = 0;
tPDO.Reg4 = 0;
tPDO.Reg5 = 0;
tPDO.Reg6 = 0;
tPDO.Reg7 = 0;
tPDO.Reg8 = 0;
// 0x60410010, 0x60640020
//【状态】【位置】
//PDO1
sta &= PDO_Config(PDO_Type.配置tPDO1, tPDO);// 批量：回帧
//PDO2
tPDO.UseRegNum = 0;
sta &= PDO_Config(PDO_Type.配置tPDO2, tPDO);// 关
//PDO3
sta &= PDO_Config(PDO_Type.配置tPDO3, tPDO);// 关
//PDO4
//【定时tPDO】
tPDO.UseRegNum = 2;
tPDO.tPDO上传触发器 = 255;// 异步
tPDO.tPDO定时触发器_1ms = 5000;//5秒上传
sta &= PDO_Config(PDO_Type.配置tPDO4, tPDO);// 批量：回帧
#endregion
return sta;
}
/// <summary>
/// PDO命令：批量读写寄存器【必须在从站工作总线命令下】
/// 相对运动6F》7F，绝对2F》3F
/// </summary>
/// <param name="Slave_id"></param>
/// <param name="type"></param>
/// <param name="CMD"></param>
public void PDO(PDO_Type type, Int64 CMD)
{
switch (type)
{
case PDO_Type.PDO1cmd:
PDO1(CMD);
break;
case PDO_Type.PDO2cmd:
PDO2(CMD);
break;
case PDO_Type.PDO3cmd:
PDO3(CMD);
break;
case PDO_Type.PDO4cmd:
PDO4(CMD);
break;
default:
break;
}
}
#endregion
#region EMCY
public void EMCY_Out(int Slave_id, Int16 H603f, byte H1001, Int32 Motor)// 测试
{//下位机故障 80h+从站id
EMCY(Slave_id, H603f, H1001, Motor);// H603f记录在 h1003-01
}
#endregion
#region Time【48bit】
/// <summary>
/// 【6字节】
/// </summary>
/// <param name="time"></param>
public void TIME_Set_Time(Int64 time)
{// 固定6字节长度
if (time < 10000) TIME(time);
else
{
DateTime dateTime = DateTime.Now;// 系统时间
Int64 now = dateTime.ToBinary();
//string str = now.ToString("D-HH-MM-SS");//
TIME(now);
}
}
public DateTime GetTimer(byte[] data)
{
DateTime time = Convert.ToDateTime(data);
return time;
}
#endregion
#endregion
/// <summary>
/// 【1】所有【2】从1000h到1FFFh 【3】从6000h到9FFFh 【4】厂家
/// </summary>
/// <param name="Level4"></param>
/// <returns></returns>
public bool Set_Device_Reset(int Level4)//【设备恢复出厂】
{
const int key出厂 = 0x64616F6C;// key出厂 = 0x64616F6C;
//if (SDO(0x1011, d等级1_4级, key出厂, 3.0)) return true;
if (set_region(0x1011, Level4, key出厂, 3.0)) return true;
return false;
// 恢复：
// 0x1011 - 01 = 0x64616F6C;//【恢复所有参数】雷赛不支持
// 0x1011 - 02 = 0x64616F6C;//【恢复从1000h到1FFFh】雷赛，断电重启生效（不要执行保存）【禾川0x1011-02】
// 0x1011 - 03 = 0x64616F6C;//【恢复从6000h到9FFFh】雷赛不支持
// 0x1011 - 04 = 0x64616F6C;//【厂家自定义】雷赛，断电重启生效（不要执行保存）
//————————————————
//版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。
//原文链接：https://blog.csdn.net/cfqq1989/article/details/128707894
}
/// <summary>
/// 【1】所有【2】从1000h到1FFFh 【3】从6000h到9FFFh 【4】厂家
/// </summary>
/// <param name="Level4"></param>
/// <returns></returns>
public bool Set_Device_Save(int Level4)//【设备参数保存】
{
const int key保存 = 0x65766173; // key保存 = 0x65766173;
//if (SDO(0x1010, d等级1_4级, key保存, 3.0)) return true;
if (set_region(0x1010, Level4, key保存, 3.0)) return true;
return false;
// 对象1010h：存储参数【0x65766173】
//•子索引00h包含支持的最高子索引。
//•子索引01h是指可能存储在CANopen设备上的所有参数。
//•子索引02h是指与通信相关的参数（从1000h到1FFFh的索引）。
//•子索引03h是指与应用程序相关的参数（索引从6000h到9FFFh）。
//•从04h到7Fh的子索引制造商可以单独存储他们选择的参数。
//•保留80h至FEh的子索引以备将来使用。
//————————————————
//版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。
//原文链接：https://blog.csdn.net/cfqq1989/article/details/128707894
}
#endregion 标准CAN总线指令
//==以下不再重要===========================================================
#region 核心：延时
/// <summary>
/// Help_Delay.delayTime(0.123);//0.1秒
/// </summary>
/// <param name="secend"></param>
public void DelaySecond(double secend)// 延时，秒
{
DateTime tempTime = DateTime.Now;
while (tempTime.AddSeconds(secend).CompareTo(DateTime.Now) > 0)
{
//Application.DoEvents();
}
}
#endregion
#region 核心：收发GET_SET 【SDO】
unsafe public int Get_Receive(ref VCI_CAN_OBJ[] obj)
{
//===1 等待加载=================
DelaySecond(0.005);//5ms // USB速度3~5ms
//===2 接口接收=================
object obj2 = new object();//对象
int num = Help_rtx.RXD(ref obj2);// can.get_Receive(CAN_DeviceType, CAN_ind, CAN1, ref obj[0], 2000, 50);
if (num < 1) return 0;// 空退
//===3 取出帧，并且分发=========
obj = (VCI_CAN_OBJ[])obj2;
//===4 刷新从站状态
int i = 0;
foreach (var item in obj)
{
if (i >= num) break;
if (item.ID >= 0x81 && item.ID <= 0x1FF)// 回信【80h+】故障
{
axis_obj[item.ID - 0x80].isError = true;
axis_obj[item.ID - 0x80].Error = get_Value_Int64(item.Data);// 从站轴
}
if (item.ID >= 0x181 && item.ID <= 0x1FF)// 回信【180h+】
{ axis_obj[item.ID - 0x180].PDO1_Int64 = get_Value_Int64(item.Data); }// 从站轴
if (item.ID >= 0x281 && item.ID <= 0x2FF)// 回信【280h+】
{ axis_obj[item.ID - 0x280].PDO2_Int64 = get_Value_Int64(item.Data); }
if (item.ID >= 0x381 && item.ID <= 0x3FF)// 回信【380h+】
{ axis_obj[item.ID - 0x380].PDO3_Int64 = get_Value_Int64(item.Data); }
if (item.ID >= 0x481 && item.ID <= 0x4FF)// 回信【480h+】
{ axis_obj[item.ID - 0x480].PDO4_Int64 = get_Value_Int64(item.Data); }
if (item.ID >= 0x701 && item.ID <= 0x77F)// 回信【700h+】状态
{ axis_obj[item.ID - 0x700].h700 = (byte)get_Value_Int64(item.Data); }
i++;
}
//===5 界面显示=================
// 分发数据帧，UI层显示
//if (Wt_get != null)
//{// 数组是引用类型
//VCI_CAN_OBJ[] uiObj = new VCI_CAN_OBJ[num];
//uiObj = obj;
//UIshow(num, uiObj);
//COB - ID Byte 0:1 Byte 2 Byte 3:7
//0x080 + Node - ID 应急错误代码(603FH) 错误寄存器(1001H) 厂商指定区域
//byte0~1： 8120 【0x603F】
// 应急错误代码
// 0000H 无错误
// 8110H CAN 溢出
// 8120H 错误被动模式（0411）
// 8130H 寿命保护/ 心跳错误
// 8140H 被迫离线恢复故障
// 8141H 被迫离线
// 8150H 发送 COB - ID 冲突
// 8210H PDO 长度错误未处理
// 8220H PDO 超过长度
//byte2： 11
// (1001H)位定义
// Bit0：generic error 一般错误
// Bit1：current 电流
// Bit2：voltage 电压 （04）
// Bit3：temperature 温度
// Bit4：通讯
// Bit5：协议402错误
// Bit6：保留
// Bit7：伺服厂家
//————————————————
//版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。
//原文链接：https://blog.csdn.net/cfqq1989/article/details/128308808
//}// EMCY：故障（从站故障）80h + 从站
//if (item.ID > 0x700 && item.ID < (0x700 + 127)) //从站状态
//{
// if (Wt_get != null)
// {
// NMTslave slave = (NMTslave)p[0];
// string sta = Enum.GetName(typeof(NMTslave), slave);
// //string str = $"从站：{item.ID - 0x700}状态{p[0].ToString("X2")}； ";
// string str = $"从站：{item.ID - 0x700}_{sta}； ";
// Wt_get(null, str);
// }
//}
return num;
}
/// <summary>
/// 【1 4 8】DLC长度，帧id，数据包64bit【小端】
/// </summary>
/// <param name="DLC"></param>
/// <param name="CANid"></param>
/// <param name="byte8"></param>
/// <returns></returns>
/// <exception cref="Exception"></exception>
public bool Set_Send(Byte DLC, UInt32 CANid, Int64 byte8)// 1 4 8
{
//int hand = 0x88;// 设备：USR-CANET200
//int cobid = 0x12345678;// 11bit或29bit
//Int64 byte8 = 0x0011223344556677;
//wait_dataRX数据池();
//can.send(hand8, cobid, byte8);
//wait_dataRX数据池();
if (DLC > 8) DLC = 8;// <=8
string cmd = madeCode((byte)DLC, (UInt32)CANid, byte8);// Code编码 "08 00000602 2B 5021 00 00000000"
if (Help_rtx.TXD(cmd) == 1)// 接口功能 "08000006022B50210000000000" // 单帧 13字节( 1 4 8 在接口解码发送)
{
return true;
}
else throw new Exception("发送失败。检查主站硬件状态（是否开启？）！");
//return false;
}
/// <summary>
/// [-1层] SDO指令
/// </summary>
/// <param name="regtype"></param>
/// <param name="reg"></param>
/// <param name="son"></param>
/// <param name="value"></param>
void SDO(Region_Type regtype, UInt16 reg, Byte son, Int32 value)// 1操作类型，2寄存器编号，3子索引，4参数值
{// SDO操作 600h+
//========================================================
// 准备发送
Byte by0 = (Byte)regtype;//1 操作码【u8】
UInt16 by1 = (UInt16)reg;//2 寄存器【u16】
Byte by2 = (Byte)son;//3 子索引【u8】
Int64 by3 = (Int32)value;//4设定值【u32】
// b0 b1b2 b3 b4b5b6b7
Byte8 = by0 + (by1 << 8) + (by2 << 24) + (by3 << 32); // 23 2000 00 11223344
Set_Send(0x08, 0x600 + Address, Byte8);// 40 2000 00 11223344
}
/// <summary>
/// [0层]【读】寄存器
/// </summary>
/// <param name="reg"></param>
/// <param name="son"></param>
/// <param name="WaitTime_S"></param>
/// <returns></returns>
unsafe public Int32 SDO(UInt16 reg, Byte son, double WaitTime_S)// 【读】寄存器，子索引【0x0000~0xFFFF】
{// 读
busy = -1;// 忙
//===1 get_数据类型=============
SDO(Region_Type.读, reg, son, 0);//读
//===2 get_帧===================
#region get_Value
VCI_CAN_OBJ[] obj = new VCI_CAN_OBJ[2000];// 创芯科技CAN分析仪最大支持2000帧接收，广成科技缓存10239帧
DelaySecond(WaitTime_S);//10ms // USB速度3~5ms
int num = Get_Receive(ref obj); // 【读取数据帧】
busy = +1;
if (num > 0)// 数据帧
{
for (int i = num - 1; i >= 0; i--)
{
if (obj[i].ID == 0x580 + Address && obj[i].Data[0] != 0x60)// SDO 600h+的回信【580h+】
{// 580h+从站地址 回帧
//=====580h 寄存器类型====================
Rx13[5] = obj[i].Data[0];// 2001.00=50000； // 4B，01 20，00, 50 C3 00 00 //
// u16寄存器
Rx13[6] = obj[i].Data[1];// 寄存器4B（b16位） 2001-00= 0x 0000C350
Rx13[7] = obj[i].Data[2];
// u8子索引
Rx13[8] = obj[i].Data[3];
// int32数据值
Rx13[9] = obj[i].Data[4];// 小端
Rx13[10] = obj[i].Data[5];
Rx13[11] = obj[i].Data[6];
Rx13[12] = obj[i].Data[7];
//=====2======================
//VCI_CAN_OBJ one = obj[i]; //
break;
}// SDO应答帧(580h+)
}
}
else
{
Region_Obj = 0x80;// 故障
return -1;
}
// 回帧结果
Region_Obj = Rx13[5];// 标记： b8 = 0x4F, b16 = 0x4B, b24 = 0x47, b32 = 0x43 //0x6040是16bit
//===3 get_Value===================
int value = get_Value_Int32(Rx13);// 解析 43,4060,00,0000 0000
// 日志====================================
if (Region_Obj < 0x43 | Region_Obj > 0x4F)// u8~32 读取失败 // 参考 enum region_Type //| Region_Type>(int)region_Type.b32
{// 0x05030000 80
string error = get_Error_str(reg, son, value); // 故障码，寄存器，子，值
if (Wt_get != null)
{
Wt_get(new byte[] { 0x00 }, "读取寄存器错误：" + error);
}
// throw new Exception("读取错误：" + error);// 提前终止
}//错误
else
{
//成功
return value;
}
#endregion
return -1;
}
/// <summary>
/// [0层]【写】寄存器
/// </summary>
/// <param name="reg"></param>
/// <param name="son"></param>
/// <param name="value"></param>
/// <param name="WaitTime_S"></param>
/// <returns></returns>
unsafe public bool SDO(UInt16 reg, Byte son, Int32 value, double WaitTime_S)//【写】
{// 写
busy = -1;
//===1 get寄存器【数据类型，32/24/16/8bit】=============
Int32 data = SDO(reg, son);//读
//===2 set_Value【设置值】===================
if (Region_Obj >= 0x43 || Region_Obj <= 0x4F)
{
SDO((Region_Type)Region_Obj - 0x20, reg, son, value);
}
else { return false; }// 【写】失败
//===3 get_帧【确认结果】===================
#region get_Value
VCI_CAN_OBJ[] obj = new VCI_CAN_OBJ[2000];// 创芯科技CAN分析仪最大支持2000帧接收，广成10239帧
DelaySecond(WaitTime_S);//10ms // USB速度3~5ms
int num = Get_Receive(ref obj); // 【读取数据帧】
busy = +1;
if (num > 0)// 数据帧
{
for (int i = num - 1; i >= 0; i--)// 【倒查数据帧】
{
int end_sta = obj[i].Data[0];// 0x80是失败
if (obj[i].ID == 0x580 + Address && obj[i].Data[0] == 0x60)// SDO 600h+的回信【580h+】
{// 580h+从站地址 回帧
return true;
}// SDO应答帧(580h+)
}
}
#endregion
Region_Obj = 0x80;// 故障
return false;
}
unsafe void UIshow(int num, VCI_CAN_OBJ[] obj)
{
//====3界面显示================================================================
// 分发数据帧，UI层显示
int len = 0;
foreach (var item in obj)
{
if (len++ >= num)
{
return;
}
byte* p = item.Data;// 8字节数据包
byte b0 = p[0];// 4f 4f 1400.02=05
byte b1 = p[1];// 00
byte b2 = p[2];// 14
byte b3 = p[3];// 02
byte b4 = p[4];// 05
byte b5 = p[5];
byte b6 = p[6];
byte b7 = p[7];
// 40 00 02 02 01 14 00 43
//===每帧==============
if (item.ID > 0x80 && item.ID < 0x80 + 128)// EMCY故障【从站故障】 // 要忽略0站
{
string str1 = $"【{item.DataLen:X2}】故障从站：{(item.ID - 0x80).ToString("X4")}={b7:X2}{b6:X2}{b5:X2}{b4:X2}{b3:X2}【{b2:X2}】【{b1:X2}{b0:X2}】；";
Wt_get(null, str1);
}
if (item.ID > 0x580 - 1 && item.ID < 0x580 + 128)// 0~127站SDO回帧
{
string reg = $"%{b0:X2}:{b2:X2}{b1:X2}-{p[3]}=0x"; // %43:6064-00=0x
Wt_get(null, $"【{item.DataLen:X2}】寄存器{reg}{b7:X2}{b6:X2}{b5:X2}{b4:X2}；");
continue;
}// SDO回帧
// 1 4 8
string str = item.DataLen.ToString("X2") + "/" + item.ID.ToString("X4") + "/"
+ $"{b7:X2}{b6:X2}{b5:X2}{b4:X2}{b3:X2}{b2:X2}{b1:X2}{b0:X2}；";
Wt_get(null, str);
}
}
#endregion
#region 核心：读写寄存器【region】
public Int32 get_region(int reg, int Son)
{
return SDO(reg, Son);
}
public Int32 get_region(int reg, int Son, double WaitTime_S)
{
return SDO(reg, Son, WaitTime_S);
}
public bool get_region(int reg, int Son, ref int data)
{
data = SDO(reg, Son);
if (Region_Obj > 0x43 | Region_Obj < 0x4F) return true;
return false;
}
/// <summary>
/// 【读参数】适合台达
/// </summary>
/// <param name="PxXXX"></param>
/// <param name="data值"></param>
/// <returns></returns>
public int get_region(string PxXXX, Int32 data值) // "P1.45"
{// 0x2000 + Pn + 常数
//寄存器地址
UInt16 reg_address = get_reg_获取寄存器地址(PxXXX);
return get_region(reg_address, 0x00);
}
//=========================================================================
public bool set_region(int reg, int Son, int data)
{
return SDO(reg, Son, data, 0.0);
}
public bool set_region(int reg, int Son, int data, double WaitTime_S)
{
return SDO(reg, Son, data, WaitTime_S);
}
public bool set_region(int reg, int Son, ref Int32 data)
{
if (SDO(reg, Son, data))
{
data = SDO(reg, Son);
return true;
}
return false;
}
/// <summary>
/// 【写参数】适合台达
/// </summary>
/// <param name="PxXXX"></param>
/// <param name="data值"></param>
/// <returns></returns>
public bool set_region(string PxXXX, Int32 data值) // "P1.45"
{// 0x2000 + Pn + 常数
return Write_reg32bit(PxXXX, data值);// 应答帧 "1890295C19"h
}
public bool Write_reg32bit(string PxXXX, Int32 data)// 10h码
{// 0x2000 + Pn + 常数
//寄存器地址
UInt16 reg_address = get_reg_获取寄存器地址(PxXXX);
//设定值
//string start = modbus.Write_Reg32bit(reg_address, data);// "7F10040E00022B25"
bool start = set_region((short)reg_address, 0, data); // 寄存器，子索引， Value
return start;
}
private ushort get_reg_获取寄存器地址(string PxXXX)// "P4.007"
{// 0x2000 + Pn + 常数
//if (reg<0x07C8)//最大寄存器
if (PxXXX.IndexOf(".") > 0) // "P5.016" 伺服电机坐标
{
string hi = PxXXX.Substring(PxXXX.IndexOf(".") - 1, 1); // 组 4
string li = PxXXX.Substring(PxXXX.IndexOf(".") + 1); // 编号 007
int hv = int.Parse(hi) * 0x0100; // 0x400
int lv = int.Parse(li); //32 0x07
ushort reg_NO = (ushort)(hv + lv); // 0x0407
return (ushort)(0x2000 + reg_NO); // can在 0x2000+ 位置
}
else
{
return ushort.Parse(PxXXX);// 标准寄存器号
}
}
/// <summary>
/// 故障码，寄存器，子索引，值
/// // 80 b3b2b1b0
/// </summary>
/// <param name="errcode"></param>
/// <param name="reg"></param>
/// <param name="Son"></param>
/// <param name="value"></param>
/// <returns></returns>
string get_Error_str(Int32 reg, Int32 Son, Int32 errcode)
{
string errorString = string.Empty;
string str1 = $"Reg{reg.ToString("X4")}h-{Son.ToString("X2")}= 0x{errcode.ToString("X8")};_";
switch (errcode)
{// 80h b3b2b1b0
case 0x05030000: errorString = str1 + "触发位没有交替变化！ "; break;
case 0x05040000: errorString = str1 + "SDO 协议超时！ "; break;
case 0x05040001: errorString = str1 + "无效/ 未知的命令字！ "; break;
case 0x05040002: errorString = str1 + "无效的块大小(仅块传输模式)！ "; break;
case 0x05040003: errorString = str1 + "无效的序号(仅块传输模式)！ "; break;
case 0x05040004: errorString = str1 + "CRC 错误(仅块传输模式)！ "; break;
case 0x05040005: errorString = str1 + "内存溢出！ "; break;
case 0x06010000: errorString = str1 + "对象不支持访问！ "; break;
case 0x06010001: errorString = str1 + "试图读只写对象！ "; break;
case 0x06010002: errorString = str1 + "试图写只读对象！ "; break;
case 0x06020000: errorString = str1 + "对象不存在！ "; break;
case 0x06040041: errorString = str1 + "对象不能映射到PDO内！ "; break;
case 0x06040042: errorString = str1 + "映射的对象的数目和长度超出 PDO 长度！ "; break;
case 0x06040043: errorString = str1 + "一般性参数不兼容！ "; break;
case 0x06040047: errorString = str1 + "一般性设备内部不兼容！ "; break;
case 0x06060000: errorString = str1 + "硬件错误导致对象访问失败！ "; break;
case 0x06060010: errorString = str1 + "数据类型不匹配，服务参数长度不匹配！ "; break;
case 0x06060012: errorString = str1 + "数据类型不匹配，服务参数长度太长！ "; break;
case 0x06060013: errorString = str1 + "数据类型不匹配，服务参数长度太短！ "; break;
case 0x06070010: errorString = str1 + "数据型态不符；参数长度不符！ "; break;
case 0x06090011: errorString = str1 + "子索引不存在！ "; break;
case 0x06090030: errorString = str1 + "超出参数的值范围(写访问时)！ "; break;
case 0x06090031: errorString = str1 + "写入参数数值太大！ "; break;
case 0x06090032: errorString = str1 + "写入参数数值太小！ "; break;
case 0x06090036: errorString = str1 + "最大值小于最小值！ "; break;
case 0x08000000: errorString = str1 + "一般性错误！ "; break;
case 0x080000A1: errorString = str1 + "读EEPROM错误！ "; break;
case 0x080000A2: errorString = str1 + "写EEPROM错误！ "; break;
case 0x080000A3: errorString = str1 + "存取EEPROM超出范围！ "; break;
case 0x080000A4: errorString = str1 + "存取EEPROM错误！ "; break;
case 0x080000A5: errorString = str1 + "密码错误！ "; break;
case 0x08000020: errorString = str1 + "数据不能传送或保存到应用！ "; break;
case 0x08000021: errorString = str1 + "由于本地控制导致数据不能传送或保存到应用！ "; break;
case 0x08000022: errorString = str1 + "对象使用中！ "; break;
case 0x08000023: errorString = str1 + "对象字典不存在！ "; break;
//0800 0023H
//对象字典动态产生错误或对象字典不存在(例如通过文件生成对象字典，但由于文件
//损坏导致错误产生)
default:
errorString = str1 + "从站错误。检查通讯？ ";
break;
}
return errorString;
}
#endregion
#region 协议can CANopen 通訊協定：NMT、SYNC、SDO、PDO、EMCY、TIME。【总线命令，同步帧，寄存器读写，寄存器批量读写，故障帧】
#region SDO
//【操作核心】勿动。 作者：QQ750273008 祁成 2023/11/26更新
/// <summary>
/// 【读】
/// </summary>
/// <param name="reg"></param>
/// <param name="son"></param>
/// <param name="WaitTime_S"></param>
/// <returns></returns>
public int SDO(int reg, int son, double WaitTime_S)// 读
{
return SDO((UInt16)reg, (Byte)son, 0.0);
}
/// <summary>
/// 【写】
/// </summary>
/// <param name="reg"></param>
/// <param name="son"></param>
/// <param name="data"></param>
/// <param name="WaitTime_S"></param>
/// <returns></returns>
public bool SDO(int reg, int son, int data, double WaitTime_S)// 写
{// 进行重载调用
bool sta = SDO((UInt16)reg, (Byte)son, (Int32)data, WaitTime_S);
return sta;
}
#endregion
#region NMT
/// <summary>
/// 总线管理（命令，从站id）；帧_id=0x00，全站00
/// 【启动从站 = 0x01,
/// 停止从站 = 0x02,
/// 预运行从站 = 0x80,
/// 重启从站 = 0x81,
/// 通讯重连 = 0x82
/// </summary>
/// <param name="mode"></param>
/// <param name="address"></param>
public void NMT(NMT_CMD modecmd, int address, double WaitTime_s)// 总线指令，并留时间，等待从站响应
{// NMT总线管理操作 00h+
byte by0 = (byte)modecmd;//总线命令
byte by1 = (byte)address;// 从站id
// b0 b1b2 b3 b4b5b6b7
Byte8 = by0 + (by1 << 8);
Set_Send(0x02, 0x00, Byte8);// 1 4 8
DelaySecond(WaitTime_s);
延时 等待生效75ms
//DateTime startTime = DateTime.Now;//记录开始时间
//do
//{
// if ((DateTime.Now - startTime).TotalMilliseconds > 75)// 75ms
// {
// break; // 超时 毫秒
// }
//} while (true);
}// 总线命令
#endregion
#region SYNC
/// <summary>
/// 用于同步信号（广播帧）留20ms电机响应
/// </summary>
public void SYNC()
{// SYNC同步帧 【80h】
Set_Send(0x00, 0x80, 0x00);// id=0x80
}//同步帧
/// <summary>
/// （S）double等待秒
/// </summary>
/// <param name="WaitTime_s"></param>
public void SYNC(double WaitTime_s)
{
SYNC();
DelaySecond(WaitTime_s);
}
/// <summary>
/// 从站心跳ms：0x1017（16bit）
/// </summary>
/// <param name="address"></param>
public void SYNC_Set_Close()
{//关闭 心跳 0x1017=2000;ms
//【0x 00000000 00 1017 2B】
Set_Send(0x08, 0x600 + Address, 0x00000000010172b);// 寄存器 0x1017-00=0x00000000,操作码 0x2b （16bit）
//delaySecond(2.0);//延时2秒
//Help_Delay.delayTime(1.123);//0.1秒
DelaySecond(1.123);
}
/// <summary>
/// 从站id，设置心跳时间【回帧700h+，时间ms】
/// </summary>
/// <param name="address"></param>
/// <param name="type"></param>
/// <param name="time_ms"></param>
public void SYNC(Int32 Slave_id, int time_ms = 2000)
{
Address = (UInt32)Slave_id;
//心跳： 0x1017 = 2000; ms
set_region(0x1017, 0x00, time_ms);// 0x7d0 9000ms
}
#endregion
#region EMCY
/// <summary>
/// 0x【厂家错误码，H1001，应急错误码】
/// </summary>
/// <param name="Slave_id"></param>
/// <param name="H603f"></param>
/// <param name="H1001"></param>
/// <param name="Motor厂家"></param>
public void EMCY(int Slave_id, Int16 H603f, byte H1001, Int32 Motor厂家)// 测试
{//下位机故障 80h+从站id
Int64 error = (Int16)H603f;// 16bit 应急错误码【b1b0】 0x1013-01
Int64 h1001 = (byte)H1001;// 寄存器0x1001【b2】
Int64 motor = Motor厂家;// 伺服厂家错误码 5字节
Int64 cmd = error + (H1001 << 16) + (motor << 24);// u32 =【b7-b4】【b2】【b1b0】
// 【厂家】【h1001】【h603f】
Set_Send(0x08, (uint)0x80 + (Byte)Slave_id, cmd);
}
#endregion
#region TIME
/// <summary>
/// 时间广播bit48
/// </summary>
/// <param name="byte8"></param>
public void TIME(Int64 byte8)
{// TIME操作 100h+
Set_Send(0x06, 0x100, byte8);// 【6】固定6字节时间
}// 时钟
#endregion
#region PDO
#region PDO配置： 指针绑定
//========快捷指针寄存器受控码
//rPDO1 200h+ tPDO1 180h+
//rPDO2 300h+ tPDO1 280h+
//rPDO3 400h+ tPDO1 380h+
//rPDO4 500h+ tPDO1 480h+
/// <summary>
/// 2023.6.25 rPDO配置【1400控制，1600绑定】
/// 祁成qq750273008
/// SET到伺服8字节，这是寄存器快捷键1400h，1600h
/// </summary>
bool PDOr_Config(PDOtype PDOn, PDO_Config_Struct obj)// PDO配置
{
bool sta = true;
byte zu = (byte)(PDOn);
short h1400 = (short)((zu - 1) + 0x1400);// 确定组队【队伍】
short h1600 = (short)((zu - 1) + 0x1600);
//1 PDO总开关：关闭 子索引5
//h1400-01定义【d31 rPDO总开关】【d30 保留】【d29（0标准帧11bit），（1扩展帧29bit）】【d28~0 帧id29bit】
sta &= SDO(h1400, 0x01, 1 << 31);//【关闭 rPDO】(原值： 0x4000 0200h + 从站【帧id29】)// 0x40000202
//2 1400配置
sta &= set_region(h1400, 0x02, obj.rPDO命令触发器); //【0~240】(第1次SYNC帧后，生效设定值）【原值：0xFF】254，255为事件，值不同立即生效
sta &= set_region(h1400, 0x03, obj.rPDO禁止时间_100us);// （死区ms）接收PDO1命令过滤间隔，单位0.1ms【禁止时间】 雷赛40ms（原值：0x00）
//set_region(h1400, 0x04, 0);// 不存在
//sta &= set_region(h1400, 0x05, 0);// 是定时触发器（原值：0x00）【不用于RPDO】
//3 1600寄存器绑定
sta &= set_region(h1600, 0x00, 0x00); // 寄存器指针:关闭 （原值：0x02）
if (obj.Reg1 != 0) sta &= set_region(h1600, 1, obj.Reg1); // 指针1 0x60400010 控制字u16
if (obj.Reg2 != 0) sta &= set_region(h1600, 2, obj.Reg2); // 指针2 0x607A0020 目标位置u32 PDO4:0x60FF0020 目标速度u32
if (obj.Reg3 != 0) sta &= set_region(h1600, 3, obj.Reg3); // 指针3
if (obj.Reg4 != 0) sta &= set_region(h1600, 4, obj.Reg4); // 指针4
if (obj.Reg5 != 0) sta &= set_region(h1600, 5, obj.Reg5); // 指针5
if (obj.Reg6 != 0) sta &= set_region(h1600, 6, obj.Reg6); // 指针6
if (obj.Reg7 != 0) sta &= set_region(h1600, 7, obj.Reg7); // 指针7
if (obj.Reg8 != 0) sta &= set_region(h1600, 8, obj.Reg8); // 指针8
// 所有寄存器加起来不能超过64bit
if (obj.UseRegNum > 8) obj.UseRegNum = 8;// 不能超过64bit
sta &= set_region(h1600, 0x00, obj.UseRegNum); // 寄存器指针有效数量
//4 rPDO使能 bit31【PDO总开关，0:开】 bit10~0【帧id 11bit，受控码】
//h1400-01定义【d31 rPDO总开关】【d30 保留】【d29（0标准帧11bit），（1扩展帧29bit）】【d28~0 帧id29bit】
sta &= set_region(h1400, 0x01, (int)(0x40000000 + 0x100 + (0x100 * zu) + Address)); // 开启PDO1功能 // 0x40000202
//5 轴对象标记
//Axis_Config_start(PDOn, obj);
return sta;
}
// 轴配置记录
void Axis_Config_start(PDOtype PDOn, PDO_Config_Struct obj)
{
byte zu = (byte)(PDOn);
if(zu==1)
{
axis_obj[address].PDO1.regUSE = obj.UseRegNum;
axis_obj[address].PDO1.regNO[1] = obj.Reg1;
axis_obj[address].PDO1.regNO[2] = obj.Reg2;
axis_obj[address].PDO1.regNO[3] = obj.Reg3;
axis_obj[address].PDO1.regNO[4] = obj.Reg4;
axis_obj[address].PDO1.regNO[5] = obj.Reg5;
axis_obj[address].PDO1.regNO[6] = obj.Reg6;
axis_obj[address].PDO1.regNO[7] = obj.Reg7;
axis_obj[address].PDO1.regNO[8] = obj.Reg8;
}
if (zu == 2)
{
axis_obj[address].PDO2.regUSE = obj.UseRegNum;
axis_obj[address].PDO2.regNO[1] = obj.Reg1;
axis_obj[address].PDO2.regNO[2] = obj.Reg2;
axis_obj[address].PDO2.regNO[3] = obj.Reg3;
axis_obj[address].PDO2.regNO[4] = obj.Reg4;
axis_obj[address].PDO2.regNO[5] = obj.Reg5;
axis_obj[address].PDO2.regNO[6] = obj.Reg6;
axis_obj[address].PDO2.regNO[7] = obj.Reg7;
axis_obj[address].PDO2.regNO[8] = obj.Reg8;
}
if (zu == 3)
{
axis_obj[address].PDO3.regUSE = obj.UseRegNum;
axis_obj[address].PDO3.regNO[1] = obj.Reg1;
axis_obj[address].PDO3.regNO[2] = obj.Reg2;
axis_obj[address].PDO3.regNO[3] = obj.Reg3;
axis_obj[address].PDO3.regNO[4] = obj.Reg4;
axis_obj[address].PDO3.regNO[5] = obj.Reg5;
axis_obj[address].PDO3.regNO[6] = obj.Reg6;
axis_obj[address].PDO3.regNO[7] = obj.Reg7;
axis_obj[address].PDO3.regNO[8] = obj.Reg8;
}
if (zu == 4)
{
axis_obj[address].PDO4.regUSE = obj.UseRegNum;
axis_obj[address].PDO4.regNO[1] = obj.Reg1;
axis_obj[address].PDO4.regNO[2] = obj.Reg2;
axis_obj[address].PDO4.regNO[3] = obj.Reg3;
axis_obj[address].PDO4.regNO[4] = obj.Reg4;
axis_obj[address].PDO4.regNO[5] = obj.Reg5;
axis_obj[address].PDO4.regNO[6] = obj.Reg6;
axis_obj[address].PDO4.regNO[7] = obj.Reg7;
axis_obj[address].PDO4.regNO[8] = obj.Reg8;
}
}
/// <summary>
/// 2023.6.25 tPDO配置【1800配置，1A00绑定】
/// 祁成qq750273008
/// </summary>
/// <param name="PDOn"></param>
/// <param name="obj"></param>
/// <returns></returns>
bool PDOt_Config(PDOtype PDOn, PDO_Config_Struct obj)
{
bool sta = true;
byte zu = (byte)(PDOn);
short h1800 = (short)((zu - 1) + 0x1800);// 确定组队【队伍】
short h1A00 = (short)((zu - 1) + 0x1A00);
//1 PDO总开关：关闭 子索引5个
//【h1800-01】定义【d31 tPDO 0开，1关】【d30 RTR 0开，1关】【d29 0标准帧，1扩展帧】【d28~0 帧id】
// RTR：远程帧回应
sta &= set_region(h1800, 0x01, 1 << 31); // 关闭 tPDO 功能(原值： 0x4000 0180h + 从站地址)// 0x40000182
//2 1800配置
sta &= set_region(h1800, 0x02, obj.tPDO上传触发器); //【0】非循环（值有变化，sync后上传），【1~240】SYNC到(设置值)次数后上传tPDO，【254或255】值变或计时到就上传。
sta &= set_region(h1800, 0x03, obj.tPDO禁止时间_100us);// （死区ms）回帧失败等待间隔，单位0.1ms 雷赛40ms（原值：0x00）
//=set_region(h1400, 0x04, 0);// 不存在
sta &= set_region(h1800, 0x05, obj.tPDO定时触发器_1ms);//【原值：0x00】//【tPDO上传触发器=255时】时间到就上传tPDO
//3 1A00寄存器绑定
sta &= set_region(h1A00, 0, 0x00); // 寄存器指针:关闭 （原值：0x01）
if (obj.Reg1 != 0) sta &= set_region(h1A00, 1, obj.Reg1); // 指针1 0x60410010 状态字u16
if (obj.Reg2 != 0) sta &= set_region(h1A00, 2, obj.Reg2); // 指针2 0x60640020 电机位置u32
if (obj.Reg3 != 0) sta &= set_region(h1A00, 3, obj.Reg3); // 指针3 0x606C0020 当前速度u32
if (obj.Reg4 != 0) sta &= set_region(h1A00, 4, obj.Reg4); // 指针4
if (obj.Reg5 != 0) sta &= set_region(h1A00, 5, obj.Reg5); // 指针5
if (obj.Reg6 != 0) sta &= set_region(h1A00, 6, obj.Reg6); // 指针6
if (obj.Reg7 != 0) sta &= set_region(h1A00, 7, obj.Reg7); // 指针7
if (obj.Reg8 != 0) sta &= set_region(h1A00, 8, obj.Reg8); // 指针8
// 所有寄存器加起来不能超过64bit
if (obj.UseRegNum > 8) obj.UseRegNum = 8; //if (obj.UseNum > 8) obj.UseNum = 8;
sta &= set_region(h1A00, 0, obj.UseRegNum); // 寄存器指针有效数量
//4 PDO使能 40000182
sta &= set_region(h1800, 1, (int)(0x40000000 + 0x80 + (0x100 * zu) + Address)); // 开启tPDO功能 // 0x40000182
//5 轴对象标记
Axis_Config_start(PDOn, obj);
return sta;
}
public bool PDO_Config(uint Slave_id, PDO_Type type, PDO_Config_Struct pdo)
{
Address = Slave_id;
bool sta = true;
switch (type)
{
case PDO_Type.配置rPDO1:// 命令下达寄存器（命令）
sta &= PDOr_Config(PDOtype.PDO1, pdo);
break;
case PDO_Type.配置rPDO2:
sta &= PDOr_Config(PDOtype.PDO2, pdo);
break;
case PDO_Type.配置rPDO3:
sta &= PDOr_Config(PDOtype.PDO3, pdo);
break;
case PDO_Type.配置rPDO4:
sta &= PDOr_Config(PDOtype.PDO4, pdo);
break;
case PDO_Type.配置tPDO1:// 状态回帧寄存器（回答）
sta &= PDOt_Config(PDOtype.PDO1, pdo);
break;
case PDO_Type.配置tPDO2:
sta &= PDOt_Config(PDOtype.PDO2, pdo);
break;
case PDO_Type.配置tPDO3:
sta &= PDOt_Config(PDOtype.PDO3, pdo);
break;
case PDO_Type.配置tPDO4:
sta &= PDOt_Config(PDOtype.PDO4, pdo);
break;
default:
sta = false;
break;
}
return sta;
}
public bool PDO_Config(PDO_Type type, PDO_Config_Struct pdo)
{
写信
//PDO_Struct pdor = new PDO_Struct();
//pdor.rPDO命令触发器 = 255;//【0~240】在1个SYNC后，rPDO值才生效。【254或255】rPDO值立即生效（不需要SYNC信号）
【241~251】参数无效【252~253】rPDO命令失效（不受rPDO命令控制）
//pdor.rPDO禁止时间_100us = 10;// 1ms过滤
//pdor.UseRegNum = 2;
//pdor.Reg1 = 0x60400010;// 16bit
//pdor.Reg2 = 0x607A0020;// 32bit
//PDO_Config(PDO_Type.配置rPDO1, pdor);
回信
//PDO_Struct pdot = new PDO_Struct();
//pdot.tPDO上传触发器 = 7;// 第n个SYNC帧后，触发tPDO帧
//pdot.tPDO禁止时间_100us = 5000;// 【上传失败用】上传帧间隔
//pdot.tPDO定时触发器_1ms = 0;//【tPDO上传触发器=255时】时间到就上传tPDO
//pdot.UseRegNum = 2;
//pdot.Reg1 = 0x60410010;// 16bit
//pdot.Reg2 = 0x60640020;// 32bit
//PDO_Config(PDO_Type.配置tPDO1, pdot);
//=========================================
return PDO_Config(Address, type, pdo);
}
#endregion
#region PDO使用
#region 作废
public void PDO_EN()
{
//6040
//0 Switch on 转换开
//1 Enable voltage电压开
//2 Quick Stop快停
//3 Enable operation 启用操作
//4 pp新设定点（正触发） hm回零操作开始（正触发）
//5 pp立即更改集合
//6 pp绝对（0）/相对（1）
//7 Fault reset 故障复位
//8 Halt 暂停 hm回零停止运行（0x10F）
//执行回零 bit4
PDO(PDOtype.PDO1, 0x2F);// 准备回零 bit4 上升沿触发
SYNC();
PDO(PDOtype.PDO1, 0x3F);
SYNC();
}
#endregion
/// <summary>
/// 批量写寄存器【数据包64bit】
/// </summary>
/// <param name="pdo"></param>
/// <param name="byte8"></param>
public bool PDO(PDOtype pdo, Int64 byte8)// 批量写寄存器【数据包64bit】
{
return Set_Send(0x08, 0x100 + (uint)pdo * 0x100 + Address, byte8);//
}// PDO指针寄存器
public bool PDO1(Int64 byte8)// 6040 607A 目标位置
{// PDO1操作 200h+
return Set_Send(0x08, 0x200 + Address, byte8);
}
public bool PDO2(Int64 byte8)
{// PDO2操作 300h+
return Set_Send(0x08, 0x300 + Address, byte8);
}
public bool PDO3(Int64 byte8)
{// PDO3操作 400h+
return Set_Send(0x08, 0x400 + Address, byte8);
}
public bool PDO4(Int64 byte8)
{// PDO4操作 500h+
return Set_Send(0x08, 0x500 + Address, byte8);
}
#endregion
#endregion
#endregion
#region 后台方法【数据帧处理】
/// <summary>
/// 制作Code编码 1 4 8
/// </summary>
/// <param name="hend"></param>
/// <param name="id"></param>
/// <param name="byte8"></param>
/// <returns></returns>
string madeCode(byte hend, UInt32 id, Int64 byte8)// 1表示帧类型，4字节是帧id，8表示帧数据包
{
string data8 = string.Empty;
for (int i = 0; i < 8; i++)
{
data8 += ((byte8 >> (i * 8)) & 0xFF).ToString("X2");// 低字节先出
}
// 单帧
string cmd = hend.ToString("X2") + id.ToString("X8") + data8;// 1 4 8 配置数据单帧
byte[] buffer = helpstring.StringsToHexBytes(cmd);// 单帧 13字节( 1 4 8 在接口解码发送)
string data = helpstring.BytesToHexStrings(buffer);// 等效编码
return data;
}
double getValue(char key, string data)// 获取字母后的小数
{//G0X9.896Y10.158Z1.000F10000
string data1, data2;
if (data.IndexOf(key) != -1)// 确定 字母
{
data1 = data.Split(key)[1];//去头
data2 = get_去尾(data1);//去尾
try
{
return double.Parse(data2);
}
catch (Exception ex)
{
throw ex;
}
}
return -1.0;
}
static string get_去尾(string str)
{
for (int i = 0; i < str.Length; i++)
{
string num = str.Substring(i, 1);
if (num == "-" || num == "." ||
num == "0" || num == "1" || num == "2" || num == "3" || num == "4" ||
num == "5" || num == "6" || num == "7" || num == "8" || num == "9") { }
else
{
return str.Substring(0, i);
}
}
return str;
}
unsafe void wait_AXIS(byte MAX, byte small)
{
VCI_CAN_OBJ[] obj = new VCI_CAN_OBJ[2500];// 创新科技CAN分析仪最大支持2000帧接收
byte[] rx13 = new byte[13];// 提取数据帧
byte at = (byte)Address;//标记
Address = MAX;
int num;
//延时 等待生效75ms
DateTime startTime = DateTime.Now;//记录开始时间
do
{
//if ((DateTime.Now - startTime).TotalMilliseconds > 3000) break;
// can.set_ClearBuffer(4, 0, 0);
num = Get_Receive(ref obj);
SYNC();
DelaySecond(0.005);//5ms // USB速度3~5ms
//Help_Delay.delayTime(0.005);//5ms // USB速度3~5ms
num = Get_Receive(ref obj);
//return;
if (num > 0)
{
for (int i = num - 1; i >= 0; i--)
{
if (obj[i].ID > 0x80 && obj[i].ID < (0x80 + 127))// 从站故障 80h+ 从站
{
// 故障呼叫
//COB - ID Byte 0:1 Byte 2 Byte 3:7
//0x080 + Node - ID 应急错误代码 错误寄存器(1001H) 厂商指定区域
//byte0~1：
// 应急错误代码
// 0000H 无错误
// 8110H CAN 溢出
// 8120H 错误被动模式（0411）
// 8130H 寿命保护/ 心跳错误
// 8140H 被迫离线恢复故障
// 8141H 被迫离线
// 8150H 发送 COB - ID 冲突
// 8210H PDO 长度错误未处理
// 8220H PDO 超过长度
//byte2：
// (1001H)位定义
// Bit0：generic error 一般错误
// Bit1：current 电流
// Bit2：voltage 电压 （04）
// Bit3：temperature 温度
//————————————————
//版权声明：本文为CSDN博主「cfqq1989」的原创文章，遵循CC 4.0 BY - SA版权协议，转载请附上原文出处链接及本声明。
//原文链接：https://blog.csdn.net/cfqq1989/article/details/128308808
}// 故障（从站故障）80h + 从站
// 等待轴定位完成
if (obj[i].ID == 0x180 + Address)// PDO1 200h+
{
rx13[5] = obj[i].Data[0];// 37
rx13[6] = obj[i].Data[1];// 16
rx13[7] = obj[i].Data[2];
rx13[8] = obj[i].Data[3];
rx13[9] = obj[i].Data[4];
rx13[10] = obj[i].Data[5];
rx13[11] = obj[i].Data[6];
rx13[12] = obj[i].Data[7];
if ((rx13[6] & 0x16) == 0x16)// bit10 0x1237 0x1637
{
Address--;//定位完成
if (Address < small)
{
Address = at;// 还原
return;
}
}
}// SDO应答帧
}
}
else
{
DelaySecond(0.205);//
//Help_Delay.delayTime(0.205);//
}
} while (true);
}
unsafe void wait_AXIS(byte addr)
{
wait_AXIS(addr, addr);
}
/// <summary>
/// 6041h状态
/// </summary>
/// <param name="state"></param>
/// <returns></returns>
public string get_6041h(byte byte0, byte byte1)
{
string str0 = Convert.ToString(byte0, 16);
string str1 = Convert.ToString(byte1, 16);
int state = Convert.ToInt32(str1 + str0, 16);
string end = string.Empty;
switch (state & 0x6F)
{
case 0x00: end = "未准备启动 "; break;
case 0x40: end = "取消启动 "; break;
case 0x21: end = "准备启动 "; break;
case 0x23: end = "启动 "; break;
case 0x27: end = "允许操作 "; break;
case 0x07: end = "快速停止激活 "; break;
case 0x0F: end = "故障效应激活 "; break;
case 0x08: end = "故障 "; break;
default:
break;
}
return end;
}
//private void delayTime(double secend)
//{
// DateTime tempTime = DateTime.Now;
// while (tempTime.AddSeconds(secend).CompareTo(DateTime.Now) > 0)
// Application.DoEvents();
//}
Int32 get_result(List<string> getend)// "46s8821：08000005814B002000E8030000"
{//回帧结果
string str = string.Empty;
string obj = string.Empty;//寄存器标记
for (int i = 0; i < getend.Count; i++)
{
string str1 = getend[i];// 提取
if (str1.Length < 26) throw new Exception("长度不够 ");//长度不够
string str2 = str1.Substring(str1.Length - 16); // 截取 8字节
obj = str2.Substring(0, 2);// 寄存器 类型
// 1111 1011 0111 0011
if (obj == "4F" | obj == "4B" | obj == "47" | obj == "43" | obj == "80")// 1字节，2字节，3字节，4字节，错误码
{
Region_Obj = Convert.ToByte(obj, 16);// 标记寄存器 类型
//if (obj == "4F") Region = 8;
//if (obj == "4B") Region = 16;
//if (obj == "47") Region = 24;
//if (obj == "43") Region = 32;
string str3 = str1.Substring(str1.Length - 8);
for (int j = 0; j < 4; j++)
{
string num = str3.Substring(str3.Length - (2 * j) - 2, 2);
str += num;
}
return Convert.ToInt32(str, 16);// 成功
}
//下一轮数据帧
}
if (obj == "60") Region_Obj = 0x60; return 0;// 写成功
//return -1;// 读取失败
}
/// <summary>
/// 解析数据帧：get_Value(u32)
/// </summary>
/// <param name="getend"></param>
/// <returns></returns>
Int32 get_Value_Int32(byte[] getend)// "46s8821：08000005814B002000E8030000"
{//回帧结果 08 00000581 4B002000E8030000
return getend[9] + (getend[10] << 8) + (getend[11] << 16) + (getend[12] << 24);
// 解析 43,4060,00,0000 0000 // 寄存器0x6040-00=0x00000000；
// E8 索引9
// 03 索引10
// 00 索引11
// 00 索引12 结果 0000 03E8
}
unsafe Int64 get_Value_Int64(byte* getend)// 数据帧data
{//
return getend[0] + (getend[1] << 8) + (getend[2] << 16) + (getend[3] << 24) +
getend[4] << 32 + (getend[5] << 40) + (getend[6] << 48) + (getend[7] << 58);
}
//void wait_dataRX数据池()
//{
// do
// {
// if (rj45.rx数据池字节 != 0)
// {
// int i = 0;// 调试 断点
// i++;
// }
// if (rj45.rx数据池字节 == 0)
// {
// break;
// }
// } while (true);
//}
//void throw_bytes()
//{
// Thread.Sleep(20);
// if (rj45.socket.Available > 0)
// {
// byte[] bytes = new byte[rj45.socket.Available];
// int count = rj45.socket.Receive(bytes, bytes.Length, SocketFlags.None);
// }
//}
#endregion
#region 模块方法
public enum POWER : byte
{
// b7 ↑故障复位
// 0x06 关闭
// 0x07 打开
// 0x0F 打开+启用操作
// 0<<1 快速停止
// 0x07 禁用操作
// 0x0F 启用操作
完全关闭 = 0x06,
快速停止 = 0x00,
禁用操作 = 0x07,
启用操作 = 0x0F //要先0x06完全关闭
}
public enum PDOtype : byte
{
PDO1 = 1,
PDO2,
PDO3,
PDO4,
}
public enum GoHome_Type : byte
{//6098
方法17 = 17,
方法18 = 18,
方法19 = 19,
方法20 = 20,
方法21 = 21,
方法22 = 22,
方法23 = 23,
方法24 = 24,
方法25 = 25,
方法26 = 26,
方法27 = 27,
方法28 = 28,
方法29 = 29,
方法30 = 30,
方法31 = 31,
方法32 = 32,
方法33 = 33,
方法34 = 34,
方法35 = 35,//零点直设
零点直设 = 37//零点直设
}
public enum cia402 : ushort// 16bit
{
故障码 = 0x603F,
控制字 = 0x6040,// 油门，刹车 06h关闭》07h伺服使能（命令关）》0Fh命令生效 00h急停
// 0 启动 1伺服开，0伺服关
// 1 电压输出 1电压输出，0关
// 2 急停 1不报警
// 3 命令（上升沿） 01命令上升沿生效 07h》0Fh
//======================================
// 4 运动指令（上升沿） 该位由0跳变为1将触发新的位置指令
// 5 0等待旧命令位置完成 1立刻更新并执行新的位置指令动作
// 6 0绝对位置 1相对位置
// 7 复位 01上升沿，故障复位
// 8 1暂停 0继续运行
// 9 速度 1使用上次的旧速度
// 10~15 空
状态字 = 0x6041,// 汽车仪表（状态）
// 0
// 1
// 2
// 3
// 4
// 5
// 6
// 7
// 8 0： 目标位置到达 （bit10为1）
// 9
// 10 1
// 11
// 12 当前位置正在缓存， 新的位置将被接受
// 13 跟随错误
// 14
// 15
快停 = 0x605A,
//定位模式
工作模式 = 0x6060, // 01h定位模式
目标位置 = 0x607A,
额定速度 = 0x6081,// 2w
加速度 = 0x6083,// 5w
减速度 = 0x6084,// 50000
// 绝对0x6040=1Fh 停 0Fh
// 相对0x6040=5Fh 停 4Fh
//速度模式
目标速度 = 0x60FF,
速度反馈 = 0x606C,
//力矩模式
//回零模式
回零方式 = 0x6098,
回零速度 = 0x6099,//-01回零高速，-02回零低速
回零加速 = 0x609A,
零点偏置 = 0x607C,
//0x603F-错误代码UINT16 RO YES
//0x6040-控制字UINT16 WO是
//0x6041-状态剑UINT16 RO是
//0x605A-快速停止选项代码INT16 RW NO
//0x6060-操作模式INT8 WO是
//0x6061-操作模式显示INT8 RO YES
//0x6064-位置实际值INT32 RO是
//0x6065-以下错误窗口UINT32 RW NO
//0x606C-速度实际值INT32 RO是
//0x6071-目标扭矩INT16 RW是
//0x6073-最大电流UINT16 RW是
//0x6074-扭矩需求INT16 RO是
//0x6078-当前实际值INT16 RO是
//0x607A-目标位置INT32 RW是
//0x607C-起始偏移INT32 RW是
//0x607E-极性UINT8 RW是
//0x607F-最大剖面速度UINT32 RW是
//0x6081-剖面速度UINT32 RW是
//0x6083-配置文件加速UINT32 RW是
//0x6084-轮廓减速UINT32 RW是
//0x6085-快速停止减速UINT32 RW是
//0x6087-扭矩斜率UINT32 RW是
//0x6098-归位方法INT8 RW是
//0x6099 2归位速度阵列-否
//0x609A-归位加速度UINT32 RW是
//0x60F4-跟随错误实际值INT32 RO YES
//0x60FD-数字输入UINT32 RW是
//0x60FE 2数字输出阵列-否
//0x60FF-目标速度INT32 RW是
//0x6502-支持的驱动器模式UINT32 RO N
//0x603F - Error code UINT16 RO YES
//0x6040 - Controlword UINT16 WO YES
//0x6041 - Statusword UINT16 RO YES
//0x605A - Quick stop option code INT16 RW NO
//0x6060 - Modes of operation INT8 WO YES
//0x6061 - Modes of operation display INT8 RO YES
//0x6064 - Position actual value INT32 RO YES
//0x6065 - Following error window UINT32 RW NO
//0x606C - Velocity actual value INT32 RO YES
//0x6071 - Target torque INT16 RW YES
//0x6073 - Max current UINT16 RW YES
//0x6074 - Torque demand INT16 RO YES
//0x6078 - Current actual value INT16 RO YES
//0x607A - Target position INT32 RW YES
//0x607C - Home offset INT32 RW YES
//0x607E - Polarity UINT8 RW YES
//0x607F - Max profile velocity UINT32 RW YES
//0x6081 - Profile velocity UINT32 RW YES
//0x6083 - Profile acceleration UINT32 RW YES
//0x6084 - Profile deceleration UINT32 RW YES
//0x6085 - Quick stop deceleration UINT32 RW YES
//0x6087 - Torque slope UINT32 RW YES
//0x6098 - Homing method INT8 RW YES
//0x6099 2 Homing speed ARRAY - NO
//0x609A - Homing acceleration UINT32 RW YES
//0x60F4 - Following error actual value INT32 RO YES
//0x60FD - Digital inputs UINT32 RW YES
//0x60FE 2 Digital outputs ARRAY - NO
//0x60FF - Target velocity INT32 RW YES
//0x6502 - Supported drive modes UINT32 RO N
}
public enum addressH : byte
{
add_0 = 0,
add_32 = 1,
add_64 = 2,
add_96 = 3,// 96+31=127
}
public class PDOregion
{
public int num;
public int[] regName { get; set; }//PDO待绑定寄存器
public byte[] regBit { get; set; }// 寄存器bit长度
}
public enum CMDtype
{
回零,
绝对定位,
相对定位,
补间,
速度,
力矩,
复位,// bit7
暂停,// bit8
}
public enum AXIS_type
{
A = 65,
B,
C,
D,
E,
F,
G,
H,
I,
J,
K,
L,
M,
N,
O,
P,
Q,
R,
S,
T,
U,
V,
W,
X = 127,
Y = 126,
Z = 125,
X1 = 124,
Y1 = 123,
Z1 = 122,
}
public enum Motor_type
{// 【1位置 3速度 4扭矩 6回零 7补间
位置1 = 1,
速度3 = 3,
扭矩4 = 4,
回零6 = 6,
补间7 = 7,
}
#endregion
#region 委托
#endregion
#region 窗体移动
//private Point mouseOff;//鼠标移动位置变量
//private bool leftFlag;//标签是否为左键
//private void Frm_MouseDown(object sender, MouseEventArgs e)//鼠标按下
//{
// if (e.Button == MouseButtons.Left)
// {
// mouseOff = new Point(-e.X, -e.Y); //得到变量的值
// leftFlag = true; //点击左键按下时标注为true;
// }
//}
//private void Frm_MouseMove(object sender, MouseEventArgs e)//鼠标移动
//{
// if (leftFlag)
// {
// Point mouseSet = Control.MousePosition;
// mouseSet.Offset(mouseOff.X, mouseOff.Y); //设置移动后的位置
// //Location = mouseSet;//Form 窗体父类字段
// }
//}
//private void Frm_MouseUp(object sender, MouseEventArgs e)//鼠标松开
//{
// if (leftFlag)
// {
// leftFlag = false;//释放鼠标后标注为false;
// }
//}
#endregion
#region 错误
//try
//{
// byte[] buffer = Encoding.Default.GetBytes(data);//以计算机的编码发送
// serialPort.Write(buffer, 0, buffer.Length);// 开始发送
//}
//catch (Exception ex) { MessageBox.Show(ex.Message); }//显示错误
//================================================
// 读取回帧-------0x40 结果
//int WaitCount = 0;//接收超次数
//DateTime startTime = DateTime.Now;//记录开始时间
// 4 等待接收
#region MyRegion
//while (true)
//{// 08 00000081 20811104 00000000
// // 超时
// if (timeOutFun)
// {
// if ((DateTime.Now - startTime).TotalMilliseconds > 300)// 超时
// {
// Region_Type = 0x80;// 标记 读取失败
// throw new Exception("(超时)读寄存器失败！"); // 超时 毫秒
// }
// }// 超时
// rx13 = Receive();//回帧报文
// if (rx13 == null)
// {
// ++WaitCount;
// if (WaitCount > 20)// 超次数
// {
// rx13 = new byte[13];
// rx13[5] = 0x80;// 故障码
// break;
// }
// continue;
// }// 超次
#endregion
#endregion
#region 参考
//6040
//0 Switch on 转换开
//1 Enable voltage电压开
//2 Quick Stop快停
//3 Enable operation 启用操作
//4 pp新设定点（正触发） hm回零操作开始（正触发）
//5 pp立即更改集合
//6 pp绝对（0）/相对（1）
//7 Fault reset 故障复位
//8 Halt 暂停 hm回零停止运行（0x10F）
//6041 上电50》启动250》06码231》07码233》0F码c237
//定位中=0x1237 定位完成=0x1637
//0准备打开=========================7
//1打开
//2操作已启用（伺服开启状态）
//3故障（驱动器将伺服关闭）
// 4电压==========================3
// 5快速停止
// 6关闭
// 7警告（驱动器仍然打开伺服）
//8非正常停止===========================2
//9远程控制
//10位置到达（到位置1）
//11内部位置超限（不支持）
//12 pp设定点 承认 HM回原点完成 ipIP模式激活 pv零速
//13 pp以下错误 HM归零错误
//
//
//定位中 1237
//定位完成 1637
//
#endregion
}
}
1213
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
