homing plc and fixes

cfg/init.cfg

@@ -0,0 +1,68 @@
+define(PLC_Homing='1')
+
+open subprog 12
+enable plc PLC_Homing
+close
+
+open plc PLC_Homing
+define(status='P100',timer='P101')
+
+L20=Motor[2].MaxDac
+Motor[2].MaxDac=500
+L21=Motor[2].FatalFeLimit
+Motor[2].FatalFeLimit=2000
+L22=Motor[2].JogSpeed
+Motor[2].JogSpeed=1
+
+L30=Motor[3].MaxDac
+Motor[3].MaxDac=500
+L31=Motor[3].FatalFeLimit
+Motor[3].FatalFeLimit=2000
+L32=Motor[3].JogSpeed
+Motor[3].JogSpeed=1
+
+
+Motor[2].PhaseFindingStep=1
+Motor[3].PhaseFindingStep=1
+timer = Sys.RunTime + 1;while (Sys.RunTime < timer){} //wait 1 sec
+
+Motor[2].HomeVel=2
+Motor[3].HomeVel=2
+
+P200=Motor[2].HomeInProgress
+home2
+home3
+timer = Sys.RunTime + 1;while (Sys.RunTime < timer){} //wait 1 sec
+P201=Motor[2].HomeInProgress
+
+status=1
+while(1)
+{
+  if(Motor[2].FeFatal && (status&2)==0)
+  {
+    status=status|2
+    Motor[2].HomeVel=-Motor[2].HomeVel
+    home2
+  }
+  if(Motor[3].HomeInProgress==0 && (status&4)==0)
+  {
+    status=status|4
+    Motor[3].HomeVel=-Motor[3].HomeVel
+    home3
+  }
+  if(Motor[2].HomeInProgress==0 && Motor[3].HomeInProgress==0)
+  {
+    status=status|8
+    break
+  }
+}
+
+Motor[2].MaxDac=L20
+Motor[2].FatalFeLimit=L21
+Motor[2].JogSpeed=L22
+Motor[3].MaxDac=L30
+Motor[3].FatalFeLimit=L31
+Motor[3].JogSpeed=L32
+
+disable plc PLC_Homing
+close