define(PLC_Homing='1')

//Homing:
//Move all motors left bottom
//move 1mm up
//move both mottors at same time

//move to limit switch, gather data, move to other limit switch, gather stop. move back to first limit switch
//motID       :P0  : Motor Number
//direction   :P1  : Motion direction to first limit switch: -1 -> #1j- ,+1 -> #1j+
//status      :P100: local status
//timer       :P101: local timer

open plc PLC_Homing
define(motID='L0',direction='P1',status='P100',timer='P101')
jog-1; jog-2; jog-3; jog-4;
Motor[1].HomeVel=0.5
Motor[2].HomeVel=0.5
Motor[3].HomeVel=0.5
Motor[4].HomeVel=0.5
status=10
while(1)
{
  if((Motor[1].MinusLimit==1 || Motor[1].FeFatal==1)){
  if((Motor[2].MinusLimit==1 || Motor[2].FeFatal==1)){
  if((Motor[3].MinusLimit==1 || Motor[3].FeFatal==1)){
  if((Motor[4].MinusLimit==1 || Motor[4].FeFatal==1)){
  status=11
  break
  }}}}
}
timer = Sys.RunTime + 2
status=100
while (Sys.RunTime < timer){} //wait 2 sec
status=101
jog1:1000; jog2:1000; jog3:1000; jog4:1000;
timer = Sys.RunTime + 2
status=200
while (Sys.RunTime < timer){} //wait 2 sec
status=201
while(1)
{
  if(Motor[1].DesVelZero==1 &&  Motor[2].DesVelZero==1 && Motor[3].DesVelZero==1 && Motor[4].DesVelZero==1)
  {
    status=12
    break
  }
}
timer = Sys.RunTime + 2
while (Sys.RunTime < timer){}
home 1; home 2; home 3; home 4
status=13
while(1) //wait homing index found done
{
  if(Motor[1].HomeComplete==1 &&  Motor[2].HomeComplete==1 && Motor[3].HomeComplete==1 && Motor[4].HomeComplete==1)
  {
    status=14
    break
  }
}
while(1) //wait moving to zero position done
{
  if(Motor[1].DesVelZero==1 &&  Motor[2].DesVelZero==1 && Motor[3].DesVelZero==1 && Motor[4].DesVelZero==1)
  {
    status=15
    break
  }
}
disable plc PLC_Homing
close