%%%
  VERSION: 1
  LANGUAGE: ENGLISH
%%%

MODULE ADVMOT (SYSMODULE, VIEWONLY)
  

  PROC HollowWristReset()
    ! Reset wrist joints on IRB5402
    VAR jointtarget joints_ori;
    VAR jointtarget joints_new;
    VAR num j36;
    VAR num j46;
    
    ! Get angle from horizontal arm to joint 6 and reset joint 4
    joints_ori:=CJointT();
    j36:=joints_ori.robax.rax_4+joints_ori.robax.rax_6;
    IndReset IRB,4\RefNum:=0\Short;
    
    ! Calculate the joint 6 preset value (which depends on the value at 
    ! which joint 4 was preset to) and reset joint 5 and 6
    joints_new:=CJointT();
    j46:=j36-joints_new.robax.rax_4;
    IndReset IRB,6\RefNum:=j46\Short;
    IndReset IRB,5\RefNum:=0\Short;
  ENDPROC
     
ENDMODULE
