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

//statusHoming:
// 1: M1 phased
// 2: M2 phased
// 4: M1 homed
// 8: M2 homed
// 16: M1 neg limit
// 32: M2 neg limit
// 64: homing done
// 128: 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
  timer = Sys.RunTime + 1;while (Sys.RunTime < timer){} //wait 1 sec

  Motor[1].HomeVel=2
  Motor[2].HomeVel=2

  home1
  home2
  timer = Sys.RunTime + 1;while (Sys.RunTime < timer){} //wait 1 sec
  statusHoming|=1
  while(1)
  {
    if(Motor[1].FeFatal && (statusHoming&2)==0)
    {
      statusHoming=statusHoming|2
      Motor[1].HomeVel=-Motor[1].HomeVel
      home1
    }
    if(Motor[2].HomeInProgress==0 && (statusHoming&4)==0)
    {
      statusHoming=statusHoming|4
      Motor[2].HomeVel=-Motor[2].HomeVel
      home2
    }
    if(Motor[1].HomeInProgress==0 && Motor[1].HomeInProgress==0)
    {
      statusHoming=statusHoming|8
      break
    }
  }

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


  Motor[1].MaxDac=L10
  Motor[1].FatalFeLimit=L11
  Motor[1].JogSpeed=L12
  Motor[2].MaxDac=L20
  Motor[2].FatalFeLimit=L21
  Motor[2].JogSpeed=L22

  disable plc PLC_Homing
close