define(PLC_Homing='1')
define(statusHoming='P100')
statusHoming=0

//statusHoming:
// 1: M1 phased
// 2: M2 phased
// 4: M1 pos limit found
// 8: M2 pos limit found
// 16: M1 homed
// 32: M2 homed
// 256: homing failed


open plc PLC_Homing
  define(timer='L1')
  statusHoming=0
  L10=Motor[1].MaxDac
  Motor[1].MaxDac=500
  L11=Motor[1].FatalFeLimit
  Motor[1].FatalFeLimit=2000
  L12=Motor[1].JogSpeed
  Motor[1].JogSpeed=1

  L20=Motor[2].MaxDac
  Motor[2].MaxDac=500
  L21=Motor[2].FatalFeLimit
  Motor[2].FatalFeLimit=2000
  L22=Motor[2].JogSpeed
  Motor[2].JogSpeed=1


  Motor[1].PhaseFindingStep=1
  Motor[2].PhaseFindingStep=1
  while(Motor[1].PhaseFindingStep>0 || Motor[2].PhaseFindingStep>0){}
  if (Motor[1].PhaseFound==1)
  {
    statusHoming|=1
    cmd "#1j/"
    send 1"phasing Y_Achse ok\n"
  }
  else
  {
    send 1"phasing Y Achsde error!\n"
    goto 0 //ERROR_END
  }
  if (Motor[2].PhaseFound==1)
  {
    statusHoming|=2
    cmd "#2j/"
    send 1"phasing X_Achse ok\n"
  }
  else
  {
    send 1"phasing X Achse error!\n"
    statusHoming|=256
    goto 0 //ERROR_END
  }


  //move until FeWarn
  Motor[1].JogSpeed=6
  Motor[2].JogSpeed=6
  timer = Sys.RunTime + .5;while (Sys.RunTime < timer){} //wait .5 sec
  jog+1
  jog+2
  while(1)
  {
    if(Motor[1].FeWarn && (statusHoming&4)==0)
    {
      cmd "#1j/"
      send 1"Y_Achse +limit\n"
      statusHoming|=4
    }
    if(Motor[2].FeWarn && (statusHoming&8)==0)
    {
      cmd "#2j/"
      send 1"X_Achse +limit\n"
      statusHoming|=8
    }
    if((statusHoming&12)==12)
      break
  }
  timer = Sys.RunTime + .5;while (Sys.RunTime < timer){} //wait .5 sec

  Motor[1].HomeVel=-3
  Motor[2].HomeVel=-3
  home1
  home2
  timer = Sys.RunTime + .5;while (Sys.RunTime < timer){} //wait .5 sec
  while(Motor[1].HomeInProgress || Motor[2].HomeInProgress){}

  if(Motor[1].HomeComplete)
    statusHoming=statusHoming|16
  else
  {
    statusHoming|=256
    send 1"Y_Achse home failed\n"
  }

  if(Motor[2].HomeComplete)
    statusHoming=statusHoming|32
  else
  {
    statusHoming|=256
    send 1"X_Achse home failed\n"
  }

  //Motor[1].PhasePos=560 // 555   581  593  558
  //Motor[2].PhasePos=1540 //1549  1531 1543 1537
  homez6,7

  goto 1

N0: //ERROR_END
    statusHoming|=256

N1: //ENDPLC
  Motor[1].MaxDac=L10
  Motor[1].FatalFeLimit=L11
  Motor[1].JogSpeed=L12
  Motor[2].MaxDac=L20
  Motor[2].FatalFeLimit=L21
  Motor[2].JogSpeed=L22
  send 1"homeing done\n"

  disable plc PLC_Homing
close