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

MODULE OptTool(SYSMODULE)

  !  Delivered since RW 4.0.90
  !
  !  Owner of this Module is
  !   Leo Krachler Collin ATRM/RO
  !
  ! Info: Seam finding tools used for Optical tracking with no AWC present
  !
  ! Functionality Based on Version: 991015
  !
  !  Revision history
  !  ----------------
  !  - Made FUNC GetJointLoc global and added error handler				13/11/03 soko
  !  - Moved ServoRobot calibration functions to SRcal.sys           28/08/03 soko
  !  - removed some unused local functions                           28/08/03 soko
  !
  CONST errnum SEN_OFFSET_ERR:=60;
  CONST errnum SEN_NO_DATA:=61;
  CONST errnum SEN_XOFFS_ERR:=62;
  !  camera variables
  LOCAL CONST num vSTART:=6;
  LOCAL CONST num vSUSPEND:=7;
  LOCAL CONST num vrXPOS:=8;
  LOCAL CONST num vrYPOS:=9;
  LOCAL CONST num vrZPOS:=10;
  LOCAL CONST num vrGAP:=11;
  LOCAL CONST num vJOINTNO:=16;
  LOCAL CONST num vrTIME:=17;
  LOCAL CONST num vrANGLE:=18;
  LOCAL CONST num vUNIT:=19;
  LOCAL CONST num vrAVGINT:=20;
  !  Camera check variables
  LOCAL CONST num vrCamCheck:=5;
  LOCAL CONST num vrFlashCheck:=21;
  LOCAL CONST num vrCalibTbl:=22;
  LOCAL CONST num vrVideo:=23;
  LOCAL CONST num vrCommCPU:=24;
  LOCAL CONST num vrLaserTmp:=25;
  LOCAL CONST num vrHeatSinkTmp:=26;
  LOCAL CONST num vrLaserPw:=27;
  LOCAL CONST num vrMotScan:=28;
  !   Additional info after a seam not found from SmartBoard and ICC board
  LOCAL CONST num vrSMInfo:=29;
  LOCAL CONST num vrICCInfo:=30;
  LOCAL CONST num vCamReset:=3;
  !
  LOCAL CONST num pON:=1;
  LOCAL CONST num pOFF:=0;
  LOCAL CONST num pYES:=1;
  LOCAL CONST num pNO:=0;
  !  Predefined joints
  LOCAL CONST num nLEFT_LAP:=1;
  LOCAL CONST num nRIGHT_LAP:=2;
  !  Unit used in the sensor
  LOCAL CONST num mea_unit:=10;
  !  Angle between used tool Z axis and camera plane, rotation around Y axis
  VAR num nSCANANGLE:=0;
  !  Defines distance between corner and scan
  LOCAL VAR num nSCANLOC:=7;
  !  Defines the movement in Z for calib high
  LOCAL VAR num nSCANHGT:=20;
  LOCAL VAR num nTotErrCount;
  LOCAL PERS wobjdata obCalib:=[FALSE,TRUE,"",[[37.0989,-929.081,10.1321],[0.707162,-0.000370669,-0.000427701,-0.707051]],[[0,0,0],[1,0,0,0]]];
  LOCAL PERS wobjdata obSeam:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[980.13,90.63,822.55],[0.940399,-0.339697,-0.0150757,0.00546697]]];
  !  Speeddata used for all slow moves during SelfTeach
  LOCAL CONST speeddata vSlowMove:=[30,10,50,10];
  LOCAL CONST num nCLEARDIST:=-10;
  !  camera variables
  VAR pose peSeamPos{100};
  VAR num nSeamGap{100};
  VAR num nValidData;
  VAR iodev ioLogFile;
  PERS seamdata smZero:=[0,0,0,0,0,0,0,0,0];
  PERS seamdata smTmp:=[0,0,0,0,0,0,0,0,0];
  PERS welddata wdZero:=[5,0];
  PERS welddata wdTmp:=[5,0];
  PERS weavedata wvTmp:=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];
  PERS trackdata trZero:=[0,1,1000,10,100,0,0,0,0,0];
  PERS trackdata trTmp:=[0,1,1000,10,100,0,0,0,0,0];
  PERS bool bOptDebug:=TRUE;
  PERS bool bOptSetupAll:=FALSE;

  PROC OptSetup()
    VAR num nAnswer;

    IF RunMode()=RUN_INSTR_FWD THEN
      TPErase;
      TPReadFK nAnswer,"Setup all ScanRef",stEmpty,stEmpty,stEmpty,"Yes","No";
      IF nAnswer=4 THEN
        bOptSetupAll:=TRUE;
      ELSE
        bOptSetupAll:=FALSE;
      ENDIF
      TPErase;
      TPReadFK nAnswer,"Activate debug",stEmpty,stEmpty,stEmpty,"Yes","No";
      IF nAnswer=4 THEN
        bOptDebug:=TRUE;
      ELSE
        bOptDebug:=FALSE;
      ENDIF
    ELSE
      bOptSetupAll:=FALSE;
    ENDIF
  ENDPROC

  LOCAL FUNC pose GetPoseAverage(
    \switch ResetX,
    pose DataArray{*},
    num RangeLow,
    num RangeHigh)

    VAR pose peSum:=[[0,0,0],[1,0,0,0]];

    FOR i FROM RangeLow TO RangeHigh DO
      IF Present(ResetX) DataArray{i}.trans.x:=0;
      peSum.trans:=peSum.trans+DataArray{i}.trans;
    ENDFOR
    peSum.trans:=peSum.trans*(1/(RangeHigh-RangeLow+1));
    RETURN peSum;
  ERROR
    RAISE;
  ENDFUNC

  PROC OptScanSeam(
    \switch Setup,
    num ScanDst,
    robtarget ToPoint,
    PERS pose RetOffs
    \INOUT robtarget RetPoint
    \INOUT num Gap,
    PERS pose Reference,
    speeddata Speed,
    PERS trackdata Track,
    PERS tooldata Tool
    \PERS wobjdata WObj)

    VAR robtarget pEnd;
    VAR aw_inhibit AwBlockSetting;
    VAR pos psFoundPos;
    VAR num nGap;
    VAR pose peScanDst;
    VAR num nScanSpeed:=10;
    VAR bool bResult;
    VAR num nNoScans:=5;
    CONST num MaxRetry:=3;
    VAR num nCounter;
    VAR num nAnswer;
    VAR bool bDoSetup;

    tracklog:=TRUE;
    IF RunMode()=RUN_INSTR_FWD THEN
      MoveL ToPoint,Speed,fine,Tool\WObj?WObj;
      RETURN;
    ENDIF
    TrLogDelete;
    smTmp:=smZero;
    trTmp:=trZero;
    trTmp.joint_no:=Track.joint_no;
    trTmp.calib_no:=Track.calib_no;
    IF Present(Setup) THEN
      !  If setup create a frame for measurement, based on
      !  camera calibration point
      Reference:=GetSensorFrame(ToPoint,Tool,Track);
      nNoScans:=15;
    ENDIF
    wdTmp.weld_speed:=Abs(ScanDst)/(nNoScans*1.5*0.1);
    pEnd:=ToPoint;
    peScanDst:=RelFrame(Reference,ScanDst,0,0);
    pEnd.trans:=pEnd.trans+(peScanDst.trans-Reference.trans);
    !   Save present AW block settings
    AwBlockSetting:=awinhibit;
    WHILE (NOT bResult) AND (nCounter<MaxRetry) DO
      awinhibit:=[TRUE,TRUE,FALSE];
      ArcL\On,ToPoint,Speed,smTmp,wdTmp,wvTmp,fine,Tool\Wobj?WObj\Track:=trTmp;
      ArcL\Off,pEnd,Speed,smTmp,wdTmp,wvTmp,fine,Tool\Wobj?WObj\Track:=trTmp;
      awinhibit:=AwBlockSetting;
      bResult:=GetTrackResults(bDoSetup,1,nNoScans,Track.max_corr,RetOffs,psFoundPos,nGap,Reference\WObj?WObj);
      nCounter:=nCounter+1;
    ENDWHILE
    IF Present(RetPoint) THEN
      RetPoint:=ToPoint;
      RetPoint.trans:=psFoundPos;
    ENDIF
    IF Present(Gap) Gap:=nGap;
    IF (NOT bResult) THEN
      TPErase;
      TPWrite "Warning! Seam could not be found";
      TPWrite " ";
      TPReadFK nAnswer,"Make a selection","","","","","PROCEED";
    ENDIF
  ERROR
    ArcKill;
    RAISE;
  ENDPROC

  PROC TrLogDelete()
    ArcKill;
    Open "ram1disk:"\File:="track.log",ioLogFile\Write;
    Close ioLogFile;
  ERROR
    IF ERRNO=ERR_FILEOPEN THEN
      Close ioLogFile;
      RETRY;
    ENDIF
    RAISE;
  ENDPROC

  LOCAL FUNC bool TrEvalLog(
    INOUT pose SeamPos{*},
    INOUT num SeamGap{*},
    INOUT num Index,
    pose Reference,
    num MaxScans,
    num Limit
    \PERS wobjdata WObj)

    VAR string stOneLine;
    CONST string NoData:="---";
    VAR bool bNoData;
    VAR bool bResult;
    VAR pos psSeamPos;
    VAR pose peDisp;
    VAR num nSeamGap;
    VAR bool bExitLoop:=FALSE;
    VAR pose peInvTransform;
    VAR pose peTransform:=[[0,0,0],[1,0,0,0]];
    VAR pose peInvRef;
    VAR pose peResult;
    VAR pose peResultX0;

    IF Present(WObj) THEN
      peInvTransform:=PoseInv(PoseMult(WObj.uframe,WObj.oframe));
    ELSE
      peInvTransform:=PoseInv(peTransform);
    ENDIF
    peInvRef:=PoseInv(Reference);
    Open "ram1disk:"\File:="track.log",ioLogFile\Read;
    stOneLine:=ReadStr(ioLogFile);
    ! Log empty or first line in log empty?
    IF ((stOneLine<>"EOF") OR (StrLen(stOneLine) = 3)) THEN
      stOneLine:=ReadStr(ioLogFile);
    ENDIF
    peDisp:=C_PROGDISP.pdisp;
    Index:=0;
    WHILE (stOneLine<>"EOF") AND (NOT bExitLoop) DO
      IF StrLen(stOneLine)=80 THEN
        IF StrMatch(stOneLine,1,NoData)<40 THEN
          stOneLine:=ReadStr(ioLogFile);
        ELSE
          bResult:=TrLogGetGap(stOneLine,nSeamGap);
          stOneLine:=ReadStr(ioLogFile);
          IF bResult THEN
            bResult:=TrLogGetPos(stOneLine,psSeamPos);
            IF bResult AND (Index<Dim(SeamGap,1)) THEN
              peResult.rot:=[1,0,0,0];
              peResult.trans:=psSeamPos+peDisp.trans;
              peResult:=PoseMult(peInvTransform,peResult);
              peResult.trans:=peResult.trans-peDisp.trans;
              peResult:=PoseMult(peInvRef,peResult);
              peResultX0:=peResult;
              peResultX0.trans.x:=0;
              IF VectMagn(peResultX0.trans)<Limit THEN
                Index:=Index+1;
                SeamGap{Index}:=nSeamGap;
                SeamPos{Index}:=peResult;
                IF Index>=MaxScans bExitLoop:=TRUE;
              ENDIF
            ENDIF
          ENDIF
        ENDIF
      ENDIF
      stOneLine:=ReadStr(ioLogFile);
    ENDWHILE
    Close ioLogFile;
    RETURN bExitLoop;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC bool GetTrackResults(
    bool DoSetup,
    num ScanDir,
    num NoScans,
    num Limit,
    PERS pose RetOffs,
    INOUT pos RetPos,
    INOUT num Gap,
    PERS pose Reference
    \PERS wobjdata WObj)

    VAR num nRange;
    VAR pose pePos1:=[[0,0,0],[1,0,0,0]];
    VAR pose pePos2:=[[0,0,0],[1,0,0,0]];
    VAR pose pePos3:=[[0,0,0],[1,0,0,0]];
    VAR robtarget p1:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    VAR robtarget p2:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    VAR robtarget p3:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    VAR bool bResult;

    IF DoSetup THEN
      bResult:=trEvalLog(peSeamPos,nSeamGap,nValidData,Reference,100,Limit\WObj?WObj);
      IF nValidData>NoScans THEN
        nRange:=Round(nValidData/7)+1;
        pePos1:=PoseMult(Reference,GetPoseAverage(\ResetX,peSeamPos,1,nRange));
        pePos1.rot:=Reference.rot;
        pePos3:=RelFrame(pePos1,0,10,0);
        pePos2:=PoseMult(Reference,GetPoseAverage(peSeamPos,(nValidData-nRange),nValidData));
        p1.trans:=pePos1.trans;
        p2.trans:=pePos2.trans;
        p3.trans:=pePos3.trans;
        ! Update reference
        Reference:=DefFrame(p1,p2,p3);
        !  Copy result
        RetPos:=Reference.trans;
        RetOffs:=[[0,0,0],[1,0,0,0]];
        !Gap:=GetNumAverage(nSeamGap,1,nRange);
        Gap:=0;
      ELSE
        ! Not enough data in log, update results and return false
        RetOffs:=[[0,0,0],[1,0,0,0]];
        RetPos:=Reference.trans;
        Gap:=0;
        RETURN FALSE;
      ENDIF
    ELSE
      IF trEvalLog(peSeamPos,nSeamGap,nValidData,Reference,NoScans,Limit\WObj?WObj) THEN
        IF bOptDebug THEN
          FOR i FROM 1 TO NoScans DO
            ErrWrite\W,"D"+NumToStr(nSeamGap{i},1)+" "+NumToStr(peSeamPos{i}.trans.x,1)+" "+NumToStr(peSeamPos{i}.trans.y,1)+" "+NumToStr(peSeamPos{i}.trans.z,1),"";
          ENDFOR
        ENDIF
        pePos1:=PoseMult(Reference,GetPoseAverage2(peSeamPos,1,NoScans));
        RetPos:=pePos1.trans;
        RetOffs.rot:=[1,0,0,0];
        RetOffs.trans:=pePos1.trans-Reference.trans;
        !Gap:=GetNumAverage(nSeamGap,1,NoScans);
        Gap:=0;
        IF bOptDebug ErrWrite\W,"R"+NumToStr(Gap,1)+" "+NumToStr(RetOffs.trans.x,1)+" "+NumToStr(RetOffs.trans.y,1)+" "+NumToStr(RetOffs.trans.z,1),"";
      ELSE
        ! Not enough data in log, update results and return false
        RetOffs:=[[0,0,0],[1,0,0,0]];
        RetPos:=Reference.trans;
        Gap:=0;
        RETURN FALSE;
      ENDIF
    ENDIF
    RETURN TRUE;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC pose GetPoseAverage2(
    pose DataArray{*},
    num RangeLow,
    num RangeHigh)

    VAR pose peSum:=[[0,0,0],[1,0,0,0]];
    VAR num nVect;
    VAR num nVectHigh;
    VAR num nIndexHigh;
    VAR num nVectLow:=1E+07;
    VAR num nIndexLow;

    FOR i FROM RangeLow TO RangeHigh DO
      DataArray{i}.trans.x:=0;
      nVect:=VectMagn(DataArray{i}.trans);
      peSum.trans:=peSum.trans+DataArray{i}.trans;
      IF nVect<nVectLow THEN
        nVectLow:=nVect;
        nIndexLow:=i;
      ENDIF
      IF nVect>nVectHigh THEN
        nVectHigh:=nVect;
        nIndexHigh:=i;
      ENDIF
    ENDFOR
    peSum.trans:=peSum.trans-DataArray{nIndexLow}.trans;
    peSum.trans:=peSum.trans-DataArray{nIndexHigh}.trans;
    peSum.trans:=peSum.trans*(1/(RangeHigh-RangeLow-1));
    RETURN peSum;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC num GetNumAverage(
    num DataArray{*},
    num RangeLow,
    num RangeHigh)

    VAR num nSum;

    FOR i FROM RangeLow TO RangeHigh DO
      nSum:=nSum+DataArray{i};
    ENDFOR
    nSum:=nSum/(RangeHigh-RangeLow+1);
    RETURN nSum;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC pos GetToolSensVect(
    robtarget ToPoint,
    tooldata tool,
    trackdata Trackd)

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

    calibno:=Trackd.calib_no;
    !* Calculate frame to sensor origo.
    !* target1:=target0*PoseInv(tool)*calib
    pose0.trans:=ToPoint.trans;
    pose0.rot:=ToPoint.rot;
    toolinv:=PoseInv(tool.tframe);
    target1:=PoseMult(pose0,toolinv);
    target1:=PoseMult(target1,scal_pose{calibno});
    psTSVect:=target1.trans-ToPoint.trans;
    !  Make a unit vector
    nToolSensDist:=Sqrt(psTSVect.x*psTSVect.x+psTSVect.y*psTSVect.y+psTSVect.z*psTSVect.z);
    psTSVect.x:=psTSVect.x/nToolSensDist;
    psTSVect.y:=psTSVect.y/nToolSensDist;
    psTSVect.z:=psTSVect.z/nToolSensDist;
    RETURN psTSVect;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC bool TrLogGetPos(
    string Str,
    INOUT pos SeamPos)

    VAR num nChPos;
    CONST string Space:=" ";
    VAR num nSpaceLoc;
    VAR num nSeamLoc{3};
    VAR num nIndex:=1;
    VAR string stPart;
    VAR bool bResult:=TRUE;

    nChPos:=StrLen(Str);
    WHILE nChPos>0 AND nIndex<4 AND bResult DO
      nSpaceLoc:=StrFindBKW(Str,nChPos,Space);
      IF (nChPos-nSpaceLoc)>1 AND nSpaceLoc<>0 THEN
        stPart:=StrPart(Str,nSpaceLoc+1,nChPos-nSpaceLoc);
        bResult:=StrToVal(stPart,nSeamLoc{nIndex});
        nIndex:=nIndex+1;
      ENDIF
      nChPos:=nSpaceLoc-1;
    ENDWHILE
    IF nIndex=4 AND bResult THEN
      SeamPos.x:=nSeamLoc{3};
      SeamPos.y:=nSeamLoc{2};
      SeamPos.z:=nSeamLoc{1};
      bResult:=TRUE;
    ELSE
      SeamPos:=[9E+09,9E+09,9E+09];
      bResult:=FALSE;
    ENDIF
    RETURN bResult;
  ERROR
    RAISE;
  ENDFUNC

  PROC ScanRef2(
    \switch Teach,
    num MaxMove,
    robtarget ToPoint,
    PERS pose RetOffs
    \INOUT robtarget RetPoint,
    PERS pose Reference
    \INOUT num Gap,
    speeddata Speed,
    PERS trackdata Trackd,
    PERS tooldata Tool,
    PERS wobjdata obWObj)

    VAR string stFKey{5};
    VAR num nErrCount;
    VAR num nAnswer;
    VAR pose peResult:=[[0,0,0],[1,0,0,0]];
    VAR pose peResultSeam;
    VAR bool bResult:=FALSE;
    VAR robtarget pReference;
    VAR robtarget pMoveVect;
    VAR pos psMoveVect;
    VAR num nMove;
    VAR num nLoopCount;
    VAR num nOffsetVector;
    VAR num nGap;
    VAR num nAngle;
    VAR num nIncrMove:=4;
    VAR string stTPOut;
    CONST speeddata vSlowMove:=[30,10,50,10];
    VAR num nLastOffset;
    !  New variable for extra info after a seam not found
    VAR string stMoreInfo;
    VAR num nErrCode;
    !
    !  Variables used for AutoProceed function
    VAR num nPresentZone;
    VAR num nDummy;

    !
    !  update tCalib
    tCalib:=Tool;
    tCalib.tframe:=scal_pose{Trackd.calib_no};
    !  Validate MaxMove
    !  Changed Maximum Move distance here
    IF Abs(MaxMove)>10 THEN
      IF MaxMove<0 THEN
        MaxMove:=-20;
      ELSE
        MaxMove:=20;
      ENDIF
    ENDIF
    !  If MaxMove = 0 then set it to 4
    IF MaxMove=0 MaxMove:=4;
    !  Define nIncrMove
    IF MaxMove<0 nIncrMove:=-nIncrMove;
    !
A:
    !  If in forward step, do setup
    IF RunMode()=RUN_INSTR_FWD THEN
      MoveL ToPoint,Speed,fine,Tool\WObj:=obWObj;
      SensorActivate;
      SelJointNo Trackd;
      TPErase;
      TPReadFK nAnswer,"Perform a Setup?","","","","YES","NO";
      TPErase;
      IF nAnswer=4 THEN
        ScanSeam MaxMove,ToPoint,Reference,bResult,nAngle,vSlowMove,Trackd,Tool,obWObj;
        IF bResult=FALSE THEN
          TPErase;
          TPWrite "Warning!";
          TPWrite "Seam could not be found";
          TPWrite "Adjust start point or used joint type";
          TPWrite "RAISE    Raise error";
          TPReadFK nAnswer,"Make a selection","","","","RAISE","";
          RAISE SEN_NO_DATA;
        ENDIF
        MoveL ToPoint,Speed,fine,Tool\WObj:=obWObj;
        IF RunMode()=RUN_INSTR_FWD THEN
          TPErase;
          TPWrite "Definition of SeamCoordinate system done";
          TPWrite "Do you want to update the ToPoint?";
          TPWrite "This function will help you program a";
          TPWrite "good search position";
          TPWrite " ";
          TPReadFK nAnswer,"Update ToPoint?","","","","YES","NO";
          TPErase;
        ELSE
          nAnswer:=5;
        ENDIF
        IF nAnswer=4 THEN
          PutScanOnSeam ToPoint,Reference,Tool,obWObj;
          !  Transfer pose back to used workobject
          Reference:=PoseFromFrame(peResult,Reference);
          !  for safety Reset RetOffs
          RetOffs:=[[0,0,0],[1,0,0,0]];
          RETURN;
        ENDIF
        Reference:=PoseFromFrame(peResult,Reference);
        !  for safety Reset RetOffs
        RetOffs:=[[0,0,0],[1,0,0,0]];
      ENDIF
    ENDIF
    !
    !  Find the seam
    nMove:=0;
    bResult:=FALSE;
    WHILE bResult=FALSE AND Abs(nMove)<=Abs(MaxMove) DO
      !  System will only retry one time
      !  WHILE bResult=FALSE AND Abs(nMove)<=4 DO
      FindJointA1 RelFrame2(ToPoint,nMove,Reference),peResult,bResult,nGap,nAngle,Speed,Trackd,Tool,obWObj;
      Speed:=vSlowMove;
      nMove:=nMove+nIncrMove;
      IF bResult=TRUE THEN
        !  Transfer result to reference frame
        peResultSeam:=PoseToFrame(peResult,Reference);
        !  Check for size of the offset
        nOffsetVector:=Sqrt(peResultSeam.trans.y*peResultSeam.trans.y+peResultSeam.trans.z*peResultSeam.trans.z);
        IF nOffsetVector>Trackd.max_corr THEN
          bResult:=FALSE;
          ! nPresentZone:=PresentZone(tWeldGun,nDummy);
          ! nModelNumber:=GInput(MOD_NO_GROUP);
          ! RobLogWrite nModelNumber,nPartCount,SEN_OFFSET_ERR,"Max_Correction","Auto retry",nPresentZone,nDummy\psApps:=peResultSeam.trans;
          TPWrite "ScanRef Max_Correction error: "\Num:=nOffsetVector;
        ENDIF
      ELSE
        !  Used to log  mumbert of retries
        ! nPresentZone:=PresentZone(tWeldGun,nDummy);
        ! nModelNumber:=GInput(MOD_NO_GROUP);
        stMoreInfo:=GetMoreInfo(\ErrCode:=nErrCode);
        TPWrite stMoreInfo;
        ! RobLogWrite nModelNumber,nPartCount,nErrCode,stMoreInfo,"Auto retry",nPresentZone,nDummy;
      ENDIF
    ENDWHILE
    IF bResult=FALSE THEN
      nOffsetVector:=Sqrt(RetOffs.trans.x*RetOffs.trans.x+RetOffs.trans.y*RetOffs.trans.y+RetOffs.trans.z*RetOffs.trans.z);
      IF nOffsetVector>Trackd.max_corr THEN
        ! Set do204WldFlt;
        TPErase;
        TPWrite "Warning! Seam could not be found";
        TPWrite " ";
        TPWrite "RetOffs larger than max_correction";
        TPWrite "Present offset";
        TPWrite ""\Pos:=RetOffs.trans;
        TPReadFK nAnswer,"Make a selection","","","","RAISE","";
        SensorSuspend;
        RAISE SEN_OFFSET_ERR;
      ENDIF
      TPErase;
      TPWrite "Warning! Seam could not be found";
      TPWrite " ";
      TPWrite "RETRY       Retry Search";
      TPWrite "PROCEED     Continue with last offset";
      TPWrite "Present offset";
      TPWrite ""\Pos:=RetOffs.trans;
      TPReadFK nAnswer,"Make a selection","","","","RETRY","PROCEED";
      SensorSuspend;
      IF nAnswer=5 THEN
        RETURN;
      ELSE
        GOTO A;
      ENDIF
    ENDIF
    !
    !  Transfer result to reference frame
    peResultSeam:=PoseToFrame(peResult,Reference);
    !  Check x value to se if the frame is close to found joint
    !  limit changd to MaxMove*1.5
    IF Abs(peResultSeam.trans.x)>Abs(MaxMove*2) OR Sqrt(peResultSeam.trans.y*peResultSeam.trans.y+peResultSeam.trans.z*peResultSeam.trans.z)>100 THEN
      !  Give warning that x distance to the reference is to large
      !  Or that the offset is larger than 100mm added 970409
      !  Instruction needs to be re-setup
      TPErase;
      TPWrite "Warning";
      TPWrite "Reference is to far away";
      TPWrite "New setup needs to be made";
      TPWrite "Seam XYZ "\Pos:=peResultSeam.trans;
      TPReadFK nAnswer,"Warning","","","","","RAISE";
      SensorSuspend;
      RAISE SEN_XOFFS_ERR;
    ENDIF
    peResultSeam.trans.x:=0;
    peResult:=PoseFromFrame(peResultSeam,Reference);
    !  Calculate offset
    RetOffs.rot:=[1,0,0,0];
    RetOffs.trans:=peResult.trans-Reference.trans;
    stTPOut:="Y="+ValToStr(Trunc(peResultSeam.trans.y\Dec:=1));
    stTPOut:=stTPOut+" Z="+ValToStr(Trunc(peResultSeam.trans.z\Dec:=1));
    stTPOut:=stTPOut+" Gap="+ValToStr(Trunc(nGap\Dec:=1));
    TPWrite stTPOut;
    IF Present(Gap) Gap:=nGap;
    !
    IF RunMode()=RUN_INSTR_FWD THEN
      TPReadFK nAnswer,"Move to found point?","","","","YES","NO";
      IF nAnswer=4 THEN
        ToPoint.trans:=peResult.trans;
        MoveL RelTool(ToPoint,0,0,nCLEARDIST),vSlowMove,fine,Tool\WObj:=obWObj;
        MoveL ToPoint,vSlowMove,fine,Tool\WObj:=obWObj;
      ENDIF
    ENDIF
    !
    IF Present(RetPoint) THEN
      ToPoint.trans:=peResult.trans;
      RetPoint:=ToPoint;
    ENDIF
    SensorSuspend;
    !
  ERROR
    IF ERRNO=SEN_TIMEOUT THEN
      IF SensorTimout()=TRUE THEN
        RETRY;
      ENDIF
    ENDIF
    RAISE;
  ENDPROC

  PROC LaserOn(
    \switch NoWaitTime
    \switch ResumePower)

    VAR num nErrCount;
    VAR num nAnswer;

    WriteVar vSTART,pON;
    bLaser_On:=TRUE;
    WriteVar vSUSPEND,pYES;
    IF Present(ResumePower) THEN
      WriteVar vSUSPEND,pNO;
    ENDIF
    IF NOT (Present(NoWaitTime)) THEN
      WaitTime 3.5;
    ENDIF
  ERROR
    IF ERRNO=SEN_TIMEOUT AND nErrCount<4 THEN
      Incr nErrCount;
      TPWrite "Sensor timout";
      WaitTime 0.5;
      RETRY;
    ELSE
      TPWrite "Warning! Possibly sensor external alarm";
      WaitTime 2;
      RAISE;
    ENDIF
  ENDPROC

  PROC LaserOff()
    VAR num nErrCount;

    bLaser_On:=FALSE;
    IF ReadVar(vSUSPEND)=pNO THEN
      WriteVar vSUSPEND,pYES;
    ENDIF
    WriteVar vSTART,pOFF;
  ERROR
    IF ERRNO=SEN_TIMEOUT AND nErrCount<1 THEN
      Incr nErrCount;
      TPWrite "Sensor timout";
      WaitTime 0.5;
      RETRY;
    ELSE
      TPWrite "Communication with sensor failed";
      RETURN;
    ENDIF
  ENDPROC

  LOCAL PROC SelectJoint(
    num JointNo)

    VAR num nIntValue;
    VAR num nMaxTime;

    WriteVar vJOINTNO,JointNo;
    WriteVar vSUSPEND,pNO;
    !  Changed from 0.3 to 0.5 back to 0.3
    WaitTime\InPos,0.3;
  ERROR
    RAISE;
  ENDPROC

  LOCAL FUNC bool SensorTimout()
    VAR num nErrCount;

    ErrWrite\W,"Sensor timout","Cami controller re-started";
    WaitTime 5;
    WriteVar vSTART,pON;
    WriteVar vSUSPEND,pYES;
    WaitTime 4;
    RETURN TRUE;
  ERROR
    IF ERRNO=SEN_TIMEOUT AND nErrCount<4 THEN
      Incr nErrCount;
      TPWrite "Sensor timout";
      WaitTime 1;
      RETRY;
    ELSE
      RETURN FALSE;
    ENDIF
  ENDFUNC

  LOCAL FUNC pose PoseToFrame(
    pose pePos1,
    pose peFrame)

    VAR pose peFrameInv;

    peFrameInv:=PoseInv(peFrame);
    pePos1:=PoseMult(peFrameInv,pePos1);
    RETURN pePos1;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC pose PoseFromFrame(
    pose pePos1,
    pose peFrame)

    pePos1:=PoseMult(peFrame,pePos1);
    RETURN pePos1;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL PROC PutScanOnSeam(
    robtarget ToPoint,
    pose peSeamFrame,
    PERS tooldata Tool,
    PERS wobjdata obWObj)

    VAR num nAnswer;
    VAR num nZOffset:=15;

    obSeam:=obWObj;
    !  Add peSeamFrame to object frame
    obSeam.oframe:=PoseMult(obSeam.oframe,peSeamFrame);
    ToPoint.trans:=[0,0,0];
    ToPoint.rot:=[1,0,0,0];
    !  rotate according to sensor scan angle
    ToPoint:=RelTool(ToPoint,0,0,0\Ry:=nSCANANGLE);
    TPErase;
    nAnswer:=0;
    WHILE nAnswer<>5 DO
      MoveL RelTool(ToPoint,0,0,nZOffset),vSlowMove,fine,tCalib\WObj:=obSeam;
      TPErase;
      TPWrite "Scan is now on the ToPoint location";
      TPWrite "Define a good search heigth by moving";
      TPWrite "the robot closer or further away";
      TPWrite "Press OK when the location is good ";
      TPReadFK nAnswer,"Adjust search heigth","Up","Down","","","OK";
      IF nAnswer=1 nZOffset:=nZOffset+5;
      IF nAnswer=2 nZOffset:=nZOffset-5;
    ENDWHILE
    !  Make a move with standard tool and workobject
    WaitTime\InPos,0.25;
    ToPoint:=CRobT(\Tool:=Tool\WObj:=obWObj);
    MoveL ToPoint,vSlowMove,fine,Tool\WObj:=obWObj;
  ENDPROC

  LOCAL FUNC robtarget RelFrame2(
    robtarget ToPoint,
    num Dx,
    pose peFrame)

    VAR robtarget pFrame;
    VAR robtarget pResult;
    VAR pos psMoveVect;

    !  Generate a vector used for movement in seam coord
    !  Copy data from frame
    pFrame.trans:=peFrame.trans;
    pFrame.rot:=peFrame.rot;
    !  Calculate vector
    pResult:=RelTool(pFrame,Dx,0,0);
    psMoveVect:=pResult.trans-pFrame.trans;
    !  add the vector to the ToPoint
    ToPoint.trans:=ToPoint.trans+psMoveVect;
    RETURN ToPoint;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC pose SeamFrame(
    num nMoveDist,
    pose pePos1,
    pose pePos2)

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

    pPos1.rot:=pePos1.rot;
    pPos2.rot:=pePos2.rot;
    IF nMoveDist>0 THEN
      pPos1.trans:=pePos1.trans;
      pPos2.trans:=pePos2.trans;
    ELSE
      pPos1.trans:=pePos2.trans;
      pPos2.trans:=pePos1.trans;
    ENDIF
    !  Define seam coordinate system
    pPosZ:=RelTool(pPos1,0,0,100);
    peSeamFrame:=DefFrame(pPos1,pPos2,pPosZ);
    peSeamFrame.rot:=peSeamFrame.rot*orTurnBack;
    peSeamFrame.trans:=pePos1.trans;
    RETURN peSeamFrame;
  ERROR
    RAISE;
  ENDFUNC

  LOCAL FUNC pose GetSensorFrame(
    robtarget ToPoint,
    tooldata tool,
    trackdata Trackd)

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

    calibno:=Trackd.calib_no;
    !* Calculate frame to sensor origo.
    !* target1:=target0*PoseInv(tool)*calib
    pose0.trans:=ToPoint.trans;
    pose0.rot:=ToPoint.rot;
    toolinv:=PoseInv(tool.tframe);
    target1:=PoseMult(pose0,toolinv);
    target1:=PoseMult(target1,scal_pose{calibno});
    RETURN target1;
  ERROR
    RAISE;
  ENDFUNC

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

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

    WaitTime 0.01;
    !  Gap
    nGap:=ReadVar(vrGAP);
    IF nGap>32767 nGap:=nGap-65536;
    IF nGap<0 nGap:=0;
    nGap:=nGap/mea_unit;
    !  Angle
    WaitTime 0.2;
    nAngle:=ReadVar(vrANGLE);
    IF nAngle>32767 nAngle:=nAngle-65536;
    nAngle:=nAngle/mea_unit;
    !  Y
    nResultY:=ReadVar(vrYPOS);
    IF nResultY>32767 nResultY:=nResultY-65536;
    nResultY:=nResultY/mea_unit;
    !  Z
    nResultZ:=ReadVar(vrZPOS);
    nResultZ:=nResultZ/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
    IF ERRNO=SEN_TIMEOUT AND nErrCount<4 THEN
      Incr nErrCount;
      TPWrite "Sensor timeout";
      WaitTime 0.5;
      RETRY;
    ELSE
      RAISE;
    ENDIF
  ENDFUNC

  LOCAL PROC 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 PROC FindJointA1(
    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 num nErrCount;

    MoveL ToPoint,Speed,fine,Tool\WObj:=obWObj;
    SelectJoint Trackd.joint_no;
    peResult:=GetJointLoc(Gap,Angle,Trackd.calib_no);
    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 PROC ScanSeam(
    num nMoveDist,
    robtarget ToPoint,
    INOUT pose peSeamFrame,
    INOUT bool bResult,
    INOUT num nSeamAngle,
    speeddata Speed,
    trackdata Trackd,
    PERS tooldata Tool,
    PERS wobjdata obWObj)

    VAR pose pePos1;
    VAR pose pePos2;
    VAR num nLoopCount;
    VAR robtarget ToPoint2;
    VAR pos psMoveVect;
    VAR num nGap;
    VAR num nTmp;
    CONST num nMAXRETRY:=4;

    !  Validate nMoveDist
    TPErase;
    TPWrite "Setting up a seam coordinate system";
    WaitTime 0.05;
    IF Abs(nMoveDist)<4 THEN
      IF nMoveDist<0 THEN
        nMoveDist:=-4;
      ELSE
        nMoveDist:=4;
      ENDIF
    ENDIF
    bResult:=FALSE;
    !  Find first point
    WHILE bResult=FALSE AND nLoopCount<nMAXRETRY DO
      FindJointA3 ToPoint,pePos1,bResult,nGap,nSeamAngle,Speed,Trackd,Tool,obWObj;
      Incr nLoopCount;
    ENDWHILE
    IF bResult=FALSE THEN
      !   No seam was found
      RETURN;
    ENDIF
    TPWrite "Point 1 found, Gap:= "\Num:=nGap;
    WaitTime 0.05;
    psMoveVect:=GetToolSensVect(ToPoint,Tool,Trackd);
    !  Calculate a vector for a short move
    IF nMoveDist>0 THEN
      psMoveVect:=psMoveVect*4;
    ELSE
      psMoveVect:=psMoveVect*(-4);
    ENDIF
    ToPoint2:=ToPoint;
    ToPoint2.trans:=ToPoint2.trans+psMoveVect;
    bResult:=FALSE;
    nLoopCount:=0;
    !  Find second temporary point
    WHILE bResult=FALSE AND nLoopCount<nMAXRETRY DO
      FindJointA3 ToPoint2,pePos2,bResult,nGap,nTmp,Speed,Trackd,Tool,obWObj;
      Incr nLoopCount;
    ENDWHILE
    IF bResult=FALSE THEN
      !   No seam was found
      RETURN;
    ENDIF
    peSeamFrame:=SeamFrame(nMoveDist,pePos1,pePos2);
    bResult:=FALSE;
    nLoopCount:=0;
    !  Find second real point
    WHILE bResult=FALSE AND nLoopCount<nMAXRETRY DO
      FindJointA3 RelFrame2(ToPoint,nMoveDist,peSeamFrame),pePos2,bResult,nGap,nTmp,Speed,Trackd,Tool,obWObj;
      Incr nLoopCount;
    ENDWHILE
    IF bResult=FALSE THEN
      !   No seam was found
      RETURN;
    ENDIF
    TPWrite "Point 2 found, Gap:= "\Num:=nGap;
    WaitTime 0.2;
    peSeamFrame:=SeamFrame(nMoveDist,pePos1,pePos2);
    !  TPWrite "Frame trans: "\Pos:=peSeamFrame.trans;
    ! TPWrite "Frame trans: "\Orient:=peSeamFrame.rot;
    ! Stop;
  ENDPROC

  LOCAL FUNC string GetMoreInfo(
    \INOUT num ErrCode)

    VAR string stSensorInfo;
    VAR num nSMInfo;
    VAR num vrICCInfo;

    RETURN "";
    !  Smart board information
    nSMInfo:=ReadVar(vrSMInfo);
    IF nSMInfo>32767 nSMInfo:=nSMInfo-65536;
    IF Present(ErrCode) ErrCode:=nSMInfo;
    TEST nSMInfo
    CASE 1:
      stSensorInfo:="SeamFound";
    CASE -1:
      stSensorInfo:="NoIntensity";
    CASE -2:
      stSensorInfo:="NoJointMatch";
    CASE -3:
      stSensorInfo:="NoJointMatch";
    CASE -4:
      stSensorInfo:="NoJointMatch";
    CASE -5:
      stSensorInfo:="PossiblyTack";
    CASE -6:
      stSensorInfo:="GapToLarge";
    CASE -7:
      stSensorInfo:="NoJointMatch";
    CASE -9:
      stSensorInfo:="InvalidAngle";
    DEFAULT:
      stSensorInfo:="SeamNotFound";
    ENDTEST
    !
    !  Icc board information
    vrICCInfo:=ReadVar(vrICCInfo);
    IF vrICCInfo>32767 vrICCInfo:=vrICCInfo-65536;
    !
    !  add information from the smart board
    TEST vrICCInfo
    CASE 1:
      stSensorInfo:=stSensorInfo+"";
    CASE -1:
      stSensorInfo:=stSensorInfo+"";
    CASE -2:
      stSensorInfo:=stSensorInfo+"";
    CASE -3:
      stSensorInfo:=stSensorInfo+"";
    CASE -4:
      stSensorInfo:=stSensorInfo+"";
    CASE -5:
      stSensorInfo:=stSensorInfo+"";
    CASE -6:
      stSensorInfo:=stSensorInfo+"";
    CASE -7:
      stSensorInfo:=stSensorInfo+"";
    CASE -9:
      stSensorInfo:=stSensorInfo+"";
    DEFAULT:
      stSensorInfo:=stSensorInfo+"";
    ENDTEST
    !
    RETURN stSensorInfo;
  ERROR
    TRYNEXT;
  ENDFUNC

  LOCAL PROC ReadYZGapAngl(
    INOUT num nResultY,
    INOUT num nResultZ,
    INOUT num nGap,
    INOUT num nAngle,
    num nCalibPos)

    !  Gap
    nGap:=ReadVar(vrGAP);
    IF nGap>32767 nGap:=nGap-65536;
    IF nGap<0 nGap:=0;
    nGap:=nGap/mea_unit;
    !  Angle
    nAngle:=ReadVar(vrANGLE);
    IF nAngle>32767 nAngle:=nAngle-65536;
    nAngle:=nAngle/mea_unit;
    !  Y
    nResultY:=ReadVar(vrYPOS);
    IF nResultY>32767 nResultY:=nResultY-65536;
    nResultY:=nResultY/mea_unit;
    !  Z
    nResultZ:=ReadVar(vrZPOS);
    nResultZ:=nResultZ/mea_unit;
    nResultZ:=nResultZ-scal_pos{nCalibPos}.z;
  ERROR
    RAISE;
  ENDPROC

  LOCAL PROC DispSeamData(
    num nJointNo)

    VAR num nErrCount;
    VAR num nY;
    VAR num nZ;
    VAR num nGap;
    VAR num nAngle;
    VAR string stTPOut;

    SelectJoint nJointNo;
    ReadYZGapAngl nY,nZ,nGap,nAngle,1;
    stTPOut:="Y="+ValToStr(Trunc(nY\Dec:=1));
    stTPOut:=stTPOut+" Z="+ValToStr(Trunc(nZ\Dec:=1));
    stTPOut:=stTPOut+" Gap="+ValToStr(Trunc(nGap\Dec:=1));
    stTPOut:=stTPOut+" Angl="+ValToStr(Trunc(nAngle\Dec:=0));
    stTPOut:=stTPOut+" Retry="+ValToStr(Trunc(nErrCount\Dec:=0));
    TPWrite stTPOut;
  ERROR
    IF ERRNO=SEN_NO_MEAS THEN
      IF nErrCount<4 THEN
        Incr nErrCount;
        WaitTime 0.1;
        RETRY;
      ELSE
        TPWrite GetMoreInfo();
        RETURN;
      ENDIF
    ELSEIF ERRNO=SEN_VALUE THEN
      TPWrite "Joint No not defined";
      RETURN;
    ELSE
      RAISE;
    ENDIF
  ENDPROC

  LOCAL PROC CamReset()
    WriteVar vCamReset,pON;
    TPWrite "Camera reset done";
    WaitTime 5;
    !   Turn laser back on
    WriteVar vSTART,pON;
    WriteVar vSUSPEND,pYES;
    ! RobLogWrite 0,nPartCount,55,"Camera reset","Man inter",0,0;
  ERROR
    TRYNEXT;
  ENDPROC

  LOCAL PROC SelJointNo(
    PERS trackdata Trackd)

    VAR num nAnswer;
    VAR num nJoint;
    CONST num nDispTime:=5;
    VAR string stDisplay;

    nJoint:=Trackd.joint_no;
    IF nJoint<1 nJoint:=1;
    IF nJoint>30 nJoint:=30;
    IF RunMode()=RUN_INSTR_FWD THEN
      WHILE nAnswer<>5 DO
        TPErase;
        TPWrite "Present Joint";
        TPWrite stJointDef{nJoint};
        TPWrite " ";
        DispSeamData nJoint;
        TPWrite " ";
        TPWrite "Press OK when joint values are good";
        nAnswer:=0;
        TPReadFK nAnswer,"If needed try new joint number with +/-","+","-","Check","","OK";
        IF nAnswer=1 THEN
          nJoint:=nJoint+1;
        ELSEIF nAnswer=2 THEN
          nJoint:=nJoint-1;
        ENDIF
      ENDWHILE
    ENDIF
    SelectJoint nJoint;
    Trackd.joint_no:=nJoint;
  ERROR
    RAISE;
  ENDPROC

  LOCAL PROC SensorSuspend()
    VAR num nErrCount;

    WriteVar vSUSPEND,pYES;
  ERROR
    IF ERRNO=SEN_TIMEOUT AND nErrCount<4 THEN
      Incr nErrCount;
      TPWrite "Sensor timout";
      WaitTime 1;
      RETRY;
    ELSE
      RAISE;
    ENDIF
  ENDPROC

  LOCAL PROC SensorActivate()
    VAR num nErrCount;

    WriteVar vSUSPEND,pNO;
  ERROR
    IF ERRNO=SEN_TIMEOUT AND nErrCount<4 THEN
      Incr nErrCount;
      TPWrite "Sensor timout";
      WaitTime 1;
      RETRY;
    ELSE
      RAISE;
    ENDIF
  ENDPROC

  LOCAL FUNC bool JointPresent()
    VAR num nResult;
    VAR num nErrCount;

    ! returns true if the joint is present
    nResult:=ReadVar(vrYPOS);
    RETURN TRUE;
  ERROR
    IF ERRNO=SEN_NO_MEAS AND nErrCount<2 THEN
      Incr nErrCount;
      Incr nTotErrCount;
      !  retry one time
      WaitTime 0.1;
      RETRY;
    ELSE
      !   TPWrite "Error requesting joint Y";
      RETURN FALSE;
    ENDIF
  ENDFUNC


  PROC CheckSeam(
    num moveX,
    num moveY,
    num moveZ,
    robtarget ToPoint,
    INOUT bool Result,
    speeddata Speed,
    PERS trackdata Trackd,
    PERS tooldata Tool,
    PERS wobjdata obWObj)

    MoveL ToPoint,Speed,fine,Tool\WObj:=obWObj;
    SelJointNo Trackd;
    !  Check if a seam is present
    Result:=JointPresent();
    IF Result=FALSE THEN
      MoveL Offs(ToPoint,moveX,moveY,moveZ),Speed,fine,Tool\WObj:=obWObj;
      SelJointNo Trackd;
      !  Check if a seam is present
      Result:=JointPresent();
    ENDIF
    IF Result=FALSE THEN
      MoveL Offs(ToPoint,-moveX,-moveY,-moveZ),Speed,fine,Tool\WObj:=obWObj;
      SelJointNo Trackd;
      !  Check if a seam is present
      Result:=JointPresent();
    ENDIF
    SensorSuspend;
    !
  ERROR
    IF ERRNO=SEN_TIMEOUT THEN
      IF SensorTimout()=TRUE THEN
        RETRY;
      ENDIF
    ENDIF
    RAISE;
  ENDPROC

  LOCAL FUNC num StrFindBKW(
    string Str,
    num ChPos,
    string Set)

    VAR bool bSetFound;

    WHILE ChPos>0 AND (NOT bSetFound) DO
      IF StrMemb(Str,ChPos,Set) THEN
        bSetFound:=TRUE;
      ELSE
        ChPos:=ChPos-1;
      ENDIF
    ENDWHILE
    RETURN ChPos;
  ERROR
    RAISE;
  ENDFUNC

  FUNC bool TrLogGetGap(
    string Str,
    INOUT num SeamGap)

    VAR bool bResult;

    bResult:=StrToVal(StrPart(Str,12,7),SeamGap);
    RETURN bResult;
  ERROR
    RAISE;
  ENDFUNC

  PROC ScanRef(
    \switch Setup,
    num ScanDst,
    robtarget ToPoint,
    PERS pose RetOffs
    \INOUT robtarget RetPoint,
    PERS pose Reference
    \INOUT num Gap,
    speeddata Speed,
    PERS trackdata Track,
    PERS tooldata Tool,
    PERS wobjdata obWObj)

    VAR robtarget pEnd;
    VAR aw_inhibit AwBlockSetting;
    VAR pos psFoundPos;
    VAR num nGap;
    VAR pose peScanDst;
    VAR num nScanSpeed:=10;
    VAR bool bResult;
    VAR num nNoScans:=5;
    CONST num MaxRetry:=3;
    VAR num nCounter;
    VAR num nAnswer;
    VAR bool bDoSetup;
    VAR bool bReSetup;
    VAR num nLimit;

    IF RunMode()=RUN_INSTR_FWD THEN
      MoveL ToPoint,Speed,fine,Tool\WObj:=obWObj;
      RETURN;
    ENDIF
    TrLogDelete;
    smTmp:=smZero;
    trTmp:=trZero;
    trTmp.joint_no:=Track.joint_no;
    trTmp.calib_no:=Track.calib_no;
    IF Present(Setup) OR bOptSetupAll THEN
      !  If setup create a frame for measurement, based on
      !  camera calibration point
      Reference:=GetSensorFrame(ToPoint,Tool,Track);
      nNoScans:=15;
      bDoSetup:=TRUE;
      !  Run setup twice, this will fix prolems with search direction
      bReSetup:=TRUE;
      !  Set limit larger during setup
      nLimit:=Abs(Track.max_corr*3);
    ELSE
      nLimit:=Abs(Track.max_corr);
    ENDIF
    wdTmp.weld_speed:=Abs(ScanDst)/(nNoScans*1.5*0.1);
    pEnd:=ToPoint;
    !   Save present AW block settings
    AwBlockSetting:=awinhibit;
    WHILE (NOT bResult) AND (nCounter<MaxRetry) DO
      peScanDst:=RelFrame(Reference,ScanDst,0,0);
      pEnd.trans:=ToPoint.trans+(peScanDst.trans-Reference.trans);
      awinhibit:=[TRUE,TRUE,FALSE];
      ArcL\On,ToPoint,Speed,smTmp,wdTmp,wvTmp,fine,Tool\Wobj:=obWObj\Track:=trTmp;
      ArcL\Off,pEnd,Speed,smTmp,wdTmp,wvTmp,fine,Tool\Wobj:=obWObj\Track:=trTmp;
      awinhibit:=AwBlockSetting;
      bResult:=GetTrackResults(bDoSetup,1,nNoScans,nLimit,RetOffs,psFoundPos,nGap,Reference\WObj:=obWObj);
      nCounter:=nCounter+1;
      IF NOT bResult THEN
        !  If retry, slow down to get more scans
        ErrWrite\W,"Retry No: "+ValToStr(nCounter),"";
        wdTmp.weld_speed:=wdTmp.weld_speed*0.75;
      ELSEIF bReSetup THEN
        bResult:=FALSE;
        bReSetup:=FALSE;
        TPWrite "Setup step 1, NoOfScans: "+ValToStr(nValidData);
      ELSEIF bDoSetup THEN
        TPWrite "Setup step 2, NoOfScans: "+ValToStr(nValidData);
      ENDIF
    ENDWHILE
    IF Present(RetPoint) THEN
      RetPoint:=ToPoint;
      RetPoint.trans:=psFoundPos;
    ENDIF
    IF Present(Gap) Gap:=nGap;
    IF (NOT bResult) AND bDoSetup THEN
      TPErase;
      TPWrite "Warning! Setup error";
      TPWrite "Seam could not be found";
      TPWrite "NoOfScans: "+ValToStr(nValidData);
      TPWrite " ";
      TPReadFK nAnswer,"Make a selection","","","","","PROCEED";
    ELSEIF (NOT bResult) THEN
      ErrWrite "Seam not found","Auto proceed, with 0 offset";
    ENDIF
  ERROR
    ArcKill;
    RAISE;
  ENDPROC

  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
