MToolTCPCalib

RoboRob

Does anyone know if it's possible to TPWrite the max_TCPerr,mean_TCPerr to the flex pendant when using this command in a routine? 

[Deleted User]

Here is a routine I wrote that writes the max and mean error.

 

MODULE TCP_Aid(SYSMODULE)

   TASK PERS robtarget p1:=[[-616.395,-1682.85,1416.17],[0.220333,0.672182,0.231325,-0.667916],[-2,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 
 TASK PERS tooldata TCP_Old:=[TRUE,[[365.401,-97.6168,246.953],[0.092296,0.701057,-0.092296,0.701057]],[34.7,[38.4,18,131.6],[1,0,0,0],1.086,1.348,1.362]];
 TASK PERS tooldata TCP_TCP:=[TRUE,[[365.401,-97.6168,246.953],[0.092296,0.701057,-0.092296,0.701057]],[34.7,[38.4,18,131.6],[1,0,0,0],1.086,1.348,1.362]];
 TASK PERS wobjdata TCP_Wobj:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]]]; 
  
     PERS jointtarget TCP_Clr:=[[-93.3922,16.7899,25.8387,0.191803,65.699,-113.72],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
     PERS jointtarget TCP_j1:=[[-102.187,28.3068,21.9536,-0.170654,68.9885,-124.376],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
     PERS jointtarget TCP_j2:=[[-107.323,25.9101,51.1249,128.832,36.6304,-247.551],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
     PERS jointtarget TCP_j3:=[[-112.807,21.5585,28.9757,5.6044,53.7068,-176.337],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
     PERS jointtarget TCP_j4:=[[-105.805,24.2599,24.3025,46.9949,41.881,-126.237],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
     PERS jointtarget TCP_jZ:=[[-100.881,25.8255,23.0675,-0.144653,67.8854,-123.088],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 
 
   LOCAL PERS pos MyPos:=[0,0,0];
   LOCAL VAR num max_TCPerr:=1;
   LOCAL VAR num mean_TCPerr:=1;
   LOCAL VAR num Xi:=0;
  LOCAL VAR num Yi:=0;
   LOCAL VAR num Zi:=0;
   LOCAL VAR bool inDebug:=TRUE;
 
 
  

  PROC ACALIB_TCP()
 
 TPErase;
 GetEEInterface; 
 TCP_TCP:=EEtooldata;
 TCP_TCP.tload:=EEtooldata.tload;
 TCP_TCP:=TCP_Old;
 TCP_Wobj := Wobj0;
 MyPos := [0,0,0];
 !.. clearance move
 MoveAbsJ TCP_ClrNoEOffs, v200, z50, TCP_TCPWObj:=TCP_Wobj;
 !.. move to position-1
 !.. and update position-1
 MoveAbsJ TCP_j1NoEOffs, v200, fine, TCP_TCPWObj:=TCP_Wobj;
TPWrite "Move your TOOL to desired location . Press PLAY when ready.";
StopNoRegain;
TPReadFK reg1, "Continue?", stEmpty, stEmpty, stEmpty, stEmpty, "Yes";
TPErase;
    WaitTime 0.3;
    TCP_j1:=CJointT();
 MoveAbsJ TCP_ClrNoEOffs, v200, z50, TCP_TCPWObj:=TCP_Wobj;
 
 !.. move to position-2
 !.. and update position-2
    MoveAbsJ TCP_j2NoEOffs, v200, fine, TCP_TCPWObj:=TCP_Wobj;
TPWrite "Move your TOOL to desired location . Press PLAY when ready.";
StopNoRegain;
TPReadFK reg1, "Continue?", stEmpty, stEmpty, stEmpty, stEmpty, "Yes";
TPErase;
 WaitTime 0.3;
    TCP_j2:=CJointT();
 MoveAbsJ TCP_ClrNoEOffs, v200, z50, TCP_TCPWObj:=TCP_Wobj;
 
 !.. move to position-2
 !.. and update position-3
    MoveAbsJ TCP_j3NoEOffs, v200, fine, TCP_TCPWObj:=TCP_Wobj;
TPWrite "Move your TOOL to desired location . Press PLAY when ready.";
StopNoRegain;
TPReadFK reg1, "Continue?", stEmpty, stEmpty, stEmpty, stEmpty, "Yes";
TPErase;
 WaitTime 0.3;
    TCP_j3:=CJointT();
 MoveAbsJ TCP_ClrNoEOffs, v200, z50, TCP_TCPWObj:=TCP_Wobj;
 
 !.. move to position-2
 !.. and update position-4
    MoveAbsJ TCP_j4NoEOffs, v200, fine, TCP_TCPWObj:=TCP_Wobj;
TPWrite "Move your TOOL to desired location . Press PLAY when ready.";
StopNoRegain;
TPReadFK reg1, "Continue?", stEmpty, stEmpty, stEmpty, stEmpty, "Yes";
TPErase;
 WaitTime 0.3;
    TCP_j4:=CJointT();
 MoveAbsJ TCP_ClrNoEOffs, v200, z50, TCP_TCPWObj:=TCP_Wobj;
 
 !.. refine Z-Elongator and X-Elongator jointtargets for TCP orientation
 !.. and update position-5 
 p1:=CalcRobT(TCP_j1, TCP_TCPWObj:=TCP_Wobj);
 MoveL RelTool(p1,0,0,-50), v200, fine,TCP_TCPWObj:=TCP_Wobj; 
    TCP_jz:=CJointT();
 MoveAbsJ TCP_jzNoEOffs, v200, z50, TCP_TCPWObj:=TCP_Wobj;

 !.. Define the TCP_TCP as the 4-point calibrated position
 !.. Define the Z-Elongator of TCP_TCP between TCP_j1 and TCP_jZ
 !MToolRotCalib TCP_j1, TCP_jZ, TCP_TCP;
 MToolTCPCalib TCP_j1,TCP_j2,TCP_j3,TCP_j4,TCP_TCP,max_TCPerr,mean_TCPerr;
IF inDebug THEN
  TPErase;
  tpwrite "--[ TCP DEFINITION.. ]----------";
  Xi:=TCP_TCP.tframe.trans.x;
  Yi:=TCP_TCP.tframe.trans.y;
  Zi:=TCP_TCP.tframe.trans.z;
  tpwrite "Transl :=  "Pos:=[Xi,Yi,Zi];
      Xi:=EulerZYX(X,TCP_TCP.tframe.rot);
      Yi:=EulerZYX(Y,TCP_TCP.tframe.rot);
      Zi:=EulerZYX(,TCP_TCP.tframe.rot);
  tpwrite "Rotatn :=  "Pos:=[Xi,Yi,Zi];
  tpwrite "--------------------------------";
  Tpwrite "TCP Max Error = "Num:=max_TCPerr;
  Tpwrite "TCP Mean Error = "Num:=mean_TCPerr;
 ELSE
  TPErase;
     Xi:=EulerZYX(X,TCP_TCP.tframe.rot);
      Yi:=EulerZYX(Y,TCP_TCP.tframe.rot);
      Zi:=EulerZYX(,TCP_TCP.tframe.rot);
 ENDIF 
 
 IF max_TCPerr > .8 THEN
 TpWrite "Put on your glasses on and try again";
 ELSE
  IF ActiveEEconfig.EEtooldataname = "CIH1tool" THEN
  CIH1tool.tframe:=TCP_TCP.tframe;
  CIH1tool.tload:=TCP_TCP.tload;
  ENDIF
 ENDIF 
 !.. clearance move
 MoveAbsJ TCP_ClrNoEOffs, v500, z50, TCP_TCPWObj:=TCP_Wobj;
 Park;
 
  ENDPROC

ENDMODULE