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

MODULE SCOUTCAL(SYSMODULE,NOSTEPIN)
  !  Copyright (c) 2002-2003 ABB Automation Technology Products AB
  !
  !  Delivered since RW 4.0.90
  !  Owner of this Module is 
  !   Leo Krachler Collin ATRM/RO
  !  
  !  Calibration module for SCOUT laser tracker
  !  Requires SCOUT software version 2.18 or higher (due to sign of RotX)
  !  This module requires module OptData.sys  (RW 4.0.90 or higher)
  !
  !  Revision history
  !  ----------------
  !  - Removed tSCOUT tooldata, changed back to tCalib           28/08/03 soko
  !  - Added SetupSCOUTcal                                       20/08/03 soko
  !  - Introduced tSCOUT tooldata                                20/08/03 soko
  !  - Split of SCOUTCAL.sys and SCOUTRANS.sys (encrypted)       05/08/03 soko
  !  - Changed sign of RotX                                      04/08/03 soko
  !  - Changed joint numbers for calibration to 1 and 2          18/07/03 soko
  !  - Added comments                                            18/07/03 soko
  !  - Added restore old sensor settings after calibration       18/07/03 soko
  !  - Removed dependency of CalcSensTrans from CalType          17/07/03 soko
  !  - Changed to 0.01 mm resolution                             17/07/03 soko
  !  - Introduced Revision history                               17/07/03 soko 
  !
  CONST num SC_Version:=1.4;
  ! Calibration Plate Geometry: Corner of V in Wobj coordinates
  CONST robtarget SC_cal_corner0:=[[90,50,0],[0,1,0,0],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  CONST robtarget SC_cal_corner1:=[[105,70,0],[0,1,0,0],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  LOCAL VAR robtarget SC_cal_corner:=[[90,50,0],[0,1,0,0],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
  CONST num SC_CornerAngle:=30;
  CONST num SC_new_type:=0;
  CONST num SC_old_type:=1;
  ! SCOUT geometry (angle between camera axis and laser beam)
  ! calibration results
  ! SCOUT joint parameters
  CONST num ABBCAL_L:=1;
  CONST num ABBCAL_R:=2;
  ! Number of sensor reads for averaging during calibration
  CONST num SC_NumReads:=10;
  ! data scaling
  LOCAL CONST num SC_pLORES:=0;
  LOCAL CONST num SC_pHIRES:=1;
  ! Wait Time for data transmission
  LOCAL CONST num SC_Wait:=0.1;
  ! LTAPP variables
  LOCAL CONST num SC_vSTART:=6;
  LOCAL CONST num SC_vSUSPEND:=7;
  LOCAL CONST num SC_vrXPOS:=8;
  LOCAL CONST num SC_vrYPOS:=9;
  LOCAL CONST num SC_vrZPOS:=10;
  LOCAL CONST num SC_vrGAP:=11;
  LOCAL CONST num SC_vrJOINTNO:=16;
  LOCAL CONST num SC_vrTIME:=17;
  LOCAL CONST num SC_vrXAngle:=18;
  LOCAL CONST num SC_vrUNIT:=19;
  LOCAL CONST num SC_vrYANGLE:=51;
  LOCAL CONST num SC_vrZANGLE:=52;
  LOCAL CONST num SC_vrFLANGE:=53;
  LOCAL VAR num SC_Alpha;
  LOCAL VAR pose LSeamPose_S;
  LOCAL VAR pose RSeamPose_S;
  LOCAL VAR pose LSeamPose_C;
  LOCAL VAR pose RSeamPose_C;
  LOCAL VAR pose C_P_T;
  LOCAL VAR pose T_P_C;
  LOCAL VAR pose T0_P_C;
  LOCAL VAR pose C_P_T0;
  LOCAL VAR pose C_P_S_L;
  LOCAL VAR pose C_P_S_R;
  LOCAL VAR pose T_P_S;
  LOCAL VAR num SC_OldUnit;
  LOCAL VAR num SC_OldJointNo;
  ! Flags
  LOCAL CONST num SC_pYES:=1;
  LOCAL CONST num SC_pNO:=0;
  LOCAL PERS tooldata ttool:=[TRUE,[[238.79,0.38067,289.445],[0.924277,0.010631,0.380335,0.030725]],[1.5,[150.7,-17.2,278.8],[1,0,0,0],0,0,0.141]];
  LOCAL VAR num SC_lang;
  LOCAL CONST string SC_msg{20,5}:=[["Do you want to setup sensor calibration?","Sensorkalibrierung einrichten?","","",""],["Is the active tool ","Ist das eingestellte Werkzeug ","","",""],[" correct?"," richtig?","","",""],["Type of calibration plate?","Welche Kalibrierplatte?","","",""],["Press OK, teach wobj abbcal, then press START","OK drücken, Wobj abbcal teachen, START drücken","","",""],["Moving to origin of calibration plate","Bewegen zum Ursprung der Kalibrierplatte","","",""],["Tool at corner of calibration plate?","Werkzeug an der Ecke der Kalibrierplatte?","","",""],["Press OK, move to calibration position, then press START","OK drücken, Roboter zur Kalibrierposition bewegen, START drücken","","",""],["Can you see the V on the SCOUT video screen?","Können Sie das V auf dem SCOUT-Videomonitor sehen?","","",""],[" Check sensor settings and modify "," Prüfen und modifizieren Sie Naht-","","",""],[" image processing data if necessary:"," einstellungen am Sensor falls notwendig:","","",""],[" Joint number 1: left joint on screen"," Nahtnr. 1: linke Naht auf Bildschirm","","",""],[" Joint number 2: right joint on screen"," Nahtnr. 2: rechte Naht auf Bildschirm","","",""],[" Sensor settings OK and sensor in automatic mode?"," Sensoreinstellungen OK und Sensor in Automatikbetrieb?","","",""],["Sensor detecting left joint?","Erkennt Sensor die linke Naht?","","",""],["Sensor detecting right joint?","Erkennt Sensor die rechte Naht?","","",""],["Setup Successful! Calibrate?","Fertig. Jetzt kalibrieren?","","",""],["Select calib number:","Bitte Calib Nummer wählen:","","",""],[" Please save module OptTool.sys!"," Bitte Modul OptTool.sys speichern!","","",""],["","","","",""]];
  LOCAL CONST string SC_menuitem{6,5}:=[["OK","OK","","",""],["YES","JA","","",""],["NO","NEIN","","",""],["EXIT","ENDE","","",""],["MOVE","BEWEG","","",""],["CALIB","KALIB","","",""]];
  LOCAL VAR num SC_currmsg:=0;

  LOCAL FUNC string SC_Str(
    num n)

    RETURN SC_msg{n,SC_lang};
  ENDFUNC

  PROC SetupSCOUTcal()
    VAR num answer;
    VAR robtarget pTmp;
    VAR string stcurtool;

    TPErase;
    TEST OpMode()
    CASE OP_AUTO:
      TPReadFK answer,"Change to MANUAL MODE.","","","","","EXIT";
      EXIT;
    ENDTEST
    TPErase;
    TPReadFK answer,"Select language","ENG","DEU","","","";
    IF answer=1 SC_lang:=1;
    IF answer=2 SC_lang:=2;
    TPErase;
    TPReadFK answer,SC_Str(1),SC_menuitem{2,SC_lang},"","","",SC_menuitem{3,SC_lang};
    IF answer=5 EXIT;
    GetSysData ttool\ObjectName:=stcurtool;
    TPErase;
    TPReadFK answer,SC_Str(2)+stcurtool+SC_Str(3),SC_menuitem{2,SC_lang},"","","",SC_menuitem{4,SC_lang};
    IF answer=5 EXIT;
    TPErase;
    TPWrite "  -----------      ----------- ";
    TPWrite " |    | |    |    |   |   |   |";
    TPWrite " |   |   |   |    |    | |    | ";
    TPWrite " |           |    |           | ";
    TPWrite " |           |    |           | ";
    TPWrite "  -----------      ----------- ";
    TPReadFK answer,SC_Str(4),"","0","","1",SC_menuitem{4,SC_lang};
    IF answer=2 SC_cal_type:=0;
    IF answer=4 SC_cal_type:=1;
    IF answer=5 EXIT;
TeachSCcal:
    TPErase;
    IF SC_cal_type=0 THEN
      TPWrite " ";
      TPWrite "  -----------   ";
      TPWrite " |    | |    o X2  ";
      TPWrite " |   |   |   |  ";
      TPWrite " |           |  ";
      TPWrite " | Y1        o X1  ";
      TPWrite "  -o---------   ";
    ELSE
      TPWrite " ";
      TPWrite "  -----------   ";
      TPWrite " |   |   |   o X2  ";
      TPWrite " |    | |    |  ";
      TPWrite " |           |  ";
      TPWrite " | Y1        o X1  ";
      TPWrite "  -o---------   ";
    ENDIF
    TPReadFK answer,SC_Str(5),SC_menuitem{1,SC_lang},"","","",SC_menuitem{4,SC_lang};
    IF answer=5 EXIT;
    Stop;
    TPErase;
    pTmp:=CRobT();
    ttool:=CTool();
    TPReadFK answer,SC_Str(6),SC_menuitem{5,SC_lang},"","","",SC_menuitem{4,SC_lang};
    IF answer=5 EXIT;
    pTmp.trans:=[0,0,0];
    MoveL pTmp,v10,fine,ttool\WObj:=abbcal;
    TPErase;
    TPReadFK answer,SC_Str(7),SC_menuitem{2,SC_lang},"","","",SC_menuitem{3,SC_lang};
    IF answer=5 THEN
      GOTO TeachSCcal;
    ENDIF
    pTmp.trans:=[0,0,10];
    MoveL pTmp,v10,fine,ttool\WObj:=abbcal;
    pTmp.trans:=[40,50,5];
    MoveL pTmp,v10,fine,ttool\WObj:=abbcal;
MoveToCalPos:
    TPErase;
    TPWrite "  -----------   ";
    TPWrite " |   |   |   |   ";
    TPWrite " |    | |    |  ";
    TPWrite " |           |  ";
    TPWrite " |     o tool|  ";
    TPWrite "  -----------   ";
    TPReadFK answer,SC_Str(8),SC_menuitem{1,SC_lang},"","","",SC_menuitem{4,SC_lang};
    IF answer=5 EXIT;
    Stop\NoRegain;
    TPErase;
    TPReadFK answer,SC_Str(9),SC_menuitem{2,SC_lang},"","","",SC_menuitem{3,SC_lang};
    IF answer=5 THEN
      GOTO MoveToCalPos;
    ENDIF
CheckSensSet:
    TPErase;
    TPWrite SC_Str(10);
    TPWrite SC_Str(11);
    TPWrite SC_Str(12);
    TPWrite SC_Str(13);
    TPReadFK answer,SC_Str(14),SC_menuitem{2,SC_lang},"","","",SC_menuitem{4,SC_lang};
    IF answer=5 EXIT;
    WriteVar 16,1;
    WriteVar 6,1;
    TPErase;
    TPReadFK answer,SC_Str(15),SC_menuitem{2,SC_lang},"","","",SC_menuitem{3,SC_lang};
    IF answer=5 THEN
      GOTO CheckSensSet;
    ENDIF
    WriteVar 6,0;
    WaitTime 2;
    WriteVar 16,2;
    WriteVar 6,1;
    TPErase;
    TPReadFK answer,SC_Str(16),SC_menuitem{2,SC_lang},"","","",SC_menuitem{3,SC_lang};
    IF answer=5 THEN
      GOTO CheckSensSet;
    ENDIF
    pCalib5:=CRobT();
    TPErase;
    TPReadFK answer,SC_Str(17),SC_menuitem{6,SC_lang},"","","",SC_menuitem{4,SC_lang};
    IF answer=5 EXIT;
    TPErase;
    TPReadFK answer,SC_Str(18),"1","2","3","4","5";
    SCOUTcalib ttool,abbcal,SC_cal_type,answer;
    TPReadFK answer,SC_Str(19),"","","","",SC_menuitem{4,SC_lang};
    EXIT;
  ENDPROC

  PROC SCOUTcalib(
    PERS tooldata mytool,
    PERS wobjdata mywobj,
    num CalType,
    num CalNumber)

    VAR robtarget CalTarget;
    VAR num nErrCount;

    WaitTime\InPos,0;
    IF CalNumber<1 OR CalNumber>5 THEN
      ErrWrite "SCOUTcalib: Argument Error","CalNumber must be between 1 and 5";
      EXIT;
    ENDIF
    IF CalType<0 OR CalType>1 THEN
      ErrWrite "SCOUTcalib: Argument Error","CalType must be 0 or 1";
      EXIT;
    ENDIF
    CalTarget:=CRobT(\Tool:=mytool\WObj:=mywobj);
    IF CalType=SC_new_type THEN
      ! new calibration plate
      ! position of "V" vertex w.r.t. calibration wobj
      SC_cal_corner:=SC_cal_corner0;
      ! left joint frame in wobj coordinates
      LSeamPose_C.rot:=OrientZYX(-SC_CornerAngle/2,0,0);
      ! right joint frame in wobj coordinates
      RSeamPose_C.rot:=OrientZYX(SC_CornerAngle/2,0,0);
      SC_Alpha:=180-SC_CornerAngle/2;
    ELSE
      ! old calibration plate
      ! position of "V" vertex w.r.t. calibration wobj
      SC_cal_corner:=SC_cal_corner1;
      ! left joint frame in wobj coordinates
      LSeamPose_C.rot:=OrientZYX(SC_CornerAngle/2,0,0);
      ! right joint frame in wobj coordinates
      RSeamPose_C.rot:=OrientZYX(-SC_CornerAngle/2,0,0);
      ! opening angle of calibration V divided by 2
      SC_Alpha:=SC_CornerAngle/2;
    ENDIF
    LSeamPose_C.trans:=[0,0,0];
    RSeamPose_C.trans:=[0,0,0];
    ! Tool to wobj transformation at calibration position
    C_P_T.trans:=CalTarget.trans;
    C_P_T.rot:=CalTarget.rot;
    ! wobj to tool transformation at calibration position
    T_P_C:=PoseInv(C_P_T);
    ! Read current sensor resolution and store it
    SC_OldUnit:=ReadVar(SC_vrUNIT);
    ! Read current sensor resolution and store it
    SC_OldJointNo:=ReadVar(SC_vrJOINTNO);
    ! Select left joint for image processing, set high resolution
    InitSensor ABBCAL_L,SC_pHIRES;
    ! Start image processing
    WriteVar SC_vSTART,SC_pYES;
    ! Read 3D position of left joint
    LSeamPose_S:=ReadSens3D(SC_NumReads);
    ! Stop image processing
    WriteVar SC_vSTART,SC_pNO;
    ! Select right joint for image processing
    InitSensor ABBCAL_R,SC_pHIRES;
    ! Start image processing
    WriteVar SC_vSTART,SC_pYES;
    ! Read 3D position of right joint
    RSeamPose_S:=ReadSens3D(SC_NumReads);
    ! Stop image processing
    WriteVar SC_vSTART,SC_pNO;
    ! Calculate rotation of sensor frame w.r.t. calibration wobj using right joint measurement
    C_P_S_R:=CalcSensRot_C(RSeamPose_C,RSeamPose_S);
    ! Calculate translation of sensor frame w.r.t. calibration wobj
    C_P_S_R:=CalcSensTrans_C(C_P_S_R,LSeamPose_S,RSeamPose_S,SC_cal_corner,SC_Alpha);
    ! Calculate TCP-to-sensor transformation
    T_P_S:=PoseMult(T_P_C,C_P_S_R);
    PrintPose T_P_S,"Sensor frame w.r.t. tool";
    ! Calculate tool0-to-sensor transformation
    tCalib.tframe:=PoseMult(mytool.tframe,T_P_S);
    ! Store calibration in array using CalNumber
    scal_pose{CalNumber}:=tCalib.tframe;
    ! Restore old sensor settings
    InitSensor SC_OldJointNo,SC_OldUnit;
  ERROR
    TEST ERRNO
    CASE SEN_NO_MEAS:
      IF nErrCount<4 THEN
        Incr nErrCount;
        WaitTime 0.3;
        RETRY;
      ELSE
        TPWrite "No measurement. Check sensor position.";
        RAISE;
      ENDIF
    CASE SEN_NOREADY:
      TPWrite "Sensor not ready";
      RAISE;
      !General sensor error
    CASE SEN_GENERRO:
      TPWrite "General sensor error";
      RAISE;
      !Sensor busy:
    CASE SEN_BUSY:
      TPWrite "Sensor not in automatic mode";
      RAISE;
      !Unknown sensor
    CASE SEN_UNKNOWN:
      TPWrite "Sensor: Unknown command";
      RAISE;
      !External sensor error
    CASE SEN_EXALARM:
      TPWrite "Sensor: External equipment error";
      RAISE;
      !Internal sensor error
    CASE SEN_CAALARM:
      TPWrite "Sensor: Camera alarm";
      RAISE;
      !Sensor temperature error
    CASE SEN_TEMP:
      TPWrite "Sensor temperature error";
      RAISE;
      !Illegal communication value
    CASE SEN_VALUE:
      TPWrite "Sensor: Unknwon joint number";
      RAISE;
      !Sensor check failure
    CASE SEN_CAMCHECK:
      TPWrite "Sensor check failure";
      RAISE;
      !Communication error
    CASE SEN_TIMEOUT:
      TPWrite "Communication error";
      RAISE;
    DEFAULT:
      TPWrite "Unknown sensor error";
    ENDTEST
  ENDPROC

  LOCAL FUNC num toNum(
    num value,
    num scaling)

    IF value>32767 value:=value-65536;
    value:=value*scaling;
    RETURN value;
  ENDFUNC

  PROC InitSensor(
    num joint_no,
    num scaling)

    WriteVar SC_vrJOINTNO,joint_no;
    WriteVar SC_vrUNIT,scaling;
    WaitTime SC_Wait;
  ENDPROC

  LOCAL FUNC pose ReadSens3D(
    num nreads)

    VAR pose SeamPoseS;
    VAR num Rx;
    VAR num Ry;
    VAR num Rz;
    VAR num nResult;
    VAR num nErrCount:=0;
    VAR num readcounter;
    VAR num y_acc:=0;
    VAR num z_acc:=0;
    VAR num Rx_acc:=0;
    VAR num Ry_acc:=0;
    VAR num Rz_acc:=0;
    VAR num scaling;

    IF ReadVar(SC_vrUNIT)=0 THEN
      scaling:=0.1;
    ELSE
      scaling:=0.01;
    ENDIF
    FOR readcounter FROM 1 TO nreads DO
      y_acc:=y_acc+toNum(ReadVar(SC_vrYPOS),scaling);
      z_acc:=z_acc+toNum(ReadVar(SC_vrZPOS),scaling)-100;
      Rx_acc:=Rx_acc+toNum(ReadVar(SC_vrXAngle),0.1);
      Ry_acc:=Ry_acc+toNum(ReadVar(SC_vrYANGLE),0.1);
      Rz_acc:=Rz_acc+toNum(ReadVar(SC_vrZANGLE),0.1);
    ENDFOR
    SeamPoseS.trans.x:=0;
    SeamPoseS.trans.y:=y_acc/nreads;
    SeamPoseS.trans.z:=z_acc/nreads;
    Rx:=Rx_acc/nreads;
    Ry:=Ry_acc/nreads;
    Rz:=Rz_acc/nreads;
    SeamPoseS.rot:=OrientZYX(Rz,0,0)*OrientZYX(0,Ry,0)*OrientZYX(0,0,Rx);
    RETURN SeamPoseS;
  ERROR
    TEST ERRNO
    CASE SEN_NO_MEAS:
      IF nErrCount<4 THEN
        Incr nErrCount;
        WaitTime 0.3;
        RETRY;
      ELSE
        TPWrite "No measurement. Check sensor position.";
        RAISE;
      ENDIF
    CASE SEN_NOREADY:
      TPWrite "Sensor not ready";
      RAISE;
      !General sensor error
    CASE SEN_GENERRO:
      TPWrite "General sensor error";
      RAISE;
      !Sensor busy:
    CASE SEN_BUSY:
      TPWrite "Sensor not in automatic mode";
      RAISE;
      !Unknown sensor
    CASE SEN_UNKNOWN:
      TPWrite "Sensor: Unknown command";
      RAISE;
      !External sensor error
    CASE SEN_EXALARM:
      TPWrite "Sensor: External equipment error";
      RAISE;
      !Internal sensor error
    CASE SEN_CAALARM:
      TPWrite "Sensor: Camera alarm";
      RAISE;
      !Sensor temperature error
    CASE SEN_TEMP:
      TPWrite "Sensor temperature error";
      RAISE;
      !Illegal communication value
    CASE SEN_VALUE:
      TPWrite "Sensor: Unknwon joint number";
      RAISE;
      !Sensor check failure
    CASE SEN_CAMCHECK:
      TPWrite "Sensor check failure";
      RAISE;
      !Communication error
    CASE SEN_TIMEOUT:
      TPWrite "Communication error";
      RAISE;
    DEFAULT:
      TPWrite "Unknown sensor error";
    ENDTEST
  ENDFUNC

  LOCAL PROC PrintPose(
    pose mypose,
    string posename)

    VAR num Rx;
    VAR num Ry;
    VAR num Rz;

    TPWrite posename;
    TPWrite "x [mm]: "\Num:=mypose.trans.x;
    TPWrite "y [mm]: "\Num:=mypose.trans.y;
    TPWrite "z [mm]: "\Num:=mypose.trans.z;
    Rx:=EulerZYX(\X,mypose.rot);
    Ry:=EulerZYX(\Y,mypose.rot);
    Rz:=EulerZYX(\Z,mypose.rot);
    TPWrite "Rx [deg]: "\Num:=Rx;
    TPWrite "Ry [deg]: "\Num:=Ry;
    TPWrite "Rz [deg]: "\Num:=Rz;
  ENDPROC
ENDMODULE
