第二节 运动轴控制

运动轴控制涉及复归，JOG运动，点动，插补等运动方式。通常情况下，运动轴的移动以Robot类的动作形式完成。当robot进行一个动作的时候，不允许进行其它动作。只有当前动作完成以后，才能进行下一动作。所以，启动一个动作以后一般在程序上需要禁止其它动作有关的按钮或控件。当动作完成以后，robot会post一条ID为global.ON_ACT_STOP的消息给窗口，用户在对应的消息处理函数中将当前动作关闭。如：

bool ret = global.robot.Move(1, 120, true, opt.GetHandle(), ref pErrCode);

if(ret)

{

//disable other buttons

}

重载WndProc，处理自定义消息：

protected override void WndProc(ref Message m)

{

switch (m.Msg)

{

case global.ON_ACT_STOP:

OnActStop(ref m);

return;

default:

break;

}

base.WndProc(ref m);

}

OnActStop函数类似如下：

private void OnActStop(ref Message m)

{

int act = (int)m.WParam;

int err = (int)m.LParam;

//首先停止动作

global.robot.StopMotion();

//根据返回值，进一步处理

if (err != 0)

{

global.bSysReady = false;

PrintErrorEx(global.ZEL_ERROR, global.GetLastError(err));

}

else

{

if (act == global.ROB_ACT_RESET_MACH)

{

global.bSysReady = true;

m_nCurErrLevel = -1;

PrintErrorEx(global.ZEL_NORMAL, "复位完成");

}

}

//恢复被禁止的按钮

DisableButtons(false);

}

用户也可以提供按钮事件显示调用global.robot.StopMotion()函数主动终止当前动作。

2.1 运动轴复归

除了JOG移动，所有运动轴在移动之前一般需要先找原点信号位置，并回到该位置。这个过程叫复归。复归有单轴复归和多轴同时复归两种情况。如：

bool ret = global.robot.GoHome(1, this.Handle);

该语句将启动轴号为1的运动轴回原点。

复归也可以使用motion类对象的接口实现。

bool ret = motion.Home(Motion.ROB_AXIS(2), opt.GetHandle(), ref err);

if (!ret) return false;

ret = robot.WaitAxis(Motion.ROB_AXIS(2), true, opt.GetHandle(), ref err);

if (!ret) return false;

使用Motion类的Home函数，进行单轴回零，需要调用Robot类的WaitAxis函数等待其完成。注意： WaitAxis函数的第2个参数必须为true，表示等待复归移动完成.

注意： 在robot启动运行的情况下， robot类的WaitAxis等待会失效，请确保调用该函数之前robot已经启动。

2.2 运动轴连续移动

JOG移动即连续轴移动，该移动通常在设备或控制系统调试或编辑阶段进行。JOG移动启动后，会一直按照指定的速度和方向运行，直到调用相关函数停止。下面代码段进行启动JOG移动：

if (global.robot.GetCurState() > Robot.MS_STANDBY)

{

return;

}

Motion motion = new Motion();

motion.FromHandle(global.robot.GetMotionHandle());

Options opt = global.robot.GetMotionOptions(Robot.ST_JOG);

int err = 0;

if (!global.robot.Start(ref err))

{

string str = "无法启动robot" + global.GetLastError(err);

MessageBox.Show(str, "错误");

return;

}

if (!motion.Jog(1, ref opt, Motion.DIR_POS, ref err))

{

string str;

str = "无法进行连续运动，错误原因:";

str += global.GetLastError(err);

MessageBox.Show(str, "错误");

}

下面代码段停止JOG移动。

Motion motion = new Motion();

motion.FromHandle(global.robot.GetMotionHandle());

Options opt = global.robot.GetMotionOptions(Robot.ST_JOG);

int err = 0;

if (!motion.StopJog(1, ref opt, ref err))

{

string str;

str = "无法进行连续运动，错误原因:";

str += global.GetLastError(err);

MessageBox.Show(str, "错误");

}

global.robot.Stop();

global.Sleep(100);

首先调用motion的StopJob函数接口停止JOG移动，然后停止robot动作状态。

2.3 单轴移动

单轴移动可以调用robot函数接口完成，也可以调用motion和robot接口一起来完成。motion类的接口移动是异步的，即发起后立即返回。Robot类的接口是同步的，会等待轴移动完成才返回。

下面代码段使用motion的接口移动第3轴到绝对位置，然后调用robot类的接口等待完成。

int axisZ =3;

int err=0;

bool ret = motion.Move(axisZ, m_nAxisZWorkPos, opt.GetHandle(), true, ref err);

global.Assert(ret, “Moving failed”);

ret = robot.WaitAxis(axisZ, false, opt.GetHandle(), ref err);

global.Assert(ret, “failed to wait”);

如果使用robot接口，调用如下：

bool ret = robot.Move(1, 120, true, opt.GetHandle(), ref pErrCode)

global.Assert(ret, “move failed”);

2.4 多轴移动

多轴移动指多条轴同时移动或先后移动。同时移动的时候通常为插补移动。Unittake目前可以提供2轴或3轴同时移动。

int axisX=1, axisY=2;

int err=0;

bool ret = robot.MoveAny2(axisX, axisY, 133, 122, true, opts.GetHandle(), ref err);

上面的代码段将移动轴1和轴2分别到133，122位置。

与单轴移动一样，也可以直接调用motion接口启动运动轴移动，然后调用robot的等待函数进行等待完成。

int axisX=1, axisY=2;

int err=0;

motion.MoveAny2(axisX, axisY, 133, 122, opts.GetHandle(),true, ref err);

robot.WaitAny2(axisX, axisY, false, opts.GetHandle(), ref err);

2.5 自定义动作

复杂的控制需要自定义动作。自定义动作通过回调函数实现，在C#语言中，需要声明为delegate。每个自定义动作封装成一个类，目前已经有一个基类。子类通过重载ActionImplementHandler函数实现具体的动作内容。这里的动作不仅仅是指控制运动轴移动，也可指输出OUTPUT信号，读取INPUT信号，甚至控制相机采图，进行图像分析和处理等。

下面是自定义动作具体实现过程。

1. 在全局命名空间声明如下的函数：

public unsafe delegate int CustomActionImplementDelegate(IntPtr hRobot, uint actId, IntPtr* pControlUnits, int unitsCount, IntPtr hNotifyWnd, IntPtr pUserData);

注意，前面有delegate, 这个是支持回调函数必须前置符。

2. 创建一个基类如ActionCollectTray,添加虚拟函数如下：

public virtual unsafe int ActionImplementHandler(ref Robot robot, uint actId, IntPtr* pControlUnits, int unitsCount, IntPtr hNotifyWnd )

{

return -1;

}

该函数用于完成所有具体动作过程。所有子类重载这个函数并具体实现。返回值为0表示成功，其它的值表示出错。

3. 添加静态函数CustomActionImplementFunc如下：

public unsafe static int CustomActionImplementFunc(IntPtr hRobot, uint actId, IntPtr* pControlUnits, int unitsCount, IntPtr hNotifyWnd, IntPtr pUserData)

{

int err = 0;

GCHandle gch = GCHandle.FromIntPtr(pUserData);

ActionCollectTray ct = (ActionCollectTray)gch.Target;

global.Assert(IntPtr.Zero != hRobot, "Invalid robot handle");

Robot robot = new Robot();

robot.FromHandle(hRobot);

err = ct.ActionImplementHandler(ref robot, actId, pControlUnits, unitsCount, hNotifyWnd);

gch.Free();

return err;

}

4. 添加一个类型为CustomActionImplementDelegate的成员变量，将CustomActionImplementFunc作为参数生成实例。

public unsafe static CustomActionImplementDelegate m_caiDelegate = new CustomActionImplementDelegate(CustomActionImplementFunc);

自动动作的子类可以继承该类，重载ActionImplementHandler函数，完成自定义动作。

下面的代码段，启动具体的自定义动作：

GCHandle gch = GCHandle.Alloc(this.m_ert);

bool ret = global.robot.Excute(global.ROB_ACT_RAISE_EMPTY_TRAY, ActionRaiseEmptyTray.m_caiDelegate, GCHandle.ToIntPtr(gch), this.Handle);

if (ret)

{

//DisableButtons(this.tab.SelectedIndex, true);

}

2.5 输入输出操作

输入输出读取当前的输出点或输出电平，通常为高(1)或低(0)， 高电平表示接通，低电平表示断开。输入点的当前状态如下操作：

int level=0;

int err=0;

bool ret = robot.GetInputLevel(14, ref level, ref err);

输出点即可以获取当前的状态，也可以改变当前的输出状态。如：

bool ret = robot.GetOutputLevel(14, ref level, ref err);

该语句获得输出点的状态。相应地，

bool ret = robot.SetOutputLevel(12, Motion.Hi,ref err);

固高EtherCAT访问输入输出点不一样。输出点位需要增加偏移量。如：

bool ret = robot.GetInputLevel(10, 2, ref level, ref err);

上面语句获取10.2编号的输入点状态，其中10为偏移量。