作用是解决C#控制伺服的难题
硬件:B3伺服 L 系列,用RS485通讯
1使用初始化:初始化伺服
Help_Delta_B3 b3 = new Help_Delta_B3();//台达伺服B3系列
//========================================
b3.B3_init_初始化(0x7f);
b3.Gcode_init_G代码初始化();
2 初始化好了之后,就可以用了
private void button6_Click(object sender, EventArgs e)
{
b3.Gcode_解析(textBox1.Text,true);
}
private void button7_Click(object sender, EventArgs e)
{
b3.Gcode_解析(textBox2.Text, true);
}
总共就这2步。textBox1.Text里填标准G代码。
//"G0X9.896Y10.158Z1.000F10000"
下面是源码
using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Threading;
using System.Threading.Tasks;
namespace help
{
/// <summary>
/// Help_Delta_B3 b3 = new Help_Delta_B3();//台达伺服 B3驱动器
/// 每帧数据,结尾必须留10ms
/// 应答报文留10ms等待。“P3.007”ms + 0.5ms
/// 伺服每帧应答最快能保证小于10ms
/// 出厂38400n82
/// 编码器24bit 16777216
///
/// </summary>
public class Help_Delta_B3
{//台达伺服B3系列 RS485 通讯
#region 字段
Help_ModBus rs485 = null;//数据帧业务
#endregion
#region 属性
public Help_ModBus help_ModBus { get => rs485; set => rs485 = value; }
//public Int32 DI { get => get_DI(); set => set_DI(value); }
public Int32 DO_状态 { get => get_DO(); }
#endregion
#region 构造
/// <summary>
/// 台达伺服B3,默认构造
/// </summary>
public Help_Delta_B3()
{
rs485 = new Help_ModBus();//数据帧业务
//rs485.delay_帧头等待 = 10;//必须10ms
rs485.delay_帧尾等待 = 10;//必须10ms
}
#endregion
#region 自定义方法
//==========================================================================
#region 基础功能
#region get
/// <summary>
/// 获取寄存器的值
/// </summary>
/// <param name="str"></param>
/// <returns></returns>
Int32 get_value(string data)
{// "44秒294ms: 7F 03 04 0080 0000 65DC"
string str = data;
if (str?.Length > 0)
{
int i = 0;
string at = rs485.address.ToString("X2") + "03"; // 地址+功能码 7F03
i = str.IndexOf(at);
if (i < 14 && str.Length > 23)//在帧开头位置
{
str = str.Substring(i + 4); //字节数+ 字节
if (str.Substring(0, 2) == "04")
{// " 2秒428ms:7F03 04 96B0 00CB 09CC"
string num0 = str.Substring(2, 4);//是【0】
string num1 = str.Substring(6, 4);//是【1】
return Convert.ToInt32(num1 + num0, 16);
}
}
}
return 0;
//try
//{
// string data = string.Empty;
// data = b3.get_电机位置();
// if (data?.IndexOf("0304") > 1)
// {
// string data1 = data.Substring(data.IndexOf("0304") + 4, 8);
// string data2 = data1.Substring(0, 4);
// string data3 = data1.Substring(4, 4);
// string data4 = data3 + data2;
// Int32 z_坐标 = Convert.ToInt32(data4, 16);
// textBox4.Text = z_坐标.ToString();
// }
//}
//catch (Exception ex)
//{
// System.Windows.MessageBox.Show(ex.Message);
// //throw;
//}
}
/// <summary>
/// 读取外部输入
/// "P4.007"
/// </summary>
/// <returns></returns>
public Int32 get_DI()
{//注意:这里不要用Task
int num = 0;
string str = read_reg16bit("P4.007");
num = get_value(str);
return num;
}
/// <summary>
/// 读取DO输出
/// "P4.009"
/// </summary>
/// <returns></returns>
public Int32 get_DO()
{//001b
string str = null;
int num = 0;
str = read_reg16bit("P4.009");
num = get_value(str);
return num;
}
/ <summary>
/ 强制DO输出
/ </summary>
/ <returns></returns>
//public string set_DO()
//{ // 输出极性 "P2.018"
// string str = read_reg16bit("P4.006");
// return str;
//}
public Int32 get_面板输入()
{
int num = 0;
string str = read_reg16bit("P4.008");
num = get_value(str);
return num;
}
public int get_软件版本()
{
int num = 0;
string str = read_reg32bit("P0.000");
num = get_value(str);
//"35秒728ms:7F0304279300009EAD"
//27930000
return num;
}
/// <summary>
/// 获取故障码
/// "P0.001"
/// </summary>
/// <param name="data"></param>
/// <returns></returns>
public int get_故障()
{//报警:故障码
int num = 0;
string str = read_reg32bit("P0.001");
num = get_value(str);
return num;
}
/// <summary>
/// 故障记录
/// "P4.000""P4.001""P4.002"P4.003""P4.004"
/// </summary>
/// <returns></returns>
public string get_故障记录()
{//13 0f21 0235 30 09
string[] str = new string[5];
str[0] = read_reg32bit("P4.000");//
string num0 = "第1故障:" + get_value(str[0]).ToString("X8");
str[1] = read_reg32bit("P4.001");//
string num1 = "第2故障:" + get_value(str[1]).ToString("X8");
str[2] = read_reg32bit("P4.002");//
string num2 = "第3故障:" + get_value(str[2]).ToString("X8");
str[3] = read_reg32bit("P4.003");//
string num3 = "第4故障:" + get_value(str[3]).ToString("X8");
str[4] = read_reg32bit("P4.004");//
string num4 = "第5故障:" + get_value(str[4]).ToString("X8");
return num0 + num1 + num2 + num3 + num4;
//return str.ToString();
}
/// <summary>
/// 伺服启动时间"P0.008"
/// </summary>
/// <returns></returns>
public int get_伺服启动时间()
{//报警:故障码
int num = 0;
string str = read_reg32bit("P0.008");
num = get_value(str);
return num;
}
/// <summary>
/// 获取电机位置
/// "P5.016"
/// </summary>
/// <returns></returns>
public int get_电机位置()
{//32bit
//P5.016
int num = 0;
//编码器位置 3秒397ms:7F03 04 96B0 00CB 09CC"
string str = read_reg32bit("P5.016");//轴位置-电机编码器
num = get_value(str);
return num;
}
public void get_CH1()
{//P0.003控制最大0x77 bit0是CH2功能设置
//p1.003
//p1.004
//p1.005
//P1.101
//P4.020偏移CH1校正mV
//模拟速度输入P4.022
}
public void get_CH2()
{//P0.003控制最大0x77 bit0是CH2功能设置
//P1.102
//P4.021偏移CH2校正mV
//模拟扭矩输入P4.023
}
#endregion
#region set
/// <summary>
/// 设置通讯从站(编号)
/// </summary>
/// <param name="add">默认0x7F</param>
public void set_address(byte add)
{
help_ModBus.address = add;// 设置站号
}
//====通信参数:287页========================================
//手动输入P2.008=0010 重新上电恢复出厂设置(出厂为 站号7F,波特率38400,n82 )
//P3. 000 = 0x007F 站号127
//P3. 001 = 0x0203 DCBA:A:(RS485波特率(0:4800,1:9600,2:19200,3:38400,4:57600,5:115200)
// //C: can(0:125Kbps,1:250Kbps,2:500kps,3:750Kbps,4:1Mbps)
// //D:0使用非轴卡,3使用台达轴卡DMCNET
//P3. 002 = 0x0006 DCBA:A:(6:8N2 (RTU8位无校验2停止)
// //(7:8E1 (RTU8位E偶校验1停止)
// //(8:8O1 (RTU数据位8,奇检验odd,停止位1)
//P3. 003 = 0x0000 DCBA:A:(0:警告并继续运转)
// //(1:通信故障处理:警告并减速(P5.003)
//P3. 004 = 0x0000 DCBA:A:(通讯逾时报警sec秒:1秒(调试用0)看门狗 0~20秒
//P3. 006 = 0x0000 DI开关来源(0:外部端子 1:内部P4.007控制),Bit 0 ~Bit 7 对应至 DI1 ~DI8(P2.010~P2.017)
// DI9 ~DI3(P2.036~P2.040)
// P2.010后2位是开关的功能第3位是控制接常开还是常闭
//P4. 007 = 00E0 bit对应软开关DI 0x0011(表示sdi5和sdi1为ON,软开关总共0x1fff)控制输入13个开关
//P3. 007 = 0x0000 通讯延时回复 0~1000ms + 0.5ms单位
/// <summary>
/// 初始化:基础通讯
/// 网口:4负(B) 5正(A)
/// </summary>
public void set_init_通讯初始化()//B3伺服:初始化寄存器 //这功能需要手动配置 // 通讯部分
{
//不要再建Task
string str;
//伺服P2.008=0x0010恢复出厂:从站0x7f,波特率38400,RTU模式,8数据,无校验,2停止位
//再改站号
//rs485.address = 0x7f;
//set_reg("P3.000", 0x0001);//站号 站号01
//set_reg("P3.001", 0x0201);//波特率 203:(can500k 串口38400) 201(can500k串口9600)
//set_reg("P3.002", 0x0008);//校验 6(8N2) 7(8E1) 8(8O1)
str = set_reg16bit("P3.003", 0x0001);//通讯故障处理 警告并减速
str = set_reg16bit("P3.004", 0x0000);//看门狗 0~20秒 通讯断开时报警 单位:秒
//str = set_reg16bit("P3.005", 0x0000);//伺服保留
str = set_reg16bit("P3.006", 0x01FFF);//输入开关源 DI输入端子选择,0:外部端子 1:内部寄存器控制 P4.007决定,Bit 0 ~Bit 7 对应至 DI1 ~DI8(P2.010~P2.017)
str = set_reg16bit("P3.007", 0x0000);// 1.5ms后应答 // 应答时间,通讯回复延迟时间 单位:0.5ms
}
/// <summary>
/// 纯PT模式
/// </summary>
public void set_init_基础参数()
{
#region 说明
//P1.001● 控制模式及控制命令输入源设定
//P1.002▲ 速度及扭矩限制设定
//P1.003 检出器脉冲输出极性设定
//P1.012 ~
//P1.014 内部扭矩指令 / 内部扭矩限制 1 ~3
//
//P1.044▲ 电子齿轮比分子 (N1)
//P1.045▲ 电子齿轮比分母 (M)
//P1.046▲ 检出器输出脉冲数设定
//P1.055 最大速度限制
//P1.097▲ 检出器输出(OA, OB)分母
//P5.003 自动保护的减速时间
//
//P5.020 ~
//P5.035 加 / 减速时间(编号#0 ~ 15)
//
//P5.016■ 轴位置-电机编码器
//P5.018 轴位置-脉冲命令
//
//P1.000▲ 外部脉冲列输入型式设定
//P2.060 电子齿轮比分子(N2)
//P2.061 电子齿轮比分子(N3)
//P2.062 电子齿轮比分子(N4)
//P5.008 软件极限:正向
//P5.009 软件极限:反向
//P6.002 ~
//P7.099 内部位置指令#1 ~ 99
//P5.060 ~
//P5.075
//内部位置指令控制#0 ~ 15 的移动速
//度设定
//P5.004 原点复归模式
//P5.005 第一段高速原点复归速度设定
//P5.006 第二段低速原点复归速度设定
//P5.007■ PR 命令触发缓存器
//P5.040 ~
// P5.055
//位置到达之后的 Delay 时间(编号
//#0 ~ 15)
//P5.098 事件上缘触发 PR 程序编号
//P5.099 事件下缘触发 PR 程序编号
//P5.015■PATH#1 ~ PATH#2 数据断电不记忆设定
//P1.001● 控制模式及控制命令输入源设定
//P1.002▲ 速度及扭矩限制设定
//P1.003 检出器脉冲输出极性设定
//P1.046▲ 检出器输出脉冲数设定
#endregion
string str, str1, str2;
#region 重要参数
//===========================================================================================================
str1 = set_reg16bit("P1.055", 3000);//最大转速限制
str2 = set_reg16bit("P1.056", 20);//预过载百分比0~120
/* 这参数很重要,低于导轨最大受力值 */
set_motor_防撞保护(30, 10);//防撞百分比 0~300力矩, 防撞时间过滤1~1000ms (P1.057,P1.058)
/* 这参数很重要,低于导轨最大受力值 */
set_PR_ZERO_机械回零(5, 2000, false);//回零力矩,确认时间20转2秒
//===伺服显示屏==============================================================================================
str1 = set_reg16bit("P0.002", (UInt16)LEDshow.L54电机目前实际扭力);//伺服显示屏:39 DI
//============================================================================================================
#endregion
str = set_reg16bit("P1.000", 0x1002);//脉冲+符号,
str1 = set_reg16bit("P1.001", (ushort)mode工作模式.PT | 0x100 | 0x0000);//PR+T混合模式 + 电机方向 + DIO
str2 = set_reg16bit("P1.002", 0x0000);//D1力矩 D0速度 限制(必须关闭)//力矩值选择P1.012,13,14
str = set_reg16bit("P1.003", 0x0000);//监视输出。0: MON1(+), MON2(+)
str1 = set_reg16bit("P0.003", 0x0010);//MON2 速度 MON1 力矩
str2 = set_reg16bit("P1.004", 100);//MON1 模拟监控输出比例
str = set_reg16bit("P1.005", 100);//MON2 模拟监控输出比例
str1 = set_reg16bit("P1.022", 0x0000);// PR 命令滤波器 禁止反转:0关闭
str2 = set_reg16bit("P1.032", 0x0060);//动态刹车再空转 p1.38 零速 AL22主电源异常
str = set_reg16bit("P1.038", 10);//参数定义零速信号条件 默认10转
str1 = set_reg16bit("P1.039", 1000);//目标速度:转速到达信号条件DO输出脚(TSPD)
//set_reg("P1.041", 50);//CH模拟量输入扭矩比例 默认100
str2 = set_reg16bit("P1.042", 500);//伺服使能后电磁刹车释放延时ms
str = set_reg16bit("P1.043", 500);//脱机后刹车抱闸延时ms
//===============================================
//set_reg("P1.044", 16777216);//分子1677 7216
//set_reg("P1.045", 3600);//分母 10个脉冲1度
//===============================================
//set_reg32bit("P1.054", 167772);//位置确认范围 0.1度
//set_reg("P1.064", 0);//模拟量输入控制电机旋转角度 PT 0关
//set_reg("P1.066", 0);//模拟位置+10v圈数DEC 0~2000(200圈)
//set_reg("P1.074", 0);//编码器ABZ位置输出源
//set_reg("P1.012", 100);//单边扭力限制 -500~500 P1.012~P1.014
//set_reg("P1.014", 100);//单边扭力限制 -500~500 P1.012~P1.014
str = set_reg16bit("P1.098", 100);//断线侦测保护(UVW)反应时间 ms 0~800ms
//set_reg("P2.050", 1);//脉冲清除
//set_reg("P2.052", 1073.741824);//分度总行程
}
/// <summary>
/// 纯外部端子脉冲输入
/// 要重启
/// </summary>
public void set_PT工作模式()
{//要重启
string str1 = set_reg16bit("P1.001", (ushort)mode工作模式.PT | 0x000 | 0x0000);//PR+T混合模式 + 电机方向 + DIO
string str2 = set_reg16bit("P1.002", 0x0000);//D1力矩 D0速度 限制(必须关闭)//力矩值选择P1.012,13,14
}
/// <summary>
/// 总线定位
/// 要重启
/// </summary>
public string set_PR工作模式()
{
string str1 = String.Empty, str2;
str1 = set_reg16bit("P1.001", (ushort)mode工作模式.PR | 0x000 | 0x0000);//PR+T混合模式 + 电机方向 + DIO
str2 = set_reg16bit("P1.002", 0x0000);//D1力矩 D0速度 限制(必须关闭)//力矩值选择P1.012,13,14
return str1;
}
/// <summary>
/// 总线定位+力矩
/// 要重启
/// </summary>
/// <param name="data">力矩</param>
public string set_PRt工作模式(UInt16 t1,UInt16 t2,UInt16 t3)
{
string str1 = String.Empty, str2;
str1 = set_reg16bit("P1.001", (ushort)mode工作模式.PR_T | 0x100 | 0x0000);//PR+T混合模式 + 电机方向 + DIO
//开启 力矩
str2 = set_reg16bit("P1.002", 0x0010);//D1力矩 D0速度 限制(必须关闭)//力矩值选择P1.012,13,14
//力矩 1
str2 = set_reg16bit("P1.012", t1);// 1号:力矩百分比
//力矩 2
str2 = set_reg16bit("P1.013", t2);//T2
//力矩 3
str2 = set_reg16bit("P1.014", t3);//T3
return str1;
}
/// <summary>
/// 设置DI "P4.007"
/// </summary>
/// <param name="start"></param>
/// <returns></returns>
public int set_DI(Int32 start)
{//注意:不能用Task (上一级已经用了task了)
//P4.007 bit对应开关状态
// 说明书3.3.2节 +0x100 常开
// DI 1 P2.010 01 SON 伺服启动
// DI 2 P2.011 08 CTRG 内部位置命令触发 呼叫PR#
// DI 3 P2.012 11 POS—0 内部位置命令选择 0 PR#码段
// DI 4 P2.013 12 POS—1
// DI 5 P2.014 16 TCM—0 力矩码段(索引)
// DI 6 P2.015 17 TCM—1
// DI 7 P2.016 20 T-P 扭矩/位置混和模式命令---选择切换 高电平:P 定位带力矩模式 低电平:纯力矩模式(正反都使能是:阻力模式)
// DI 8 P2.017 21 EMGS 紧急停止
// DI 9 P2.036 02 ARST 异常重置
// DI 10 P2.037 37 正点动
// DI 11 P2.038 38 负点动
// DI 12 P2.039 46 停车钮
// DI 13 P2.040 100 关 常开
int str = get_value(set_reg16bit("P4.007", start));//0x1fff bit 12~0 对应输入开关 Di 13~0
return str;
}
/// <summary>
/// 模拟输出监控 P0.003
/// 0 电机速度
/// 1 电机扭矩
/// 2 脉冲命令频率
/// 3 速度命令
/// 4 扭矩命令
/// 5 VBUS 电压
/// 6 P1.101 的设定值
/// 7 P1.102 的设定值
/// </summary>
/// <param name="MON1">0~7</param>
/// <param name="MON2">0~7</param>
/// <returns></returns>
public string set_模拟输出监控(Int32 MON1, Int32 MON2)
{//模拟输出监控
string str = read_reg32bit("P0.003");
return str;
}
/// <summary>
/// 驱动器显示//ushort num = (ushort)(LEDshow)Enum.Parse(typeof(LEDshow), comboBox1.Text);
/// </summary>
/// <param name="led"></param>
public void set_LED显示(LEDshow led)
{
try
{
//ushort num = (ushort)(LEDshow)Enum.Parse(typeof(LEDshow), comboBox1.Text);
ushort num = (ushort)led;
set_reg16bit("P0.002", num);
}
//catch (Exception ex) { MessageBox.Show(ex.Message); }
catch (Exception ex) { System.Windows.MessageBox.Show(ex.Message); }//显示错误
}
public int set_JOG点动速度(UInt16 speed)
{
string str = set_reg16bit("P4.005", speed);//点动速度
int end = get_value(str);
return end;
}
/// <summary>
/// "P5.008"
/// </summary>
/// <param name="data"></param>
/// <returns></returns>
public int set_正软限位(Int32 data)
{
int end = 0;
string str = set_reg32bit("P5.008", data);
end = get_value(str);
return end;
}
/// <summary>
/// "P5.009"
/// </summary>
/// <param name="data"></param>
/// <returns></returns>
public int set_负软限位(Int32 data)
{
int end = 0;
string str = set_reg32bit("P5.009", data);
end = get_value(str);
return end;
}
/// <summary>
/// 电子齿轮比
/// "P1.044"分子
/// </summary>
/// <param name="a_分子"></param>
/// <param name="b_分母"></param>
public int[] set_电子齿轮比(int a_分子, int b_分母,double d_导程,double f_负限位,double z_正限位)
{
int[] clb = new int[2];
clb[0] = get_value(set_reg32bit("P1.044", a_分子));//16777216 //这个固定,编码器一圈脉冲
clb[1] = get_value(set_reg32bit("P1.045", b_分母));// 360 //360表示一圈脉冲
//默认一圈 十万脉冲。
set_负软限位((int)( f_负限位/d_导程 * 16777216));
set_正软限位((int)(z_正限位/d_导程* 16777216));
return clb;
}
/// <summary>
/// 精度:位置确认范围"P1.054",误差报警值"P2.035"
/// 默认167772
/// 167772/16777216 //24bit
/// // 正负 1%度
/// </summary>
/// <param name="num">电机一圈16777216</param>
public int set_精度(Int32 num)
{//十进制32bit
//误差报警阀值"P2.035" //默认50331648
//编码器1677721600(360 / 0.33份)
int numend = 0;
numend = get_value(set_reg32bit("P1.054", num));//位置确认范围
return numend;
}
public int set_最大速度(UInt16 speed)
{
int sd = 0;
sd = get_value(set_reg16bit("P1.055", speed));//最大速度
return sd;
}
public int set_过负载警告(UInt16 n120)
{
int re = 0;
re = get_value(set_reg16bit("P1.056", n120));//最大速度
return re;
}
/// <summary>
/// 力矩报警阀值
/// P1.057防撞百分比 0~300
/// P1.058时间1~1000ms
/// </summary>
/// <param name="max">0~300%</param>
/// <param name="time_ms">1~1000ms</param>
public void set_motor_防撞保护(ushort max, ushort time_ms)//力矩报警阀值 , ms
{
//要小于导轨承受值
string str;
str = set_reg16bit("P1.057", max);//50 防撞百分比 0~300 P0.002=54电机实时扭力
str = set_reg16bit("P1.058", time_ms);//10 防撞时间过滤1~1000ms 2个值都超过时AL30
}
/// <summary>
/// 内部扭矩限制 1
/// "P1.002"=0x0010
/// "P1.012"
/// </summary>
/// <param name="mun"></param>
public int set_限制力矩(ushort lj)
{
int re = 0;
//set_reg("P1.002", 0x0000);//关闭扭矩限制(必须关闭)//力矩值P1.012,13,14
// UZYX x:限速 y:限力 0关1开
//速度 P1.009 , P1.010 ,P1.011
//力矩 P1.012 , P1.013 ,P1.014
string str = set_reg16bit("P1.002", 0x0010);//限力限速模式//力矩值P1.012,13,14
re = get_value(set_reg16bit("P1.012", lj));//力矩1号 //需要TCM0
return re;
}
/// <summary>
/// 回零力矩P1.87,确认延时P1.88(2~2000ms)
/// v1.02版本 基础值是10+设定值
/// </summary>
/// <param name="data">力矩</param>
/// <param name="time_ms">时间,小于2000ms</param>
public void set_motor_回零力矩(int data, int time_ms)// "P1.087" //力矩报警阀值 扭力回零准位 1~300% /扭力计时 2~2000ms
{//v1.02版本是( 值+10 )
/* 这参数很重要,低于导轨最大受力值 */
//if (data > 20)
//{
// data = 20;
//}
string str = set_reg16bit("P1.087", data);//扭力回零准位 1~300%
if (time_ms < 2)
{
time_ms = 2;
}
if (time_ms > 2000)
{
time_ms = 2000;
}
str = set_reg16bit("P1.088", time_ms);//扭力计时 2~2000ms
}
/// <summary>
/// 机械回零
/// 呼叫执行 PR# 0 回原点 P5.007=0;
/// </summary>
/// <param name="data">回零力矩</param>
/// <param name="time_ms">确认时间,小于2000ms</param>
/// <param name="run">立即执行bool</param>
public void set_PR_ZERO_机械回零(ushort data, ushort time_ms, bool run)//回零力矩,确认时间ms,立即回零
{//机械回零操作
string str;
/* 这参数很重要,低于导轨最大受力值 */
set_motor_回零力矩(data, time_ms);//"P1.087"力矩报警阀值 扭力回零准位 1~300% /扭力计时 2~2000ms
//set_motor_防撞保护(50, 10);//防撞百分比 0~300, 防撞时间过滤1~1000ms
str = set_reg16bit("P5.004", 0x010A);//机械回零模式:超程反转,
str = set_reg32bit("P5.005", 400);//回零第一次高速复归速度设定 400转
str = set_reg32bit("P5.006", 100); //回零第二次低速复归速度设定 100转
//PR# 0 功能定义
//D7 0 上电时:0停止 1立即机械回零
//D6 0 空
//D5 0 延时时间索引 0(P5.040) ~ F(P5.055)
//D4 0 第2次回零减速时间索引 0(P5.020) ~ F(P5.035) P5.006=200转
//D3 0 第1次回零减速时间索引 0(P5.020) ~ F(P5.035) P5.005=1000转 (200ms~8000ms) 0.200ms 1.300ms 2.500ms 3.600ms 4.800ms 5.900ms 6.1000ms //F.30ms(范围1~1200ms)
//D2 0 自动加速时间索引 0(P5.020) ~ F(P5.035)
//
//D1 0 回零之后 00(停止)01(#1) ~63(#99)
//D0 0
str = set_reg32bit("P6.000", 0x00000000);//32bit原点复归定义 //参数定义 DCBA UZYX (YX00#编号)z加速时间P5.020 u第一减速 a第二减速 b DLY延时 d 1开机回零点
str = set_reg32bit("P6.001", 0);//32bit原点定义值 //参考点偏移 ?( Z脉冲宽度大概152~156
//PR#
if (run)
{
str = set_reg16bit("P5.007", 0);//bit16直接呼叫 PR#0 机械回零
}
}
public string set_CH1校正(UInt16 data)
{
string str = string.Empty;
str = set_reg16bit("P2.008", 22);
str = set_reg16bit("P4.020", data);//P2.008要开 22
return str;
}
public string set_CH2校正(UInt16 data)
{
//Task.Run(() => { });
string str = set_reg16bit("P2.008", 22);
str = set_reg16bit("P4.021", data);//P2.008要开 22
return str;
}
/// <summary>
/// 呼叫PR# 1~99
/// "P5.007"
/// </summary>
/// <param name="num"></param>
public void set_PR呼叫(ushort num)//"P5.007"
{
//Task.Run(() => { });
set_reg16bit("P5.007", num);
}
#endregion
#endregion
#region PR+T 模式(混合模式:定位加力矩)
//=======定位加力矩========================================================
#region 开关量初始化
/// <summary>
/// PR+T 模式初始化DI
/// 设置DI 初始化 开关量 端子输入
/// </summary>
public void set_DI_init_PRT()// PR+T 输入初始化
{
#region 说明
//L型机:
//(方向:35+24v,39+3.3v,37-GND)
//(脉冲:36+24v,43+3.3v,41-GND)
//18力矩输入
//20速度输入
//===============================
//极性和功能// | 0x100是常开
//DI1 ~ DI8: P2.010 ~ P2.017
//DI9 ~DI13: P2.036 ~ P2.040
//来源
//P3.006 输入接点(DI)来源 Bit 12 ~ Bit 0 对应至 DI13 ~ DI1;
//0:输入接点状态由外部硬件端子控制。
//1:输入接点状态由系统参数 P4.007 控制。
//P4.007 bit对应开关状态
// 说明书3.3.2节 +0x100 常开
// DI 1 P2.010 01 SON 伺服启动
// DI 2 P2.011 08 CTRG 内部位置命令触发 呼叫PR#
// DI 3 P2.012 11 POS—0 内部位置命令选择 0 PR#码段
// DI 4 P2.013 12 POS—1
// DI 5 P2.014 16 TCM—0 力矩码段
// DI 6 P2.015 17 TCM—1
// DI 7 P2.016 20 T-P 扭矩/位置混和模式命令---选择切换 高电平:力矩模式
// DI 8 P2.017 21 EMGS 紧急停止
// DI 9 P2.036 02 ARST 异常重置
// DI 10 P2.037 37 正点动
// DI 11 P2.038 38 负点动
// DI 12 P2.039 46 STP_电机停止
// DI 13 P2.040 100 软件读取位(拉高,以免读取DI是00)
#endregion
//输入开关信号滤波 0~20ms
set_reg16bit("P2.009", 0);//开关过滤杂波 0~20 单位:ms 毫秒
//===DI源===============================
set_reg16bit("P3.006", 0x1FFF);//开关量来源0x1FFF(bit12~0) 1内部 P4.007
//===断电丢失=============================
set_reg16bit("P2.010", 0x100 | (ushort)io_in_功能.SON_伺服使能);//DI 1端子定义 01 DCBA:C(0:接常闭,1接常开)BA功能码
set_reg16bit("P2.011", 0x100 | (ushort)io_in_功能.CTRG_PR模式节拍_上升沿);//DI 2端子定义
set_reg16bit("P2.012", 0x100 | (ushort)io_in_功能.POS0_PR模式码段);//DI 3端子定义
set_reg16bit("P2.013", 0x100 | (ushort)io_in_功能.POS1_PR模式码段);//DI 4端子定义
set_reg16bit("P2.014", 0x100 | (ushort)io_in_功能.TCM0_多段力矩切换);//DI 5端子定义
set_reg16bit("P2.015", 0x100 | (ushort)io_in_功能.TCM1_多段力矩切换);//DI 6端子定义
set_reg16bit("P2.016", 0x100 | (ushort)io_in_功能.T_P_力矩位置模式);//DI 7端子定义
set_reg16bit("P2.017", (ushort)io_in_功能.EMGS_急停开关);//DI 8端子定义 // 不接开关应为急停报警 AL13
set_reg16bit("P2.036", 0x100 | (ushort)io_in_功能.ARST_复位);//DI 9端子定义
set_reg16bit("P2.037", 0x100 | (ushort)io_in_功能.JOGU_正点动);//DI 10端子定义
set_reg16bit("P2.038", 0x100 | (ushort)io_in_功能.JOGU_负点动);//DI 11端子定义
set_reg16bit("P2.039", 0x100 | (ushort)io_in_功能.STP_电机停止);//DI 12端子定义
//set_reg("P2.040", (ushort)io_in_功能.ARST_复位 | 0x100);//DI 13端子定义
//初始化开关状态
set_DI(0 |//0x0051
1 << 0 //DI1 使能
| 0 << 1 //DI2
| 0 << 2 //DI3
| 0 << 3
| 1 << 4 //DI5 力矩1
| 0 << 5
| 1 << 6//DI7 0纯力矩 1定位+力矩
| 0 << 7
| 0 << 8
| 0 << 9
| 0 << 10
| 0 << 11
| 0 << 12//DI13脚。
);
}
/// <summary>
/// PR+T 模式初始化DO
/// 设置D0初始化 端子输出
/// </summary>
public void set_DO_init_PRT()// PR+T 输出初始化
{
// + 0x100是常开
// DO 1 01 SRDY 伺服准备完成
// DO 2 03 ZSPD 电机零速度
// DO 3 04 TSPD 目标速度到达
// DO 4 05 TPOS 目标位置到达
// DO 5 07 ALRM 伺服警示
// DO 6 端子定义100 关
set_reg16bit("P2.018", 0x100 | (ushort)io_out_功能.SRDY_伺服上电);//DO 1端子定义101 电源
set_reg16bit("P2.019", 0x100 | (ushort)io_out_功能.ZSPD_零速);//DO 2端子定义103 零速
set_reg16bit("P2.020", 0x100 | (ushort)io_out_功能.TSPD_到速);//DO 3端子定义109 原点复归完成, 坐标有效
set_reg16bit("P2.021", 0x100 | (ushort)io_out_功能.TPOS_到位);//DO 4端子定义105 坐标到位信号(参数 P1.054 设定值)
set_reg16bit("P2.022", 0x100 | (ushort)io_out_功能.ALRM_故障);//DO 5端子定义007 伺服故障
set_reg16bit("P2.041", 0x100 | (ushort)io_out_功能.BRKR_刹车线);//DO 6端子定义100 关 0x100是常开
}
#region IO 操作
/// <summary>
/// 按钮开关:置位
/// </summary>
/// <param name="key">开关位置</param>
/// <returns></returns>
public bool key_置位(UInt32 key)
{
UInt32 start = (UInt32)get_DI();//获取开关量 //0x00000051
if (start == 0)//错误帧
{//不应该进入这里,应答数据帧错了
string str = rs485.help_SerialPort.get_应答报文.ToString();//检查
return false;
}
UInt32 end = start;//记录原始数据
start &= key; //获取开关状态 start 0x00000001
set_DI((int)(end |= key)); //开
return true;
}
/// <summary>
/// 按钮开关:复位
/// </summary>
/// <param name="key">开关位置</param>
/// <returns></returns>
public bool key_复位(UInt32 key)
{
UInt32 start = (UInt32)get_DI();//获取开关量 //0x00000051
if (start == 0)//错误帧
{//不应该进入这里,应答数据帧错了
string str = rs485.help_SerialPort.get_应答报文.ToString();//检查
return false;
}
UInt32 end = start;//记录原始数据
start &= key; //获取开关状态 start 0x00000001
//& 关
key = ~key;//0xfffffffe
end &= key; //bit 关
set_DI((int)end);//执行关闭
return false;
}
/// <summary>
/// 按钮开关:翻转
/// </summary>
/// <param name="key"></param>
/// <returns></returns>
public int key_翻转(UInt32 key)
{//不能用Task(不能让本层与ui并列运行)
UInt32 k_开关 = key;
UInt32 k_复位 = ~key;
UInt32 k_状态 = 0;
UInt32 start = (UInt32)get_DI();//获取开关量 //0x00000051
if (start == 0)//错误帧
{//不应该进入这里,应答数据帧错了
string str = rs485.help_SerialPort.get_应答报文.ToString();//检查
key_置位((int)io_DI.ARST复位);//复位驱动器(使下次读,不会为0)
return 4;
}
k_状态 = start;//记录原始数据
k_状态 = start & key; //获取开关状态 start 0x00000001
if (k_状态 == 0)
{//====开==============================
set_DI((int)(start |= key));
//====调查结果========================
int st1 = get_DI();
int st = st1 & (int)key;
if (st > 0)
{
return 1; // 结果 ok
}
return 0;
}
else
{//====关==============================
k_状态 = start & k_复位;//bit 关
set_DI((int)k_状态);//执行关闭
//====调查结果========================
int st1 = get_DI();
int st = st1 & (int)key;
if (st > 0)
{
return 1; // 结果 ok
}
return 0;
}
}
//set_DI(ushort start)
public int set_DI_使能_PRT()
{
return key_翻转((int)io_DI.伺服使能);
//set_DI(0 |
// 1 << 0 //使能
// //| 1 << 1// PR# 跳跃
// //| 1 << 2// pos0 索引
// //| 1 << 3// pos1
// | 1 << 4// t0 力矩
// //| 1 << 5// t1
// | 1 << 6// t/p 切换(0:力矩模式 1:PR模式)
// //| 1 << 7// 急停 (默认常闭,所以通电取反)
// //| 1 << 8// 复位
// //| 1 << 9// 正
// //| 1 << 10//反
// //| 1 << 11
// //| 1 << 12
// );
}
public bool set_b备用使能()
{
set_DI(0 |
1 << 0 //使能
//| 1 << 1// PR# 跳跃
//| 1 << 2// pos0 索引
//| 1 << 3// pos1
| 1 << 4//力矩t0
//| 1 << 5//力矩t1
| 1 << 6// t/p 切换(0:纯力矩模式 1:PR模式)
//| 1 << 7// 急停
//| 1 << 8// 复位
//| 1 << 9// 正
//| 1 << 10//反
//| 1 << 11
//| 1 << 12
);
return true;
}
/// <summary>
/// T/P 切换
/// 0:T力矩模式
/// 1:P定位+T力矩模式
/// </summary>
/// <returns></returns>
public int set_DI_TP切换_PRT()
{// 0:t纯力矩 1:p定位带力矩
return key_翻转((int)io_DI.TP力定切换);
//key_置位((int)io_DI.TCM0力矩);//力矩1号
//return key_翻转((int)io_DI.TP力定切换);
//Task.Run(() =>
//{
//return set_DI(0
// //1 << 0 //使能
// //| 1 << 1// PR#
// //| 1 << 2// pos0 位置
// //| 1 << 3// pos1
// //| 1 << 4// t0 力矩
// //| 1 << 5// t1
// | 1 << 6// t/p 切换(0:力矩模式 1:PR模式)
// //| 1 << 7// 急停 (默认常闭,所以通电取反)
// //| 1 << 8// 复位
// //| 1 << 9// 正
// //| 1 << 10//反
// //| 1 << 11
// //| 1 << 12
// );
//});
}
public int set_DI_急停_PRT()
{
return key_翻转((int)io_DI.EMGS急停);
//P4.007 bit对应开关状态
// 说明书3.3.2节 +0x100 常开
// DI 1 P2.010 01 SON 伺服启动
// DI 2 P2.011 08 CTRG 内部位置命令触发 呼叫PR#
// DI 3 P2.012 11 POS—0 内部位置命令选择 0 PR#码段
// DI 4 P2.013 12 POS—1
// DI 5 P2.014 16 TCM—0 力矩码段
// DI 6 P2.015 17 TCM—1
// DI 7 P2.016 20 T-P 扭矩/位置混和模式命令---选择切换 高电平:PR模式
// DI 8 P2.017 21 EMGS 紧急停止
// DI 9 P2.036 02 ARST 异常重置
// DI 10 P2.037 37 正点动
// DI 11 P2.038 38 负点动
// DI 12 P2.039 100 关 常开
// DI 13 P2.040 100 关 常开
复位
//set_DI(0
// //1 << 0 //使能
// //| 1 << 1// PR#
// //| 1 << 2// pos0 位置
// //| 1 << 3// pos1
// //| 1 << 4// t0 力矩
// //| 1 << 5// t1
// | 1 << 6// t/p 切换(0:力矩模式 1:PR模式)
// //| 1 << 7// 急停 (默认常闭,所以通电取反)
// | 1 << 8// 复位
// //| 1 << 9// 正
// //| 1 << 10//反
// //| 1 << 11
// //| 1 << 12
// );
}
public void set_DI_复位_PRT()
{
key_翻转((int)io_DI.ARST复位);
//P4.007 bit对应开关状态
// 说明书3.3.2节 +0x100 常开
// DI 1 P2.010 01 SON 伺服启动
// DI 2 P2.011 08 CTRG 内部位置命令触发 呼叫PR#
// DI 3 P2.012 11 POS—0 内部位置命令选择 0 PR#码段
// DI 4 P2.013 12 POS—1
// DI 5 P2.014 16 TCM—0 力矩码段
// DI 6 P2.015 17 TCM—1
// DI 7 P2.016 20 T-P 扭矩/位置混和模式命令---选择切换 高电平:PR模式
// DI 8 P2.017 21 EMGS 紧急停止
// DI 9 P2.036 02 ARST 异常重置
// DI 10 P2.037 37 正点动
// DI 11 P2.038 38 负点动
// DI 12 P2.039 100 关 常开
// DI 13 P2.040 100 关 常开
复位
//set_DI(0
// //1 << 0 //使能
// //| 1 << 1// PR#
// //| 1 << 2// pos0 位置
// //| 1 << 3// pos1
// //| 1 << 4// t0 力矩
// //| 1 << 5// t1
// | 1 << 6// t/p 切换(0:力矩模式 1:PR模式)
// //| 1 << 7// 急停 (默认常闭,所以通电取反)
// | 1 << 8// 复位
// //| 1 << 9// 正
// //| 1 << 10//反
// //| 1 << 11
// //| 1 << 12
// );
}
/// <summary>
/// 正点动
/// </summary>
/// <param name="speed">速度</param>
public void set_DI_jogu_正点动_PRT(UInt16 speed)
{//正转
set_reg16bit("P4.005", speed); //点动速度
key_翻转((int)io_DI.正转);
}
/// <summary>
/// 负点动
/// </summary>
/// <param name="speed">速度</param>
public int set_DI_jogu_负点动_PRT(UInt16 speed)
{//反转
set_reg16bit("P4.005", speed); //点动速度
return key_翻转((int)io_DI.反转);
}
#endregion
#endregion
#region 初始化 PR+T
/// <summary>
/// PR+T模式
/// 0:纯力矩 1:定位加力矩 (T/P 引脚)
/// </summary>
public void set_PRT_Mode_init_定位加力矩模式_初始化() // 模式初始化
{//以这个为主
#region 说明
//伺服模式PR
//P5.004=0x0109 原点复归模式
//P5.005 第一段高速原点复归速度设定
//P5.007■ PR 命令触发缓存器写入 0 表示开始原点复归;
//写入 1 ~ 99 表示开始执行指定 PR 程序,相当于 DI.CTRG + POSn。
//P5.008 软件极限:正向
//P5.009 软件极限:反向
//P5.015=0x0011 ■ PATH#1 ~ PATH#2 数据断电不记忆设定
//P5.016■ 轴位置-电机编码器
//P5.018 轴位置-脉冲命令
//
//P6.000=0x 1000 0000 原点复归定义
//P6.001 原点坐标
//
//P6.002=0x0000 0011, #1
//P6.003=坐标
//P6.004=0x0000 0011, #2
//P6.005=坐标
//
#endregion
string str, str1, str2;
#region 重要参数
//===========================================================================================================
//set_电子齿轮比(1677726, 100000,5,-1,4900.00);//167 77216 / 10 0000
set_电子齿轮比(1, 1, 5.0, -1, 485.00);//167 77216 / 10 0000 (127.99圈* 5mm导程)
//====齿轮比需要手动设置====================================================================================
str1 = set_reg16bit("P1.055", 3000);//最大转速限制
str2 = set_reg16bit("P1.056", 20);//预过载百分比0~120
/* 这参数很重要,低于导轨最大受力值 */
set_motor_防撞保护(40, 10);//防撞百分比 0~300力矩, 防撞时间过滤1~1000ms P1.057
/* 这参数很重要,低于导轨最大受力值 */
set_PR_ZERO_机械回零(4, 2000, false);//回零力矩,确认时间20转2秒
//===伺服LED显示屏========================================================开机显示=============
//str1 = set_reg16bit("P0.002", (UInt16)LEDshow.L39DI状态);//伺服显示屏:39 DI
str1 = set_reg16bit("P0.002", (UInt16)LEDshow.L0电机坐标PUU理论坐标);
str = set_reg16bit("P1.000", 0x1002);//脉冲+符号,
str1 = set_reg16bit("P1.001", (ushort)mode工作模式.PR_T | 0x100 | 0x0000);//PR+T混合模式 + 电机方向 + DIO
str2 = set_reg16bit("P1.002", 0x0010);//开启扭力限制 //D1力矩 D0速度 //力矩值选择P1.012,13,14 (由DI的TCM0~1决定力矩来源)
str2 = set_reg16bit("P1.012", 13);//开启扭力限制(1号扭力值: XX牛米)
set_DI_init_PRT();// PR-T 输入引脚初始化
set_DO_init_PRT();
//======================================================
Gcode_init_G代码初始化();//G00和G01的功能
#endregion
str = set_reg16bit("P1.003", 0x0000);//监视输出。0: MON1(+), MON2(+)
str1 = set_reg16bit("P0.003", 0x0010);//MON2 速度 MON1 力矩
str2 = set_reg16bit("P1.004", 100);//MON1 模拟监控输出比例
str = set_reg16bit("P1.005", 100);//MON2 模拟监控输出比例
str1 = set_reg16bit("P1.022", 0x0000);// PR 命令滤波器 禁止反转:0关闭
str2 = set_reg16bit("P1.032", 0x0060);//动态刹车再空转 p1.38 零速 AL22主电源异常
str = set_reg16bit("P1.038", 10);//参数定义零速信号条件 默认10转
str1 = set_reg16bit("P1.039", 1000);//目标速度:转速到达信号条件DO输出脚(TSPD)
//set_reg("P1.041", 50);//CH模拟量输入扭矩比例 默认100
str2 = set_reg16bit("P1.042", 20);//伺服使能后电磁刹车释放延时ms(最大1000ms)
str = set_reg16bit("P1.043", 20);//脱机后刹车抱闸延时ms
//===============================================
//set_reg("P1.044", 16777216);//分子1677 7216
//set_reg("P1.045", 3600);//分母 10个脉冲1度
//===============================================
//set_reg32bit("P1.054", 167772);//位置确认范围 0.1度
//set_reg("P1.064", 0);//模拟量输入控制电机旋转角度 PT 0关
//set_reg("P1.066", 0);//模拟位置+10v圈数DEC 0~2000(200圈)
//set_reg("P1.074", 0);//编码器ABZ位置输出源
//set_reg("P1.012", 100);//单边扭力限制 -500~500 P1.012~P1.014
//set_reg("P1.014", 100);//单边扭力限制 -500~500 P1.012~P1.014
str = set_reg16bit("P1.098", 100);//断线侦测保护(UVW)反应时间 ms 0~800ms
//set_reg("P2.050", 1);//脉冲清除
//set_reg("P2.052", 1073.741824);//分度总行程
//=== PR# 1 ==============================
// G00 快速定位
//str = set_reg32bit("P5.075", (UInt16)(speed * 10.0));//速度索引F段 :设置速度 倍率10
//str = set_reg32bit("P5.021", 300);//加速索引1段 :设置时间 ms
//str = set_reg32bit("P5.022", 200);//减速索引2段 :设置时间 ms
//=== PR# 2 ==============================
// G01 金加工速度
//str = set_reg32bit("P5.035", 30);//加速索引F段 :设置时间 ms
//str = set_reg32bit("P5.034", 20);//减速索引E段 :设置时间 ms
}
#endregion
#region 定位操作
public void Gcode_init_G代码初始化()
{
string str = string.Empty;
//G00索引
str = set_reg32bit("P5.021", 3000);//加速索引1段 :设置时间 ms
str = set_reg32bit("P5.022", 200);//减速索引2段 :设置时间 ms
//D7 0 空
//D6 0 自动下一步PR# 0关
//D5 0 延时时间索引 0(P5.040) ~ F(P5.055) 0ms~5500毫秒 //定位完成后再延时0ms
//D4 F SPD目标速度索引 0(P5.060) ~ F(P5.075) 20转~3000转 0.20转 1.50转 2.100 3.200 4.300 5.500 6.600 7.800 8.1000 9.1300 A.1500 B.1800 C.2000 D.2300 E.2500 F.3000转
//D3 2 DEC减速时间索引 0(P5.020) ~ F(P5.035) 200ms~8000ms 0.200ms 1.300ms 2.500ms 3.600ms 4.800ms 5.900ms 6.1000ms //F.30ms(范围1~1200ms)
//D2 1 ACC加速时间索引 0(P5.020) ~ F(P5.035) //F.30ms(范围1~1200ms)
//D1 3 OPT工作模式下的补充(TYPE工作模式2时) bit3bit2CMD=00ABS绝对定位 01REL相对定位 10INC光栅尺绝对定位 11CAP光栅尺相对定位 bit1本段PR#进入减速时允许下一PR#加入 bit0允许打断前一任务
//D0 2 TYPE工作模式: 1定速 2定位单步 3自动定位PR#下一步 7呼叫PR#跳跃 8写参数到路径 A分度定位 (240页)
str = set_reg32bit("P6.002", 0x000F2132);//单步定位,绝对值定位,允许在减速时插入下一PR#路径,允许打断当前旋转, 速度来源 F段速度,
//G01索引
str = set_reg32bit("P5.035", 30);//加速索引F段 :设置时间 ms
str = set_reg32bit("P5.034", 20);//减速索引E段 :设置时间 ms
str = set_reg32bit("P6.004", 0x000Aef32);//A段速度<1500
}
/// <summary>
/// G00快速定位(速度,坐标)
/// </summary>
/// <param name="speed">速度</param>
/// <param name="where">坐标</param>
/// <param name="run">立即旋转</param>
/// <param name="axis">轴地址:从站号</param>
public void G00(AXIS axis, double where, double speed, bool run)
{//PR#1
string str = string.Empty;
if (speed > 30000) // 3000转
{
speed = 30000;
}
if (axis == AXIS.X)
{
help_ModBus.address = 0x7f;//从站
}
else { return ; }
//str = set_reg32bit("P5.075", (UInt16)(speed * 10.0));//速度索引F段 :设置速度 倍率10
str = set_reg32bit("P5.075", (UInt16)(speed));
//str = set_reg32bit("P5.021", 300);//加速索引1段 :设置时间 ms
//str = set_reg32bit("P5.022", 200);//减速索引2段 :设置时间 ms
//D7 0 空
//D6 0 自动下一步PR# 0关
//D5 0 延时时间索引 0(P5.040) ~ F(P5.055) 0ms~5500毫秒 //定位完成后再延时0ms
//D4 F SPD目标速度索引 0(P5.060) ~ F(P5.075) 20转~3000转 0.20转 1.50转 2.100 3.200 4.300 5.500 6.600 7.800 8.1000 9.1300 A.1500 B.1800 C.2000 D.2300 E.2500 F.3000转
//D3 2 DEC减速时间索引 0(P5.020) ~ F(P5.035) 200ms~8000ms 0.200ms 1.300ms 2.500ms 3.600ms 4.800ms 5.900ms 6.1000ms //F.30ms(范围1~1200ms)
//D2 1 ACC加速时间索引 0(P5.020) ~ F(P5.035) //F.30ms(范围1~1200ms)
//D1 3 OPT工作模式下的补充(TYPE工作模式2时) bit3bit2CMD=00ABS绝对定位 01REL相对定位 10INC光栅尺绝对定位 11CAP光栅尺相对定位 bit1本段PR#进入减速时允许下一PR#加入 bit0允许打断前一任务
//D0 2 TYPE工作模式: 1定速 2定位单步 3自动定位PR#下一步 7呼叫PR#跳跃 8写参数到路径 A分度定位 (240页)
//str = set_reg32bit("P6.002", 0x000F2132);//单步定位,绝对值定位,允许在减速时插入下一PR#路径,允许打断当前旋转, 速度来源 F段速度,
str = set_reg32bit("P6.003", (int)(where*16777216/5.0));//PR# 1坐标(G00)
if (run) set_PR呼叫(1);//定位PR#1
#region 等待完成
int i = 0;
do
{
i++;
int end = get_DO();
if ((end & 1 << 3) > 0) break;
if (i > 50) break;
} while (true);
#endregion
}
/// <summary>
/// G01金加工速度,小于1500转
/// </summary>
/// <param name="speed">速度</param>
/// <param name="where">定位坐标</param>
/// <param name="run">立即旋转</param>
public void G01(AXIS axis, double where, double speed, bool run)
{//PR#2
string str;
if (speed > 15000) speed = 15000;//加工限速
if (axis == AXIS.X)
{
help_ModBus.address = 0x7f;//从站
}
else
{
help_ModBus.address = 0x00;//从站
}
//str = set_reg32bit("P5.070", (UInt16)(speed * 10.0));//速度索引A段 :设置速度 倍率10
str = set_reg32bit("P5.070", (UInt16)(speed));//速度索引A段 :设置速度
//str = set_reg32bit("P5.035", 130);//加速索引F段 :设置时间 ms
//str = set_reg32bit("P5.034", 20);//减速索引E段 :设置时间 ms
//D7 0 空
//D6 0 自动下一步 0关
//D5 0 延时时间索引 0(P5.040) ~ F(P5.055) 0ms~5500毫秒 P5.040=0ms;
//D4 A SPD目标速度索引 0(P5.060) ~ F(P5.075) 20转~3000转 20转~3000转 0.20 1.50 2.100 3.200 4.300 5.500 6.600 7.800 8.1000 9.1300 A.1500 B.1800 C.2000 D.2300 E.2500 F.3000转
//D3 E DEC减速时间索引 0(P5.020) ~ F(P5.035) 200ms~8000ms 0.200ms 1.300ms 2.500ms 3.600ms 4.800ms 5.900ms 6.1000ms //F.30ms(范围1~1200ms)
//D2 F ACC加速时间索引 0(P5.020) ~ F(P5.035)
//D1 3 OPT工作模式下的补充(TYPE工作模式2时) bit3bit2CMD=00ABS绝对定位 01REL相对定位 10INC光栅尺绝对定位 11CAP光栅尺相对定位 bit1本段PR#进入减速时允许下一PR#加入 bit0允许打断前一任务
//D0 2 TYPE工作模式: 1定速 2定位单步 3自动定位PR#下一步 7呼叫PR#跳跃 8写参数到路径 A分度定位 (240页)
//str = set_reg32bit("P6.004", 0x000Aef32);//A段速度<1500
str = set_reg32bit("P6.005", (int)(where*16777216/5.0));//PR# 2坐标(G01)
if (run) set_PR呼叫(2);//定位PR#2
#region 等待完成
int i = 0;
do
{
i++;
int end = get_DO();
if ((end & 1 << 3) > 0) break;
if (i > 50) break;
} while (true);
#endregion
}
#endregion
#endregion
//这个放弃
#region PR 模式
#region IO 操作
//设置DI初始化 开关量 端子输入
public void set_io_DI_init()
{//L型机:方向 35和37接24v 脉冲 36和41接24v 18力矩输入 20速度输入
//极性和功能// | 0x100是常开
//DI1 ~ DI8: P2.010 ~ P2.017
//DI9 ~DI13: P2.036 ~ P2.040
//来源
//P3.006 输入接点(DI)来源控制开关 Bit 12 ~ Bit 0 对应至 DI13 ~ DI1;
//0:输入接点状态由外部硬件端子控制。
//1:输入接点状态由系统参数 P4.007 控制。
//P4.007 bit对应开关状态
//
// DI 1 P2.010 101 使能 常开
// DI 2 P2.011 137 正点动
// DI 3 P2.012 138 反点动
// DI 4 P2.013 117 力矩 TCM0_多段力矩切换 = 0x16,TCM1_多段力矩切换 = 0x17, //116
// DI 5 P2.014 102 复位
// DI 6 P2.015 022 负限位
// DI 7 P2.016 023 正限位
// DI 8 P2.017 021 急停开关 常闭
// DI 9 P2.036 100 关 常开
// DI 10 P2.037 100 关 常开
// DI 11 P2.038 100 关 常开
// DI 12 P2.039 100 关 常开
// DI 13 P2.040 100 关 常开
//输入开关信号滤波 0~20ms
set_reg16bit("P2.009", 0);//开关过滤杂波 0~20 单位:ms 毫秒
//==================================
set_reg16bit("P3.006", 0x1FFF);//开关量来源0x1FFF(bit12~0) 0端子 1内部 P4.007
//===断电丢失=============================
set_reg16bit("P2.010", (ushort)io_in_功能.SON_伺服使能 | 0x100);//DI 1端子定义 01 DCBA:C(0:接常闭,1接常开)BA功能码
set_reg16bit("P2.011", (ushort)io_in_功能.JOGU_正点动 | 0x100);//DI 2端子定义
set_reg16bit("P2.012", (ushort)io_in_功能.JOGU_负点动 | 0x100);//DI 3端子定义
set_reg16bit("P2.013", (ushort)io_in_功能.TCM0_多段力矩切换 | 0x100);//DI 4端子定义
set_reg16bit("P2.014", (ushort)io_in_功能.ARST_复位 | 0x100);//DI 5端子定义
//set_reg("P2.015", 0);//DI 6端子定义
//set_reg("P2.016", 0);//DI 7端子定义
//set_reg("P2.017", 0);//DI 8端子定义
//set_reg("P2.036", 0);//DI 9端子定义
//set_reg("P2.037", 0);//DI 10端子定义
//set_reg("P2.038", 0);//DI 11端子定义
//set_reg("P2.039", 0);//DI 12端子定义
//set_reg("P2.040", (ushort)io_in_功能.ARST_复位 | 0x100);//DI 13端子定义
}
//设置D0初始化 端子输出
public void set_io_DO_init()
{
// + 0x100是常开
// DO 1 端子定义101 电源
// DO 2 端子定义103 零速
// DO 3 端子定义109 原点复归完成, 坐标有效
// DO 4 4端子定义105 坐标到位信号(参数 P1.054 设定值)
// DO 5 5端子定义007 伺服故障
// DO 6 端子定义100 关
set_reg16bit("P2.018", 0);//DO 1端子定义101 电源
set_reg16bit("P2.019", 0);//DO 2端子定义103 零速
set_reg16bit("P2.020", 0);//DO 3端子定义109 原点复归完成, 坐标有效
set_reg16bit("P2.021", 0);//DO 4端子定义105 坐标到位信号(参数 P1.054 设定值)
set_reg16bit("P2.022", 0);//DO 5端子定义007 伺服故障
set_reg16bit("P2.041", 0);//DO 6端子定义100 关 0x100是常开
}
public void set_io_out(ushort data)
{
set_reg16bit("P4.7", data);
//P4.007 bit对应开关状态
//
// DI 1 P2.010 101 使能 常开
// DI 2 P2.011 137 正点动
// DI 3 P2.012 138 反点动
// DI 4 P2.013 117 力矩
// DI 5 P2.014 102 复位
// DI 6 P2.015 022 负限位
// DI 7 P2.016 023 正限位
// DI 8 P2.017 021 急停开关 常闭
// DI 9 P2.036 100 关 常开
// DI 10 P2.037 100 关 常开
// DI 11 P2.038 100 关 常开
// DI 12 P2.039 100 关 常开
// DI 13 P2.040 100 关 常开
}
public void jogu_正点动(UInt16 speed)
{
set_reg16bit("P4.005", speed); //点动速度
// DI 1 P2.010 101 使能 常开0x100
// DI 2 P2.011 108 CTRG 呼叫PR#
// DI 3 P2.012 111 POS0 PR#码段
// DI 4 P2.013 112 POS1
// DI 5 P2.014 116 TCM0 力矩码段
// DI 6 P2.015 117 TCM1
// DI 7 P2.016 120 T-P 混合模式(扭+位)
// DI 8 P2.017 121 急停开关 常闭
// DI 9 P2.036 100 关 常开
// DI 10 P2.037 100 关 常开
// DI 11 P2.038 100 关 常开
// DI 12 P2.039 100 关 常开
// DI 13 P2.040 102 复位 常开
set_reg16bit("P4.007", 1 << 0//使能
| 1 << 7//急停开关
); // DI2 DI1
}
public void jogu_负点动(UInt16 speed)
{
set_reg16bit("P4.005", speed); //点动速度
// DI 1 P2.010 101 使能 常开0x100
// DI 2 P2.011 108 CTRG 呼叫PR#
// DI 3 P2.012 111 POS0 PR#码段
// DI 4 P2.013 112 POS1
// DI 5 P2.014 116 TCM0 力矩码段
// DI 6 P2.015 117 TCM1
// DI 7 P2.016 120 T-P 混合模式(扭+位)
// DI 8 P2.017 121 急停开关 常闭
// DI 9 P2.036 100 关 常开
// DI 10 P2.037 100 关 常开
// DI 11 P2.038 100 关 常开
// DI 12 P2.039 100 关 常开
// DI 13 P2.040 102 复位 常开
set_reg16bit("P4.007", 1 << 0//使能
| 1 << 7//急停开关
); // DI3 DI1
}
public void set_复位()
{
// DI 1 P2.010 101 使能 常开0x100
// DI 2 P2.011 108 CTRG 呼叫PR#
// DI 3 P2.012 111 POS0 PR#码段
// DI 4 P2.013 112 POS1
// DI 5 P2.014 116 TCM0 力矩码段
// DI 6 P2.015 117 TCM1
// DI 7 P2.016 120 T-P 混合模式(扭+位)
// DI 8 P2.017 121 急停开关 常闭
// DI 9 P2.036 100 关 常开
// DI 10 P2.037 100 关 常开
// DI 11 P2.038 100 关 常开
// DI 12 P2.039 100 关 常开
// DI 13 P2.040 102 复位 常开
set_reg16bit("P4.007", 1 << 0//使能
| 1 << 7//急停开关
| 1 << 12//复位
); // DI3 DI1
}
//设置使能
public void set_SON(bool start)
{
//set_reg();
}
#endregion
#endregion
#region 寄存器操作
// 1 获取参数寄存器编号 P3.002 =》 寄存器 0x0304
private ushort get_reg_获取寄存器地址(string reg)
{
//if (reg<0x07C8)//最大寄存器
if (reg.IndexOf(".") > 0) // "P5.016" 伺服电机坐标
{
string hi = reg.Substring(reg.IndexOf(".") - 1, 1); // 5
string li = reg.Substring(reg.IndexOf(".") + 1); // 16
int hv = int.Parse(hi) * 0x0100; // 0x500
int lv = int.Parse(li) * 2; //32 0x20
ushort reg_NO = (ushort)(hv + lv); // 0x0520
return reg_NO;
}
return 0;
}
//====read==========================================
//读单个寄存器 16bit
public string read_reg16bit(string reg)
{
UInt16 reg_address = get_reg_获取寄存器地址(reg);
string data = rs485.Read_Reg(reg_address, 2); // 在寄存器地址,读1个寄存器 (B3的寄存器是bit32的,必须读2次)
return data;//返回寄存器结果
}
//读32位寄存器 32bit
public string read_reg32bit(string reg)
{
UInt16 reg_address = get_reg_获取寄存器地址(reg);
string data = rs485.Read_Reg(reg_address, 2);//在寄存器地址,读2个寄存器(u16[0] u16[1] )
return data;//返回寄存器结果
}
//====write=========================================
//写单个寄存器(16bit)
public string set_reg16bit(string reg, Int32 data)
{// 16bit寄存器
UInt16 reg_address = get_reg_获取寄存器地址(reg);
string start = rs485.Write_Reg16Bit(reg_address, (ushort)data);//发送命令
return start;
}
//写32位寄存器
public string set_reg32bit(string reg, Int32 data)
{
//寄存器地址
UInt16 reg_address = get_reg_获取寄存器地址(reg);
//设定值
string start = rs485.Write_Reg32bit(reg_address, data);
return start;
}
#endregion
#region 枚举定义
//io定义:端子定义
public enum io_in_功能 : ushort //1字节
{
OFF = 0,
SON_伺服使能 = 0x01,
ARST_复位 = 0x02,
GAINUP_增益倍率 = 0x03,
CCLR_清除脉冲值 = 0x04,
ZCLAMP_电机零速时停转 = 0x05,
CMDINV_速度扭矩模式切换 = 0x06,
//==========================================================
CTRG_PR模式节拍_上升沿 = 0x08, // PR模式专用:与 POS 配合使用 //P5.007直接呼叫
TRQLM_限制扭矩 = 0x09,
VPL_电机自闭模式 = 0x0C,
VPRS_模拟位置指令清除 = 0x0D,
SPDLM_限速 = 0x10,
//=============================
POS0_PR模式码段 = 0x11,
POS1_PR模式码段 = 0x12,
POS2_PR模式码段 = 0x13,
POS3_PR模式码段 = 0x1A,
POS4_PR模式码段 = 0x1B,
POS5_PR模式码段 = 0x1C,
POS6_PR模式码段 = 0x1E,
ABSE_绝对位置模式 = 0x1D,
ABSC_绝对位置置0 = 0x1F,
SPD0_多段速切换 = 0x14,
SPD1_多段速切换 = 0x15,
TCM0_多段力矩切换 = 0x16,
TCM1_多段力矩切换 = 0x17,
S_P_速度位置模式 = 0x18,
S_T_速度力矩模式 = 0x19,
T_P_力矩位置模式 = 0x20,
EMGS_急停开关 = 0x21,
NL_负限位 = 0x22,
PL_正限位 = 0x23,
ORGP伺服原点 = 0x24,
SHOM_执行回原点 = 0x27, //P5.004
PT_PR_模式切换 = 0x2B,
JOGU_正点动 = 0x37,
JOGU_负点动 = 0x38,
EV1_事件1 = 0x39,
EV2_事件2 = 0x3A,
EV3_事件3 = 0x3B,
EV4_事件4 = 0x3C,
GNUM0_多段齿轮比 = 0x43,
GNUM1_多段齿轮比 = 0x44,
INHP_禁止脉冲 = 0x45,//必须DI4脚才能实时性
STP_电机停止 = 0x46,
PFQS_急停速度 = 0x47
}
public enum io_DI : Int32
{
//P4.007 bit对应开关状态
// 说明书3.3.2节 +0x100 常开
// DI 1 P2.010 01 SON 伺服启动
// DI 2 P2.011 08 CTRG 内部位置命令触发 呼叫PR#
// DI 3 P2.012 11 POS—0 内部位置命令选择 0 PR#码段
// DI 4 P2.013 12 POS—1
// DI 5 P2.014 16 TCM—0 力矩码段
// DI 6 P2.015 17 TCM—1
// DI 7 P2.016 20 T-P 扭矩/位置混和模式命令---选择切换 高电平:力矩模式
// DI 8 P2.017 21 EMGS 紧急停止
// DI 9 P2.036 02 ARST 异常重置
// DI 10 P2.037 37 正点动
// DI 11 P2.038 38 负点动
// DI 12 P2.039 100 关 常开
// DI 13 P2.040 100 关 常开
伺服使能 = 1 << 0, //DI 1
CTRG触发 = 1 << 1, //DI 2
POS0 = 1 << 2, //DI 3
POS1 = 1 << 3, //DI 4
TCM0力矩 = 1 << 4, //DI 5
TCM1力矩 = 1 << 5, //DI 6
/// <summary>
/// DI 7 (0:力矩模式 1:PR模式,可用TCM0选择力矩)
/// </summary>
TP力定切换 = 1 << 6, //DI 7 (0:力矩模式 1:PR模式,可用TCM0选择力矩)
EMGS急停 = 1 << 7, //DI 8
ARST复位 = 1 << 8, //DI 9
正转 = 1 << 9, //DI 10
反转 = 1 << 10, //DI 11
停止 = 1 << 11
//=1<< 12,
//=1<< 13
//=1<< 14,
//=1<< 15
}
//io定义:端子定义
public enum io_out_功能 : ushort //1字节
{
SRDY_伺服上电 = 0x01,
SON_伺服使能 = 0x02,
ZSPD_零速 = 0x03,
TSPD_到速 = 0x04,
TPOS_到位 = 0x05,
TQL_限扭 = 0x06,
ALRM_故障 = 0x07,
BRKR_刹车线 = 0x08,
HOME_已回零 = 0x09,
ABSW_绝对光栅故障 = 0x0D,
IDXD_分度完成 = 0x0E,
OLW_过负载 = 0x10,
WARN_警告 = 0x11,//(正反极限、通讯异常、低电压、风扇异常)
OVF_位置溢出 = 0x12,
SNL_负限位 = 0x13,
SPL_正限位 = 0x14,
Cmd_ok_指令完成 = 0x15,
CAP_OK_程序完成 = 0x16,
MC_OK_指令和定位都完成 = 0x17,
SP_OK_速度到达 = 0x19,
Zon1 = 0x2C,
Zon2 = 0x2D,
Zon3 = 0x2E,
Zon4 = 0x2F,
SPO_0 = 0x30, //P4.006 bit0 输出
SPO_1 = 0x31, //P4.006 bit1 输出
SPO_2 = 0x32, //P4.006 bit2 输出
SPO_3 = 0x33, //P4.006 bit3 输出
SPO_4 = 0x34, //P4.006 bit4 输出
SPO_5 = 0x35, //P4.006 bit5 输出
SPO_6 = 0x36, //P4.006 bit6 输出
SPO_7 = 0x37, //P4.006 bit7 输出
SPO_8 = 0x38, //P4.006 bit8 输出
SPO_9 = 0x39, //P4.006 bit9 输出
SPO_A = 0x3A, //P4.006 bit10 输出
SPO_B = 0x3B, //P4.006 bit11 输出
SPO_C = 0x3C, //P4.006 bit12 输出
SPO_D = 0x3D, //P4.006 bit13 输出
SPO_E = 0x3E, //P4.006 bit14 输出
SPO_F = 0x3F, //P4.006 bit15 输出
ABSR,
ABSD
}
//工作模式
public enum mode工作模式 : ushort //1字节
{
//单一
PT = 0x00, //外部脉冲
PR = 0x01, //内位置
S = 0x02, //速度
T = 0x03, //力矩
Sz = 0x04, //速度带零速模式
Tz = 0x05, //力矩带零扭模式
//混合
PT_S = 0x06, // 脉冲+限速
PT_T = 0x07, // 脉冲+限力
PR_S = 0x08, // 内位置+限速
PR_T = 0x09, // 内位置+限力 、、 用这个
S_T = 0x0A, // 速度+限力
DMC_NET = 0x0B, // 台达以太网
CANopen = 0x0C, //
EtherCAT = 0x0C,//
PT_PR = 0x0D, //位置+脉冲
PT_PR_S = 0x0E,//位置+脉冲+限速
PT_PR_T = 0x0F//位置+脉冲+限力
}
/// <summary>
/// ushort num = (ushort)(LEDshow)Enum.Parse(typeof(LEDshow), comboBox1.Text);
/// </summary>
public enum LEDshow : short
{
#region 说明 //AL.013 急停报警 // P0.001当前伺服报警
//P0.002=0~26 //0电机坐标PUU(理论坐标) P5.016
// //1位置命令的目前坐标PUU
// //2位置命令与回授位置的差异值PUU
// //3电机编码器目前回授的位置坐标Pulse(实际实时坐标)
// //4位置命令的目前坐标Pulse
// //5位置命令与回授位置的差异值,单位为编码器单位pulse
// //6驱动器接收到脉冲命令的频率,单位为 kpps适用于 PT / PR 模式。
// //7电机目前转速
// //8由模拟信道输入的速度命令,单位为 0.01 伏特
// //9整合的速度命令,单位为 0.1 rpm。
// //10由模拟信道输入的扭力命令,单位为 0.01 伏特
// //11整合的扭力命令,单位为百分比(%)。
// //12平均负载率
// //13峰值负载率
// //14 DC Bus 电压
// //15负载惯量比
// //16 IGBT 温度
// //17 共振频率
// //18 与 Z 相偏移量范围为-4999 ~ +5000。
// //19映像参数内容# 1
// //20映像参数内容# 2
// //35分度坐标命令
// //38电池电压
// //39 DI 状态(整合)
// //40 DO 状态(硬件)
// //49脉冲命令(CN1)输入的脉冲计数值。
// //51电机目前实际速度
// // 54电机目前实际扭力
// //55电机目前实际电流
// //56整流后的电容器电压
// //67PR 目标速度
// //111驱动器伺服错误码
// //-124编码器温度 0.0.124
//=============================================
#endregion
//P0.002=0~26
L0电机坐标PUU理论坐标 = 0x00,// P5.016
L1位置命令的目前坐标PUU,
L2位置命令与回授位置的差异值PUU,
L3电机编码器目前回授的位置坐标Pulse实际实时坐标,
L4位置命令的目前坐标Pulse,
L5位置命令与回授位置的差异值单位为编码器单位pulse,
L6驱动器接收到脉冲命令的频率单位为kpps适用于PTPR模式,
L7电机目前转速,
L8由模拟信道输入的速度命令单位为001伏特,
L9整合的速度命令单位为01rpm,
L10由模拟信道输入的扭力命令单位为001伏特,
L11整合的扭力命令单位为百分比,
L12平均负载率,
L13峰值负载率,
L14DCBus电压,
L15负载惯量比,
L16IGBT温度,
L17共振频率,
L18与Z相偏移量范围为正负5k,
L19映像参数内容井1,
L20映像参数内容井2,
L35分度坐标命令 = 35,
L38电池电压 = 38,
L39DI状态,
L40DO状态,
L49脉冲命令CN1输入的脉冲计数值 = 49,
L51电机目前实际速度 = 51,
L54电机目前实际扭力 = 54,
L55电机目前实际电流 = 55,
L56整流后的电容器电压,
L67PR目标速度 = 67,
L111驱动器伺服错误码 = 111,
L负124编码器温度 = -124 //0.0.124,
}
public enum ALM异警 : short
{//13紧急停止
//0f21
//0235位置计数器溢位警告
//30电机碰撞错误
//09位置控制误差过大
AL001过电流 = 1,
AL002过电压 = 2,
AL003低电压 = 3,
AL004电机匹配异常 = 4,
AL005回生错误 = 5,
AL006过负荷 = 6,
AL007速度控制误差过大 = 7,
AL008异常脉冲控制命令 = 8,
AL009位置控制误差过大 = 9,
AL010回生状态下电压异常 = 10,
AL011位置检出器异常 = 11,
AL012校正异常 = 12,
AL013紧急停止 = 13,
AL014反向极限异常 = 14,
AL015正向极限异常 = 15,
AL016IGBT温度异常 = 16,
AL017内存异常 = 17,
AL018位置检出器输出异常 = 18,
AL019 = 19,
AL020串行通讯逾时 = 20,
AL021 = 21,
AL022主回路电源异常 = 22,
AL023预先过负载警告 = 23,
AL024编码器初始磁场错误 = 24,
AL025编码器内部错误 = 25,
AL026编码器内部数据可靠度错误 = 26,
AL027编码器内部重置错误 = 27,
AL028编码器高电压错误或编码器内部错误 = 28,
AL029格雷码错误 = 29,
AL030电机碰撞错误 = 30,
AL031电机动力线错or断线侦测 = 31,
AL032 = 32,
AL033 = 33,
AL034编码器内部通讯异常 = 34,
AL035编码器温度超过保护上限 = 35,
AL036 = 36,
Al037 = 37,
AL038 = 38,
AL039 = 39,
AL040 = 40,
Al041 = 41,
AL042模拟速度电压输入过高 = 42,
AL043 = 43,
AL044驱动器功能使用率警告 = 44,
AL045电子齿轮比设定错误 = 45,
AL046 = 46,
AL047 = 47,
AL048 = 48,
AL049 = 49,
AL050 = 50,
AL051 = 51,
AL53电机参数未确认 = 53,
AL235位置计数器溢位警告 = 235
}
public enum AXIS : ushort//轴地址
{
A = 1,// Modbus 从站地址
B = 2,
C = 3,
D = 4,
E = 5,
F = 6,
G = 7,
H = 8,
I = 9,
J = 10,
K = 11,
L = 12,
M = 13,
N = 14,
O = 15,
P = 16,
Q = 17,
R = 18,
S = 19,
T = 20,
U = 21,
V = 22,
W = 23,
X = 24,
Y = 25,
Z = 26,
}
#endregion
#endregion
#region 移植专用
public int B3_init_初始化(byte address)
{//注意:顺序不要改,只有脱机下才能改工作模式。
help_ModBus.address = address;
set_DI(0);//脱机状态
//设备初始化
set_PRT_Mode_init_定位加力矩模式_初始化(); // 模式初始化
//输出初始化
set_DO_init_PRT();// PR+T 输出初始化
//输入初始化
set_DI_init_PRT();// PR+T 输入初始化
return 0;
}
/// <summary>
/// 一行G代码
/// G0X9.896Y10.158Z1.000F10000
/// G1Z-2.000F10000
/// G1X9.993Y10.147F10000
/// </summary>
/// <param name="str"></param>
/// <returns></returns>
public int Gcode_解析(string str, bool run)//"G0X9.896Y10.158Z1.000F10000"
{// G0X9.896Y10.158Z1.000F10000
// G1Z-2.000F10000
// G1X9.993Y10.147F10000
AXISclass gc = new AXISclass(str);
double x坐标 = -1;
double y坐标 = -1;
double z坐标 = -1;
double a坐标 = -1;
double b坐标 = -1;
double c坐标 = -1;
double d坐标 = -1;
double e坐标 = -1;
double u坐标 = 0;
int f_速度 = 0;
x坐标 = gc.x_坐标;
y坐标 = gc.y_坐标;
z坐标 = gc.z_坐标;
f_速度 = gc.f_速度;
e坐标 = gc.e_坐标;
a坐标 = gc.a_坐标;
b坐标 = gc.b_坐标;
c坐标 = gc.c_坐标;
if (gc.g_指令 != -1)
{
if (gc.g_指令 == 0)
{
if (x坐标 != -1) G00(AXIS.X, x坐标, f_速度, true);
if (y坐标 != -1) G00(AXIS.Y, y坐标, f_速度, true);
if (z坐标 != -1) G00(AXIS.Z, z坐标, f_速度, true);
if (e坐标 != -1) G00(AXIS.E, e坐标, f_速度, true);
if (a坐标 != -1) G00(AXIS.A, a坐标, f_速度, true);
if (b坐标 != -1) G00(AXIS.B, b坐标, f_速度, true);
if (c坐标 != -1) G00(AXIS.C, c坐标, f_速度, true);
if (d坐标 != -1) G00(AXIS.D, d坐标, f_速度, true);
}
if (gc.g_指令 == 1)
{
if (x坐标 != -1) G01(AXIS.X, x坐标, f_速度, true);
}
}
return 0;
}
#endregion
}
// G代码解析
public class AXISclass
{
#region 字段
static int G指令 = -1;
private double X_坐标 = -1;
private double Y_坐标 = -1;
private double Z_坐标 = -1;
static int F_速度 = -1;
private double A_坐标 = -1;
private double B_坐标 = -1;
private double C_坐标 = -1;
private double D_坐标 = -1;
private double E_坐标 = -1;
private double G_坐标 = -1;
private double U_坐标 = -1;
private double V_坐标 = -1;
private double W_坐标 = -1;
#endregion
#region 属性
// public int _坐标{get { return _坐标; }set { _坐标 = value; }}
public int g_指令 { get => G指令; set => G指令 = value; }
public double x_坐标 { get { return X_坐标; } set { X_坐标 = value; } }
public double y_坐标 { get { return Y_坐标; } set { Y_坐标 = value; } }
public double z_坐标 { get { return Z_坐标; } set { Z_坐标 = value; } }
public int f_速度 { get { return F_速度; } set { F_速度 = value; } }
public double a_坐标 { get { return A_坐标; } set { A_坐标 = value; } }
public double b_坐标 { get { return B_坐标; } set { B_坐标 = value; } }
public double c_坐标 { get { return C_坐标; } set { C_坐标 = value; } }
public double e_坐标 { get { return E_坐标; } set { E_坐标 = value; } }
public double u_坐标 { get { return U_坐标; } set { U_坐标 = value; } }
public double v_坐标 { get { return V_坐标; } set { V_坐标 = value; } }
public double w_坐标 { get { return W_坐标; } set { W_坐标 = value; } }
#endregion
#region 构造
public AXISclass(string str)
{
get_AXIS_坐标(str);
}
#endregion
#region 方法
public void get_AXIS_坐标(string str)
{
string data = str;
int g;
if (data.IndexOf("G") != -1)
{
string AXIS_X = data.Substring(data.IndexOf("G") + 1);
AXIS_X = AXIS_X.Substring(0, 2);//小数
try
{
g = int.Parse(AXIS_X);
}
catch (Exception)
{
AXIS_X = AXIS_X.Substring(0, 1);//小数
g = int.Parse(AXIS_X);
}
G指令 = g;
}
if (data.IndexOf("X") != -1)
{
string AXIS_X = data.Substring(data.IndexOf("X") + 1);
AXIS_X = AXIS_X.Substring(0, AXIS_X.IndexOf(".") + 1 + 3);//小数
double x = double.Parse(AXIS_X);
X_坐标 = x;
}
data = str;
if (data.IndexOf("Y") != -1)
{
string AXIS位置 = data.Substring(data.IndexOf("Y") + 1);
AXIS位置 = AXIS位置.Substring(0, AXIS位置.IndexOf(".") + 1 + 3);//小数
double x = double.Parse(AXIS位置);
Y_坐标 = x;
}
data = str;
if (data.IndexOf("Z") != -1)
{
string AXIS位置 = data.Substring(data.IndexOf("Z") + 1);
AXIS位置 = AXIS位置.Substring(0, AXIS位置.IndexOf(".") + 1 + 3);//小数
double x = double.Parse(AXIS位置);
Z_坐标 = x;
}
data = str;
if (data.IndexOf("F") != -1)
{
string AXIS位置 = data.Substring(data.IndexOf("F") + 1);
double x = double.Parse(AXIS位置);
F_速度 = (int)(x);
}
data = str;
if (data.IndexOf("E") != -1)
{
string AXIS位置 = data.Substring(data.IndexOf("E") + 1);
AXIS位置 = AXIS位置.Substring(0, AXIS位置.IndexOf(".") + 1 + 3);//小数
double x = double.Parse(AXIS位置);
E_坐标 = x;
}
data = str;
if (data.IndexOf("A") != -1)
{
string AXIS位置 = data.Substring(data.IndexOf("A") + 1);
AXIS位置 = AXIS位置.Substring(0, AXIS位置.IndexOf(".") + 3);//小数
double x = double.Parse(AXIS位置);
A_坐标 = x;
}
data = str;
if (data.IndexOf("B") != -1)
{
string AXIS位置 = data.Substring(data.IndexOf("B") + 1);
AXIS位置 = AXIS位置.Substring(0, AXIS位置.IndexOf(".") + 3);//小数
double x = double.Parse(AXIS位置);
B_坐标 = x;
}
data = str;
if (data.IndexOf("C") != -1)
{
string AXIS位置 = data.Substring(data.IndexOf("C") + 1);
AXIS位置 = AXIS位置.Substring(0, AXIS位置.IndexOf(".") + 3);//小数
double x = double.Parse(AXIS位置);
C_坐标 = x;
}
}
public double get_AXIS_坐标1(string data)
{
string data1, data2;
//G0X9.896Y10.158Z1.000F10000
if (data.IndexOf("G") != -1)
{
data1 = data.Split('G')[1];//去头
data2 = data1.Split('X')[0];//去尾
g_指令 = int.Parse(data2); //G
}
if (data.IndexOf("X") != -1)
{
data1 = data.Split('X')[1];
data2 = data1.Split('Y')[0];
x_坐标 = double.Parse(data2);//X
}
if (data.IndexOf("Y") != -1)
{
data1 = data.Split('Y')[1];
data2 = data1.Split('Z')[0];
y_坐标 = double.Parse(data2);//Y
}
if (data.IndexOf("Z") != -1)
{
data1 = data.Split('Z')[1];
data2 = data1.Split('F')[0];
z_坐标 = double.Parse(data2);//Z
}
if (data.IndexOf("F") != -1)
{
data1 = data.Split('F')[1];
data2 = data1.Split('\r')[0];
f_速度 = int.Parse(data2);//F
}
if (data.IndexOf("A") != -1)
{
data1 = data.Split('A')[1];
data2 = data1.Split('B')[0];
a_坐标 = double.Parse(data2);//Z
}
if (data.IndexOf("B") != -1)
{
data1 = data.Split('B')[1];
data2 = data1.Split('C')[0];
b_坐标 = double.Parse(data2);//Z
}
if (data.IndexOf("C") != -1)
{
data1 = data.Split('C')[1];
data2 = data1.Split('F')[0];
c_坐标 = double.Parse(data2);//Z
}
return 0.0;
}
//if (num == "0" || num == "1" || num == "2" || num == "3" || num == "4" || num == "5"
// || num == "6" || num == "7" || num == "8" || num == "9")
// {
// }
#endregion
#region 委托
#endregion
}
}