第三章 轴的手自动第一次成功版

第三章 轴的手自动第一次成功版

---

---

前言

使用说明：
Gohome为FB块，建议使用时一直刷新，
通过控制其输入的EN / Execute 来使轴进行回零
1、EN置1，开启回零功能
2、设置轴号AxisID、回零高速度HSpeed、回零低速度LSpeed、搜索原点方向MoveDir，
按实际设置搜索限位范围和搜索Home范围ScanLmtPos和ScanHomePos，
按需设置离开距离EscapePos和偏移OffSet
3、（可选）设置运动参数TrapP
4、置位启动信号Execute，轴就按照设置的参数进行回原点。Execute为上升沿信号（已经内置上升沿）
6、回零成功后，完成信号Done置1，当Execute置1、EN置0、Stop置1，自动复位Done，
7、在EN为1时，Stop有信号可停止该轴的运动

---

一、创建PRG子程序AxisIDPRG

PROGRAM AxisIDPRG
VAR
	AxisIDyi:AxisID1FB;
	AxisIDer:AxisID1FB;
	AxisIDsan:AxisID1FB;
	bAxisIDyi: BOOL;
	bAxisIDer: BOOL;
	bAxisIDsan: BOOL;
	bStop: BOOL;
	Pos: DINT;

	SZDQHyi:SZDQH;
	SZDQHer:SZDQH;
	SZDQHsan:SZDQH;

	AxisIDyishouzi: BOOL;
	AxisIDershouzi: BOOL;
	AxisIDsanshouzi: BOOL;

	AxisIDyishoudong: BOOL;
	AxisIDershoudong: BOOL;
	AxisIDsanshoudong: BOOL;

	AxisIDyizidong: BOOL;
	AxisIDerzidong: BOOL;
	AxisIDsanzidong: BOOL;
END_VAR

SZDQHyi(shouzi:=AxisIDyishouzi ,
		zidong:=AxisIDyizidong ,
		shoudong:=AxisIDyishoudong ,
		kaiguan=>bAxisIDyi);
AxisIDyi(
	EN:=1 ,
	Execute:=bAxisIDyi ,
	Stop:=bStop ,
	AxisID:=1 ,
	EscapePos:=Pos,
	Done=> );
	
IF bAxisIDyi THEN
	bAxisIDyi :=1;
	GT_AxisOn(1);
END_IF
IF AxisIDyi.Done THEN
	bAxisIDyi :=0;
END_IF

二、创建FB功能块AxisID1FB

1.变量

代码如下（示例）：

FUNCTION_BLOCK AxisID1FB
VAR_INPUT
	EN: BOOL := 0;
	Execute: BOOL := 0;
	Stop: BOOL := 0;
	AxisID: INT := 0;
	LSpeed: REAL := 5;
	EscapePos: DINT := 20000;
	TrapP: TTrapPrm := (acc:= 3, dec:= 3, velStart:= 0, smoothTime:= 10);
END_VAR
VAR_OUTPUT
	Done: BOOL;
END_VAR
VAR
	EN_R: R_TRIG;
	EN_F: F_TRIG;
	Exec_R: R_TRIG;
	Timer3: TON;
	psts: DINT := 0;
	count: INT := 1;
	pclock: DWORD := 0;

END_VAR

2.读入数据

代码如下（示例）：

Exec_R(CLK:=Execute);
EN_F(CLK:=EN);

IF EN THEN
GT_GetSts(AxisID,ADR(psts),count,ADR(pclock));
IF Stop THEN
		HStep:=0;
		Done:=0;
	END_IF

	IF Exec_R.Q AND NOT (Stop) THEN
		HStep:=1;
	END_IF

CASE HStep OF
	0:
		;
	1:
		GT_ClrSts(AxisID,1);
		GT_ZeroPos(AxisID,1);
		GT_PrfTrap(AxisID);
		GT_SetTrapPrm(AxisID,ADR(TrapP));
		GT_SetVel(AxisID, ABS(LSpeed));
		GT_SetPos(AxisID,ABS(EscapePos));
		GT_Update(SHL(DWORD#1,AxisID-1));
		HStep:=2;
	2:
		Timer3(IN:=1,PT:=T#0.3s);
		IF Timer3.Q THEN
		HStep:=3;
			Timer3(IN:=0,PT:=T#0.3s);
		END_IF
	3:
		IF NOT(psts.10) THEN
			GT_Stop(SHL(DWORD#1,AxisID-1),1);
			HStep:=4;
		END_IF
	4:
		Done:=1;
		HStep:=0;

END_CASE
END_IF

IF EN_F.Q THEN
	HStep:=0;
	Done:=0;
	GT_Stop(SHL(DWORD#1,AxisID-1),1);
END_IF

三、创建FB功能块SZDQH

FUNCTION_BLOCK SZDQH
VAR_INPUT
	shouzi:BOOL;
	zidong:BOOL;
	shoudong:BOOL;
END_VAR
VAR_OUTPUT
	kaiguan:BOOL;
END_VAR
VAR
	zidong_R: R_TRIG;
	shoudong_R: R_TRIG;
END_VAR

//（*kaiguan:=(shouzi AND zidong_R.Q)OR (NOT shouzi AND shoudong_R.Q);*)
kaiguan:=(shouzi AND zidong)OR (NOT shouzi AND shoudong);

---

总结

这个是真正成功了。效果是复位按钮手动按一下，走一节位置，按一下，走一节位置，持续不断。