/JOB
//NAME UF-ROT8
//POS
///NPOS 0,0,0,0,0,0
//INST
///DATE 2021/02/24 14:54
///ATTR SC,RW,MC
///GROUP1 RB1
///LVARS 25,25,25,25,0,25,0,0
NOP
' reference User Frame
GETARG LB000 IARG#(1)
' degree of rotation
GETARG LR000 IARG#(2)
' calc end of bend
SET LD000  EXPRESS LR000 * 10000
' calc 1st mid of bend
SET LD001  EXPRESS LR000 * 10000 / 3
' calc 2nd mid of bend
SET LD002  EXPRESS LR000 * 10000 / 3 * 2
' axis of rotation Rx-1 Ry-2 Rz-3
GETARG LB001 IARG#(3)
' speed mm/sec
GETARG LR001 IARG#(4)
SET LI000  EXPRESS LR001 * 10
' get over roation in degrees
GETARG LR002 IARG#(5)
' calc over rot8 mid
SET LD003  EXPRESS ( LR000 + LR002 / 2 ) * 10000
'calc over rot8
SET LD004  EXPRESS ( LR000 + LR002 ) * 10000
' get over roation delay
GETARG LR003 IARG#(6)
SET LI001  EXPRESS LR003 * 100
' new reference User Frame
GETARG LB003 IARG#(7)
' get uf x offset
GETARG LR011 IARG#(8)
SET LD011  EXPRESS LR011 * 1000
' get uf y offset
GETARG LR012 IARG#(9)
SET LD012  EXPRESS LR012 * 1000
' get uf z offset
GETARG LR013 IARG#(10)
SET LD013  EXPRESS LR013 * 1000
' get current pos
GETS LPX020 $PX000
' convert p-var to arg1 uf
CNVRT LPX020 LPX020 UF#(LB000)
' copy p-var
SET LP021 LP020
' clear p-var
SUB LP021 LP021
' add x offset
SETE LP021 (1) LD011
' add y offset
SETE LP021 (2) LD012
' add z offset
SETE LP021 (3) LD013
' copy p-var (x2)
SET LP022 LP021
SET LP023 LP021
' set xx
GETE LD021 LP022 (1)
ADD LD021 100
SETE LP022 (1) LD021
' set xy
GETE LD022 LP023 (2)
ADD LD022 100
SETE LP023 (2) LD022
MFRAME UF#(LB003) LPX021 LPX022 LPX023
' get current pos
GETS LPX000 $PX000
' convert p-var to arg1 uf
CNVRT LPX000 LPX000 UF#(LB003)
' copy p-var
SET LP001 LP000
' clear p-var
SUB LP001 LP001
' copy p-var (x9)
SET LP002 LP001
SET LP003 LP001
SET LP004 LP001
SET LP005 LP001
SET LP006 LP001
SET LP007 LP001
SET LP008 LP001
SET LP009 LP001
SET LP010 LP001
' set rotation
IFTHENEXP LB001=1
	' set Rx rotation pos 1
	 SETE LP001 (4) LD001
	' set Rx rotation pos 2
	 SETE LP002 (4) LD002
	' set Rx rotation pos 3
	 SETE LP003 (4) LD000
	' set Rx over rot8 mid pos
	 SETE LP004 (4) LD003
	' set Rx over rot8 pos
	 SETE LP005 (4) LD004
ELSEIFEXP LB001=2
	' set Ry rotation pos 1
	 SETE LP001 (5) LD001
	' set Ry rotation pos 2
	 SETE LP002 (5) LD002
	' set Ry rotation pos 3
	 SETE LP003 (5) LD000
	' set Ry over rot8 mid pos
	 SETE LP004 (5) LD003
	' set Ry over rot8 pos
	 SETE LP005 (5) LD004
ELSEIFEXP LB001=3
	' set Rz rotation pos 1
	 SETE LP001 (6) LD001
	' set Rz rotation pos 2
	 SETE LP002 (6) LD002
	' set Rz rotation pos 3
	 SETE LP003 (6) LD000
	' set Rz over rot8 mid pos
	 SETE LP004 (6) LD003
	' set Rz over rot8 pos
	 SETE LP005 (6) LD004
ENDIF
'calc pos 1
MULMAT LP006 LP001 LP000
'calc pos 2
MULMAT LP007 LP002 LP000
'calc pos 3
MULMAT LP008 LP003 LP000
' calc over rotate mid pos
MULMAT LP009 LP004 LP000
' calc over rotate pos
MULMAT LP010 LP005 LP000
MOVC LP006 V=LI000  //pos 1
MOVC LP007 V=LI000  //pos 2
MOVC LP008 V=LI000  //final bend pos
MOVL LP008 V=LI000  //movc break
'over rotation check
IFTHENEXP LR002<> 0.000000E+00
	 MOVC LP008 V=LI000  //final bend pos
	 MOVC LP009 V=LI000  //over rot8 mid pos
	 MOVC LP010 V=LI000  //over rot8 pos
	 MOVL LP010 V=LI000  // movc break
	' over rotate delay
	 TIMER T=LI002
	 MOVC LP010 V=LI000  //over rot8 pos
	 MOVC LP009 V=LI000  //over rot8 mid pos
	 MOVC LP008 V=LI000  //final bend pos
ENDIF
MOVL LP008 V=LI000  //final bend pos
END