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

MODULE SRCal(SYSMODULE)
  !  Copyright (c) 2002-2003 ABB Automation Technology Products AB
  !  Info: Calibration Routine for Laser Sensors with wide field of view (with 90 degree corner)
  !  such as ServoRobot M-SPOT 90, MetaTorch
  !  Based on old OptTool Version: 991015
  !
  !  Delivered since RW 4.0.90
  !  Owner of this Module is 
  !   Leo Krachler Collin ATRM/RO
  !  
  !  Revision history
  !  ----------------
  !  - created Module                                14/08/03 soko
  !
  !  camera variables
  !
  LOCAL CONST num SR_START:=6;
  LOCAL CONST num SR_SUSPEND:=7;
  LOCAL CONST num SR_XPOS:=8;
  LOCAL CONST num SR_YPOS:=9;
  LOCAL CONST num SR_ZPOS:=10;
  LOCAL CONST num SR_GAP:=11;
  LOCAL CONST num SR_JOINTNO:=16;
  LOCAL CONST num SR_TIME:=17;
  LOCAL CONST num SR_ANGLE:=18;
  LOCAL CONST num SR_UNIT:=19;
  LOCAL CONST num SR_AVGINT:=20;
  !  Camera check variables
  LOCAL CONST num SR_CamCheck:=5;
  LOCAL CONST num SR_FlashCheck:=21;
  LOCAL CONST num SR_CalibTbl:=22;
  LOCAL CONST num SR_Video:=23;
  LOCAL CONST num SR_CommCPU:=24;
  LOCAL CONST num SR_LaserTmp:=25;
  LOCAL CONST num SR_HeatSinkTmp:=26;
  LOCAL CONST num SR_LaserPw:=27;
  LOCAL CONST num SR_MotScan:=28;
  !   Additional info after a seam not found from SmartBoard and ICC board
  LOCAL CONST num SR_SMInfo:=29;
  LOCAL CONST num SR_ICCInfo:=30;
  LOCAL CONST num SR_CamReset:=3;
  !
  LOCAL CONST num SR_pON:=1;
  LOCAL CONST num SR_pOFF:=0;
  LOCAL CONST num SR_pYES:=1;
  LOCAL CONST num SR_pNO:=0;
  !  Predefined joints for calibration plate
  LOCAL CONST num SR_nLeftLap:=1;
  LOCAL CONST num SR_nRightLap:=2;
  !  Unit used in the sensor 
  LOCAL CONST num SR_mea_unit:=10;
  !  Angle between used tool Z axis and camera plane, rotation around Y axis
  VAR num SR_ScanAngle:=0;
  !  Defines distance between corner and scan
  LOCAL VAR num SR_nSCANLOC:=7;
  !  Defines the movement in Z for calib high
  LOCAL VAR num SR_nSCANHGT:=20;
  LOCAL VAR num SR_nTotErrCount;
  !  camera variables
  VAR num SR_nValidData;
  VAR iodev SR_ioLogFile;
  LOCAL VAR trackdata trData;
 
  LOCAL FUNC num JointY()
    VAR num nResult;
    VAR num nErrCount;

    nResult:=ReadVar(SR_YPOS);
    IF nResult>32767 nResult:=nResult-65536;
    nResult:=nResult/SR_mea_unit;
    RETURN nResult;
  ERROR
    IF ERRNO=SEN_NO_MEAS AND nErrCount<4 THEN
      Incr nErrCount;
      Incr SR_nTotErrCount;
      WaitTime 0.3;
      RETRY;
    ELSE
      !   TPWrite "Error requesting joint Y";
      RAISE;
    ENDIF
  ENDFUNC

  LOCAL PROC SelectJoint(
    num JointNo)

    VAR num nIntValue;
    VAR num nMaxTime;

    WriteVar SR_JOINTNO,JointNo;
    WriteVar SR_SUSPEND,SR_pNO;
    !  Changed from 0.3 to 0.5 back to 0.3
    WaitTime\InPos,0.3;
  ERROR
    RAISE;
  ENDPROC

  LOCAL PROC SR_FindJointA3(
    robtarget ToPoint,
    INOUT pose peResult,
    INOUT bool bResult,
    INOUT num Gap,
    INOUT num Angle,
    speeddata Speed,
    trackdata Trackd,
    PERS tooldata Tool,
    PERS wobjdata obWObj)

    VAR pose peArr{3};
    VAR num nErrCount;
    VAR num nGap{3};
    VAR num nAngle{3};

    MoveL ToPoint,Speed,fine,Tool\WObj:=obWObj;
    SelectJoint Trackd.joint_no;
    FOR i FROM 1 TO 3 DO
      peArr{i}:=GetJointLoc(nGap{i},nAngle{i},Trackd.calib_no);
      WaitTime 0.05;
    ENDFOR
    peArr{1}.trans:=peArr{1}.trans+peArr{2}.trans;
    peArr{1}.trans:=peArr{1}.trans+peArr{3}.trans;
    peArr{1}.trans:=peArr{1}.trans*(1/3);
    peResult:=peArr{1};
    Gap:=(nGap{1}+nGap{2}+nGap{3})/3;
    Angle:=(nAngle{1}+nAngle{2}+nAngle{3})/3;
    bResult:=TRUE;
  ERROR
    IF ERRNO=SEN_NO_MEAS THEN
      IF nErrCount<4 THEN
        Incr nErrCount;
        WaitTime 0.1;
        RETRY;
      ELSE
        bResult:=FALSE;
        RETURN;
      ENDIF
    ELSE
      RAISE;
    ENDIF
  ENDPROC


  LOCAL FUNC pose GetJointLoc(
    INOUT num nGap,
    INOUT num nAngle,
    num nCalibPos)

    VAR num nResultY;
    VAR num nResultZ;
    VAR robtarget pPresent;
    VAR pose peResult;

    WaitTime 0.01;
    !  Gap
    nGap:=ReadVar(SR_GAP);
    IF nGap>32767 nGap:=nGap-65536;
    IF nGap<0 nGap:=0;
    nGap:=nGap/SR_mea_unit;
    !  Angle
    nAngle:=ReadVar(SR_ANGLE);
    IF nAngle>32767 nAngle:=nAngle-65536;
    nAngle:=nAngle/SR_mea_unit;
    !  Y
    nResultY:=ReadVar(SR_YPOS);
    IF nResultY>32767 nResultY:=nResultY-65536;
    nResultY:=nResultY/SR_mea_unit;
    !  Z
    nResultZ:=ReadVar(SR_ZPOS);
    nResultZ:=nResultZ/SR_mea_unit;
    nResultZ:=nResultZ-scal_pos{nCalibPos}.z;
    !  Get present location
    WaitTime 0.01;
    pPresent:=CRobT(\Tool:=tCalib);
    !  Add measurement
    pPresent:=RelTool(pPresent,0,nResultY,nResultZ);
    peResult.trans:=pPresent.trans;
    peResult.rot:=pPresent.rot;
    RETURN peResult;
  ERROR
    RAISE;
  ENDFUNC


  LOCAL PROC WriteToErrLog(
    num CalibNo)

    !  Max 24 ch
    VAR string stHeader;
    !  Max 40 ch
    VAR string stReason;
    VAR string stRL2;
    VAR string stRL3;

    stHeader:="Camera calibration done";
    stReason:="Data for CalibNo: "+NumToStr(CalibNo,0);
    stRL2:="X= "+NumToStr(scal_pose{CalibNo}.trans.x,1)+" Y= "+NumToStr(scal_pose{CalibNo}.trans.y,1)+" Z= "+NumToStr(scal_pose{CalibNo}.trans.z,1);
    stRL3:="Z Offset:  "+NumToStr(scal_pos{CalibNo}.z,1);
    ErrWrite\W,stHeader,stReason\RL2:=stRL2;
  ENDPROC

  LOCAL FUNC tooldata AdjustSensTool(
    tooldata tTool,
    tooldata tTool2)

    CONST orient orTrTurnBack:=[0.707107,0,-0.707107,0];
    VAR robtarget pOrig;
    VAR robtarget pX;
    VAR pose peCalib;

    !  Convert tool and tool2 to robtargets
    pOrig.trans:=tTool.tframe.trans;
    pOrig.rot:=tTool.tframe.rot;
    pX.trans:=tTool2.tframe.trans;
    pX.rot:=tTool2.tframe.rot;
    !  Set up seam coordinate system
    peCalib:=DefFrame(pOrig,pX,RelTool(pOrig,0,10,0));
    peCalib.rot:=peCalib.rot*orTrTurnBack;
    !   Copy to tTool
    tTool.tframe:=peCalib;
    !   Print out result
    ! TPWrite "nAngY:= "\Num:=EulerZYX(\Y,peCalib.rot);
    !  TPWrite "nAngX:= "\Num:=EulerZYX(\X,peCalib.rot);
    ! TPWrite "peCalib:= "\Pos:=peCalib.trans;
    RETURN tTool;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC tooldata CalcSensTool(
    tooldata tTool,
    robtarget pTarget,
    PERS wobjdata WObj)

    !  pse2 point on corner
    VAR pose pse3:=[[0,0,0],[1,0,0,0]];
    VAR pose pse4:=[[0,0,0],[1,0,0,0]];
    VAR pose pse5:=[[0,0,0],[1,0,0,0]];
    VAR robtarget pPresent;
    VAR pose peTarget;

    peTarget.trans:=pTarget.trans;
    peTarget.rot:=pTarget.rot;
    !  get present point with tool 0
    WaitTime\InPos,1;
    pPresent:=CRobT(\Tool:=tool0\WObj:=WObj);
    pse3.trans:=pPresent.trans;
    pse3.rot:=pPresent.rot;
    pse4:=PoseInv(pse3);
    pse5:=PoseMult(pse4,peTarget);
    tTool.tframe:=pse5;
    RETURN tTool;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL PROC MoveRobXY(
    INOUT robtarget Pos1,
    INOUT num nMoveD,
    PERS tooldata Tool,
    PERS wobjdata WObj)

    VAR num nYCoord;
    VAR string st1;
    VAR string st2;
    CONST num nRevMove:=3;
    CONST num nMinMove:=4;

    MoveL Pos1,v10,fine,Tool\WObj:=WObj;
    WaitTime\InPos,0.5;
    !  Set Z offset on Y axis
    SelectJoint SR_nLeftLap;
    FOR j FROM nMinMove TO 20 STEP 1 DO
      nMoveD:=j;
      MoveL Offs(Pos1,nMoveD*Sin(45),nMoveD*Sin(45),0),v10,fine,Tool\WObj:=WObj;
      WaitTime\InPos,0.1;
      nYCoord:=JointY();
      st1:=ValToStr2(nYCoord,2,7);
      st2:=ValToStr2(nMoveD,0,3);
      st1:="Y="+st1+" nMoveDist="+st2;
      TPErase;
      TPWrite "Moving out";
      TPWrite st1;
      WaitTime 0.1;
    ENDFOR
    nMoveD:=nMoveD-nRevMove;
  ERROR
    IF ERRNO=SEN_NO_MEAS THEN
      nMoveD:=nMoveD-nRevMove;
      RETURN;
    ELSE
      RAISE;
    ENDIF
  ENDPROC

  LOCAL FUNC string ValToStr2(
    num nVal,
    num nDec,
    num nTotLength)

    VAR string st1;
    VAR num nStrL;
    VAR num nDecLoc;
    VAR num nAddO;

    st1:=ValToStr(Trunc(nVal\Dec:=nDec));
    IF nDec<>0 THEN
      nStrL:=StrLen(st1);
      nDecLoc:=StrFind(st1,1,".");
      IF nDecLoc=(nStrL+1) THEN
        !  Add a point
        st1:=st1+".";
        nAddO:=nDec;
      ELSE
        nAddO:=nDec-(nStrL-nDecLoc);
      ENDIF
      !  Check if 0 need to be added
      IF nAddO>0 THEN
        FOR i FROM 1 TO nAddO DO
          st1:=st1+"0";
        ENDFOR
      ENDIF
    ENDIF
    nStrL:=StrLen(st1);
    FOR i FROM nStrL TO nTotLength DO
      st1:=" "+st1;
    ENDFOR
    RETURN st1;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC robtarget PointToFrame(
    robtarget Point1,
    pose peFrame)

    VAR pose peFrameInv;
    VAR pose pePos1;

    pePos1.trans:=Point1.trans;
    pePos1.rot:=Point1.rot;
    peFrameInv:=PoseInv(peFrame);
    pePos1:=PoseMult(peFrameInv,pePos1);
    Point1.trans:=pePos1.trans;
    Point1.rot:=pePos1.rot;
    RETURN Point1;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC robtarget PointFromFrame(
    robtarget Point1,
    pose peFrame)

    VAR pose pePos1;

    pePos1.trans:=Point1.trans;
    pePos1.rot:=Point1.rot;
    pePos1:=PoseMult(peFrame,pePos1);
    Point1.trans:=pePos1.trans;
    Point1.rot:=pePos1.rot;
    RETURN Point1;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL PROC MeasureYZ(
    INOUT num YCoord,
    INOUT num ZCoord,
    num ZOffset,
    num JointNo)

    VAR num nY1{5};
    VAR num nY2{5};
    VAR num nZ1{5};
    VAR num nZ2{5};
    VAR string stInd:="*";
    VAR string st1;
    VAR string st2;
    VAR string st3;
    VAR string stJointType;

    SR_nTotErrCount:=0;
    SelectJoint JointNo;
    IF JointNo=1 THEN
      stJointType:="Left Lap";
    ELSE
      stJointType:="Right Lap";
    ENDIF
    WaitTime\InPos,1;
    FOR i FROM 1 TO 5 DO
      FOR j FROM 1 TO 5 DO
        nY1{j}:=JointY();
        nZ1{j}:=JointZ(ZOffset);
        st1:=ValToStr2(nY1{j},2,7);
        st2:=ValToStr2(nZ1{j},2,7);
        st3:=ValToStr2(SR_nTotErrCount,0,3);
        st1:="Y="+st1+" Z="+st2+" Tot retry="+st3;
        stInd:=stInd+"*";
        TPErase;
        TPWrite stJointType;
        TPWrite st1;
        TPWrite stInd;
        WaitTime 0.1;
      ENDFOR
      nY2{i}:=Average(nY1);
      nZ2{i}:=Average(nZ1);
    ENDFOR
    YCoord:=Average(nY2);
    ZCoord:=Average(nZ2);
    st1:=ValToStr2(YCoord,2,7);
    st2:=ValToStr2(ZCoord,2,7);
    st3:=ValToStr2(SR_nTotErrCount,0,3);
    st1:="Y="+st1+" Z="+st2+" Tot retry="+st3;
    TPErase;
    TPWrite "Average result";
    TPWrite st1;
    WaitTime 1;
  ERROR
    RAISE;
  ENDPROC

  LOCAL FUNC num JointZ(
    num ZOffset)

    VAR num nResult;
    VAR num nErrCount;

    nResult:=ReadVar(SR_ZPOS);
    nResult:=nResult/SR_mea_unit;
    nResult:=nResult-ZOffset;
    RETURN nResult;
  ERROR
    IF ERRNO=SEN_NO_MEAS AND nErrCount<4 THEN
      Incr nErrCount;
      Incr SR_nTotErrCount;
      WaitTime 0.3;
      RETRY;
    ELSE
      !   TPWrite "Error requesting joint Z";
      RAISE;
    ENDIF
  ENDFUNC

  LOCAL PROC FindScanCent(
    INOUT robtarget Pos1,
    INOUT robtarget CentScanPos,
    INOUT num ZOffset,
    PERS tooldata Tool,
    PERS wobjdata WObj,
    INOUT bool bResult)

    VAR num nXaxis{2};
    VAR num nYaxis{2};
    VAR num nXaxis2{2};
    VAR num nYaxis2{2};
    VAR num nYDist;
    VAR num nL1;
    VAR num nL2;
    VAR num nAngle1;
    VAR num nAngle2;
    VAR num nMoveD;
    VAR num nZ1{5};

    MoveL Pos1,v10,fine,Tool\WObj:=WObj;
    !  Set Z offset on Y axis
    SelectJoint SR_nLeftLap;
    WaitTime\InPos,0.5;
    ZOffset:=0;
    FOR j FROM 1 TO 5 DO
      nZ1{j}:=JointZ(ZOffset);
      WaitTime 0.1;
    ENDFOR
    ZOffset:=Average(nZ1);
    MeasureYZ nYaxis{1},nYaxis{2},ZOffset,SR_nLeftLap;
    MeasureYZ nXaxis{1},nXaxis{2},ZOffset,SR_nRightLap;
    MoveRobXY Pos1,nMoveD,Tool,WObj;
    MoveL Offs(Pos1,nMoveD*Sin(45),nMoveD*Sin(45),0),v10,fine,Tool\WObj:=WObj;
    WaitTime\InPos,0.5;
    MeasureYZ nYaxis2{1},nYaxis2{2},ZOffset,SR_nLeftLap;
    ! MeasureYZ nXaxis2{1},nXaxis2{2},ZOffset,SR_nRightLap;
    nL1:=DistC1C2(nYaxis,nXaxis);
    nL2:=DistC1C2(nYaxis,nYaxis2);
    nAngle1:=ASin((nMoveD*Sin(45))/nL2);
    nYDist:=Cos(nAngle1)*nL1;
    nAngle2:=ATan((nXaxis{2}-nYaxis{2})/Abs(nXaxis{1}-nYaxis{1}));
    MoveL Pos1,v10,fine,Tool\WObj:=obCalib;
    CentScanPos:=Pos1;
    WaitTime\InPos,0.5;
    CentScanPos.trans:=[0,0,0];
    CentScanPos.rot:=[1,0,0,0];
    CentScanPos.trans.y:=nYDist;
    CentScanPos:=RelTool(CentScanPos,0,0,0\Rz:=nAngle1);
    CentScanPos:=RelTool(CentScanPos,0,0,0\Ry:=SR_ScanAngle);
    CentScanPos:=RelTool(CentScanPos,0,0,0\Rx:=nAngle2);
    CentScanPos:=RelTool(CentScanPos,0,0,0\Ry:=-SR_ScanAngle);
    CentScanPos:=RelTool(CentScanPos,0,-nYaxis{1},0);
    !  Don't turn around x axis
    CentScanPos:=RelTool(CentScanPos,0,0,0\Ry:=SR_ScanAngle);
    CentScanPos:=RelTool(CentScanPos,0,0,0\Rx:=-nAngle2);
    CentScanPos:=RelTool(CentScanPos,0,0,0\Ry:=-SR_ScanAngle);
    !TPWrite "nL1: "\Num:=nL1;
    ! TPWrite "nL2: "\Num:=nL2;
    ! TPWrite "nAngle1: "\Num:=nAngle1;
    ! TPWrite "nYDist: "\Num:=nYDist;
    !TPWrite "nAngle2: "\Num:=nAngle2;
    !TPWrite "C Pos"\Pos:=CentScanPos.trans;
    IF nAngle1>46 OR nAngle1<44 THEN
      !  TPWrite "Result= FALSE";
      bResult:=FALSE;
    ELSEIF nYDist>(SR_nSCANLOC/Sin(45)+1) OR nYDist<(SR_nSCANLOC/Sin(45)-1) THEN
      !   TPWrite "Result= FALSE";
      bResult:=FALSE;
    ELSE
      !  TPWrite "Result= TRUE";
      bResult:=TRUE;
    ENDIF
    WaitTime 1;
  ERROR
    RAISE;
  ENDPROC

  LOCAL FUNC num DistC1C2(
    num Cord1{*},
    num Cord2{*})

    VAR num nY;
    VAR num nZ;

    nY:=Cord2{1}-Cord1{1};
    nZ:=Cord2{2}-Cord1{2};
    RETURN Sqrt(nY*nY+nZ*nZ);
  ERROR
    RAISE;
  ENDFUNC

  LOCAL PROC CheckJointSel()
    VAR num nYLeft;
    VAR num nYRight;

    SelectJoint SR_nLeftLap;
    WaitTime 0.5;
    nYLeft:=JointY();
    SelectJoint SR_nRightLap;
    WaitTime 0.5;
    nYRight:=JointY();
    IF nYLeft>(nYRight+5) THEN
      TPWrite "Joints defined correctly";
      WaitTime 0.5;
    ELSE
      TPWrite "nYLeft "\Num:=nYLeft;
      TPWrite "nYRight "\Num:=nYRight;
      TPWrite "Joints defined in-correctly";
      RAISE 1;
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  LOCAL FUNC num Average(
    num vector{*})

    VAR num index1;
    VAR num index2;
    VAR num max_indx;
    VAR num max;

    FOR index1 FROM 1 TO 4 DO
      max:=vector{index1};
      max_indx:=index1;
      FOR index2 FROM index1+1 TO 5 DO
        IF vector{index2}>max THEN
          max:=vector{index2};
          max_indx:=index2;
        ENDIF
      ENDFOR
      vector{max_indx}:=vector{index1};
      vector{index1}:=max;
    ENDFOR
    RETURN ((vector{2}+vector{3}+vector{4})/3);
  ENDFUNC

  LOCAL FUNC robtarget AlignWithZ(
    robtarget pPos1)

    VAR num nXRot;
    VAR num nYRot;
    VAR num nZRot;
    VAR num nChoice;

    nXRot:=Abs(EulerZYX(\X,pPos1.rot));
    nYRot:=Abs(EulerZYX(\Y,pPos1.rot));
    nZRot:=EulerZYX(\Z,pPos1.rot);
    ! TPWrite "nXRot: "\Num:=nXRot;
    !TPWrite "nYRot: "\Num:=nYRot;
    !TPWrite "nZRot: "\Num:=nZRot;
    IF nXRot<170 THEN
      TPWrite "Plate defined incorrectly";
      RAISE 1;
    ENDIF
    IF nYRot>10 THEN
      TPWrite "Plate defined incorrectly";
      RAISE 1;
    ENDIF
    pPos1.rot:=OrientZYX(nZRot,0,180);
    RETURN pPos1;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC robtarget CalcCorner(
    robtarget pPosOrig,
    robtarget pPosX,
    robtarget pPos2)

    VAR robtarget pPosZ;
    VAR pose peSeamFrame;
    VAR pose peSeamFrameInv;
    CONST orient orTurnBack:=[0.707107,-0.707107,0,0];
    VAR pose pePos1;

    !Set up seam coordinate system
    pPosZ:=RelTool(pPosOrig,0,0,-100\Rx:=0\Ry:=0\Rz:=0);
    peSeamFrame:=DefFrame(pPosOrig,pPosX,pPosZ);
    peSeamFrame.rot:=peSeamFrame.rot*orTurnBack;
    !Get position up to a seam coorinate system
    pePos1.trans:=pPos2.trans;
    pePos1.rot:=pPos2.rot;
    peSeamFrameInv:=PoseInv(peSeamFrame);
    pePos1:=PoseMult(peSeamFrameInv,pePos1);
    !  Use only X component
    pePos1.trans.y:=0;
    pePos1.trans.z:=0;
    !Convert back to original frame  
    pePos1:=PoseMult(peSeamFrame,pePos1);
    pPos2.trans:=pePos1.trans;
    pPos2.rot:=pePos1.rot;
    RETURN pPos2;
  ERROR
    RAISE;
  ENDFUNC

  PROC CameraCalib(
    num CalibNo,
    robtarget Pos1,
    robtarget Pos2,
    robtarget Pos3,
    robtarget Pos4,
    PERS robtarget PosSens,
    PERS tooldata Tool
    \num ScanAngle
    \num ScanLoc
    \num ScanHgt)

    VAR pose peTarget:=[[0,0,0],[1,0,0,0]];
    VAR robtarget pTarget;
    VAR num nChoice;
    VAR robtarget pPosC1;
    VAR robtarget pPosC2;
    VAR num err_count;
    VAR pose peCalib;
    VAR robtarget pCalib;
    VAR num nOffsZ;
    VAR num nTmp;
    VAR bool bTmp;
    VAR tooldata tCalibHigh;
    VAR bool bResult:=FALSE;
    VAR pose peResult;
    VAR num nGap;
    VAR num nAngle;
    VAR robtarget pCheck;

    IF OpMode()=OP_MAN_PROG THEN
      TPErase;
      TPWrite "Program to be used for calibration";
      TPWrite "of 2D laser sensor (ServoRobot, META)";
      TPReadFK nChoice," ","","","","EXIT","PROCEED";
      IF nChoice=4 RETURN;
    ENDIF
    IF Present(ScanAngle) THEN
      SR_ScanAngle:=ScanAngle;
    ELSE
      SR_ScanAngle:=0;
    ENDIF
    IF Present(ScanLoc) THEN
      SR_nSCANLOC:=ScanLoc;
    ELSE
      SR_nSCANLOC:=7;
    ENDIF
    IF Present(ScanHgt) THEN
      SR_nSCANHGT:=ScanHgt;
    ELSE
      SR_nSCANHGT:=20;
    ENDIF
    WriteVar SR_START,SR_pON;
    bLaser_On:=TRUE;
    !  Calculate corner
    pPosC1:=CalcCorner(Pos2,Pos1,Pos3);
    pPosC2:=CalcCorner(Pos3,Pos4,Pos2);
    !  Make average
    pPosC1.trans:=pPosC1.trans+pPosC2.trans;
    pPosC1.trans:=pPosC1.trans*0.5;
    !  Calculate frame for calibration plate
    peCalib:=DefFrame(pPosC1,Pos1,Pos4);
    obCalib.uframe:=peCalib;
    obCalib.oframe:=[[0,0,0],[1,0,0,0]];
    !  Move tool to corner
    pCalib:=PointToFrame(pPosC1,peCalib);
    pCalib:=AlignWithZ(pCalib);
    MoveJ Offs(pCalib,0,0,10),v50,fine,Tool\WObj:=obCalib;
    MoveL pCalib,v10,fine,Tool\WObj:=obCalib;
    IF OpMode()=OP_MAN_PROG THEN
      TPErase;
      TPWrite "With a correctly defined plate and";
      TPWrite "Tool Center Point the tool should point";
      TPWrite "at the corner of the plate";
      TPReadFK nChoice,"Proceed if on corner","","","","EXIT","PROCEED";
      IF nChoice=4 RETURN;
    ENDIF
    !  Convert PosSens to calibration frame
    pCalib:=PointToFrame(PosSens,peCalib);
    MoveL Offs(pCalib,0,0,5),v50,fine,Tool\WObj:=obCalib;
    MoveL pCalib,v10,fine,Tool\WObj:=obCalib;
    !  Check if joints are defined correctly
    CheckJointSel;
    WHILE bResult=FALSE DO
      !  Find center of scan on normal height
      WaitTime\InPos,5;
      pCalib:=CRobT(\Tool:=Tool\WObj:=obCalib);
      FindScanCent pCalib,pTarget,nOffsZ,Tool,obCalib,bResult;
      TPWrite "Center of scan found at ";
      TPWrite ""\Pos:=pTarget.trans;
      tCalib:=CalcSensTool(Tool,pTarget,obCalib);
      !  Define next start search location
      pCalib.rot:=[1,0,0,0];
      pCalib:=RelTool(pCalib,0,0,0\Rz:=45);
      pCalib.trans:=[0,SR_nSCANLOC/Sin(45),0];
      pCalib.trans:=[(SR_nSCANHGT*Tan(SR_ScanAngle)*Sin(45)),(SR_nSCANLOC/Sin(45)+SR_nSCANHGT*Tan(SR_ScanAngle)*Sin(45)),SR_nSCANHGT];
      !  Find center of scan on high location
      FindScanCent pCalib,pTarget,nTmp,tCalib,obCalib,bTmp;
      TPWrite "Center of scan high found at";
      TPWrite ""\Pos:=pTarget.trans;
      tCalibHigh:=CalcSensTool(Tool,pTarget,obCalib);
      !  Calculate calibration based on the two TCP
      tCalib:=AdjustSensTool(tCalib,tCalibHigh);
      !  Define new start point
      pCalib.trans:=[0,SR_nSCANLOC/Sin(45),0];
      pCalib.rot:=[1,0,0,0];
      pCalib:=RelTool(pCalib,0,0,0\Rz:=45);
      pCalib:=RelTool(pCalib,0,0,0\Ry:=SR_ScanAngle);
      MoveL pCalib,v10,fine,tCalib\WObj:=obCalib;
    ENDWHILE
    !  Copy data to scal_pose
    scal_pose{CalibNo}:=tCalib.tframe;
    scal_pos{CalibNo}.z:=nOffsZ;
    !  Update PosSens
    WaitTime\InPos,0.25;
    PosSens:=CRobT(\Tool:=Tool\WObj:=wobj0);
    WriteToErrLog CalibNo;
    !  WriteToLogFile CalibNo;
    !  Make a move with Tool and wobj0
    !  Calibration check
    pCheck:=PosSens;
    trData.calib_no:=CalibNo;
    trData.joint_no:=SR_nLeftLap;
    SR_FindJointA3 RelTool(PosSens,0,0,-5),peResult,bResult,nGap,nAngle,v20,trData,Tool,wobj0;
    IF bResult THEN
      pCheck.trans:=peResult.trans;
      MoveL RelTool(pCheck,0,0,-5),v20,fine,Tool\WObj:=wobj0;
      MoveL pCheck,v20,fine,Tool\WObj:=wobj0;
      TPReadFK nChoice,"Tool should now be on seam","","","","","PROCEED";
    ELSE
      TPReadFK nChoice,"Left seam could not be found","","","","","PROCEED";
    ENDIF
    trData.joint_no:=SR_nRightLap;
    SR_FindJointA3 RelTool(PosSens,0,0,-5),peResult,bResult,nGap,nAngle,v20,trData,Tool,wobj0;
    IF bResult THEN
      pCheck.trans:=peResult.trans;
      MoveL RelTool(pCheck,0,0,-5),v20,fine,Tool\WObj:=wobj0;
      MoveL pCheck,v20,fine,Tool\WObj:=wobj0;
      TPReadFK nChoice,"Tool should now be on seam","","","","","PROCEED";
    ELSE
      TPReadFK nChoice,"Right seam could not be found","","","","","PROCEED";
    ENDIF
    MoveL RelTool(PosSens,0,0,-10),v20,fine,Tool\WObj:=wobj0;
    WriteVar SR_START,SR_pOFF;
  ERROR
    RAISE;
  ENDPROC

  !

  LOCAL FUNC pos VectPointToCalib(
    robtarget Point,
    tooldata tool,
    trackdata Track
    \INOUT num Magnitude)

    VAR pos psTSVect;
    VAR num nPointCalibMagn;
    VAR pose toolinv;
    VAR pose target1;
    VAR pose pose0;
    VAR num calibno;

    !  Returns a point to camera calibration unit vector and length
    !* Calculate frame to sensor origo.
    !* target1:=target0*PoseInv(tool)*calib
    pose0.trans:=Point.trans;
    pose0.rot:=Point.rot;
    toolinv:=PoseInv(tool.tframe);
    target1:=PoseMult(pose0,toolinv);
    target1:=PoseMult(target1,scal_pose{Track.calib_no});
    psTSVect:=target1.trans-Point.trans;
    !  Make a unit vector
    nPointCalibMagn:=VectMagn(psTSVect);
    IF Present(Magnitude) Magnitude:=nPointCalibMagn;
    psTSVect:=psTSVect*(1/nPointCalibMagn);
    RETURN psTSVect;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC pose RelFrame(
    pose Frame,
    num OffsX,
    num OffsY,
    num OffsZ
    \num RotX
    \num RotY
    \num RotZ)

    VAR robtarget pTmp;

    pTmp.trans:=Frame.trans;
    pTmp.rot:=Frame.rot;
    pTmp:=RelTool(pTmp,OffsX,OffsY,OffsZ\Rx?RotX\Ry?RotY\Rz?RotZ);
    Frame.trans:=pTmp.trans;
    Frame.rot:=pTmp.rot;
    RETURN Frame;
  ENDFUNC
ENDMODULE
