&ACCESS RVO2
&REL 134
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
&PARAM EDITMASK = *
DEF MoveToFrame( )
;FOLD INI
  ;FOLD BASISTECH INI
    GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
    INTERRUPT ON 3 
    BAS (#INITMOV,0 )
  ;ENDFOLD (BASISTECH INI)
  ;FOLD USER INI
  INTERRUPT ON 5  ; our 'estop' in the SA Robot interface
  
  ; Enable the interrupt for sending data and start the timer
  ;INTERRUPT ON 7
  ;$TIMER_STOP[5] = FALSE
  
  ;ENDFOLD (USER INI)
;ENDFOLD (INI)

; Move to Commanded Frame position
$ADVANCE=0
;FOLD Move to Commanded Frame position
  InMoveFcn = TRUE
  $BWDSTART=FALSE
  ;PPDAT.VEL = velocityPct
  ;PPDAT.ACC = accelerationPct
  PDAT_ACT=PPDAT
  FDAT_ACT=FPDAT
  BAS(#CP_PARAMS,velocityPct)
  BAS(#ACC_CP,accelerationPct)
  ;BAS(#PTP_PARAMS,100)
  ;S ?B010?,T ?B1010?
  PPOS.X=position[1]
  PPOS.Y=position[2]
  PPOS.Z=position[3]
  PPOS.A=position[4]
  PPOS.B=position[5]
  PPOS.C=position[6]
  PPOS.S=closure[1]
  PPOS.T=closure[2]
  LIN PPOS

;ENDFOLD
$ADVANCE=3
; Inform the RAC that the motion is complete
SendMotionComplete()

;ProgramComplete()

END