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

MODULE SENCALIB(SYSMODULE, NOSTEPIN)

  !*****************************************************************
  ! Laser Sensor Calibration
  ! Copyright (c) ABB Robotics Products AB 1992  All rights reserved
  !*****************************************************************

  LOCAL CONST num standing:=1;
  LOCAL CONST num mea_unit:=10;
  LOCAL VAR bool invalid:=FALSE;
  LOCAL PERS tooldata ctool:=[TRUE,[[0,0,0],[1,0,0,0]],[5,[0,0,0],[1,0,0,0],0,0,0]];
  LOCAL VAR pos vectab;
  LOCAL CONST errnum posdist_err:=1;
  LOCAL CONST errnum aclockw_err:=2;
  LOCAL CONST errnum clockw_err:=3;
  LOCAL CONST errnum calib_stop:=4;
  LOCAL CONST errnum paralx_err:=5;
  LOCAL CONST errnum paraly_err:=6;
  LOCAL CONST errnum mea_err:=7;
  LOCAL CONST num left_lap:=1;
  LOCAL CONST num right_lap:=2;
  LOCAL VAR num mcount:=0;
  !* count measurements.        
  LOCAL VAR num se_cal{12}:=[0,0,0,0,0,0,0,0,0,0,0,0];
  LOCAL VAR num sin_xrot;
  LOCAL VAR num cos_xrot;
  !* Sin and cos valuse of   *)
  LOCAL VAR num sin_yrot;
  LOCAL VAR num cos_yrot;
  !* rotations about the x,y *)
  LOCAL VAR num sin_zrot;
  LOCAL VAR num cos_zrot;
  !* and z axes.             *)  
  LOCAL VAR iodev outfile;
  PERS pose scal_pose{5}:=[[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];
  PERS pos  scal_pos{5}:=[[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0]];

  LOCAL VAR robtarget pos_a;
  LOCAL VAR robtarget pos_b;
  LOCAL VAR robtarget pos_c;
  LOCAL VAR robtarget pos_d;
  LOCAL VAR robtarget pos_e;
  LOCAL VAR num mounting;
  LOCAL VAR bool simulate := FALSE;
  
  PROC calibrate(
    PERS tooldata in_tool,
    num calibno,
    robtarget pos_a_local,
    robtarget pos_b_local,
    robtarget pos_c_local,
    robtarget pos_d_local,
    num mounting_local
    \bool sim_sensor)

    pos_a := pos_a_local;
    pos_b := pos_b_local;
    pos_c := pos_c_local;
    pos_d := pos_d_local;
    mounting := mounting_local;
    
    IF Present (sim_sensor) THEN
      simulate := sim_sensor;
    ELSE  
      simulate := FALSE;
    ENDIF
      
    TPErase;
    Open ""\File:="calib.log",outfile;
    ctool:=in_tool;
    ! Check the start points pos_a to pos_d.
    check_pos;
    ! Measure and calculate calibration frame.
    calib calibno;
    Close outfile;
  ERROR
    ! STOP ROBOT MOVE
    !    StopMove;
    ! STOP SENSOR
    !    Wsensor(stop);
    Write outfile,"Program: CALIBRATE";
    Write outfile," ";
    TEST ERRNO
    CASE 1:
      Write outfile,"ERROR: The distances between pos 1 - 3 are to short";
      TPWrite "ERROR: The distances between pos 1 - 3 are to short";
    CASE 2:
      Write outfile,"ERROR: The positions 1 - 3 are given anti clockwise";
      TPWrite "ERROR: The positions 1 - 3 are given anti clockwise";
    CASE 3:
      Write outfile,"ERROR: The positions 1 - 3 are given clockwise";
      TPWrite "ERROR: The positions 1 - 3 are given clockwise";
    CASE 5:
      Write outfile,"ERROR: The vector point2-point3 is more than 30 deg from";
      Write outfile,"       the x-axis in the robot base coordinate system.";
      TPWrite "ERROR: The vector point2-point3 is more than 30 deg from";
      TPWrite "       the x-axis in the robot base coordinate system.";
    CASE 6:
      Write outfile,"ERROR: The vector point1-point2 is more than 30 deg from";
      Write outfile,"       the y-axis in the robot base coordinate system.";
      TPWrite "ERROR: The vector point1-point2 is more than 30 deg from";
      TPWrite "       the y-axis in the robot base coordinate system.";
    CASE 7:
      Write outfile,"ERROR: The sensor can not measure the edge";
      TPWrite "ERROR: The sensor can not measure the edge";
    DEFAULT:
      ! STOP PROGRAM EXECUTION
      Stop;
    ENDTEST
  ENDPROC

  PROC stop_calib()
    Stop;
  ENDPROC

  PROC check_pos()
    clockwise;
    posdist;
    xaxis;
    yaxis;
    testaxis4;
  ERROR
    RAISE;
  ENDPROC

  !******************************************************)
  PROC clockwise()
    !*  
    !*  ABSTRACT:
    !*  IF mounting=standing THEN
    !*    The function tests if x,y,z are given clockwise in
    !*    basecoordinat system.
    !*    IF not error:=clockw.
    !*  ELSE
    !*    The function tests if x,y,z are given anti-clockwise in
    !*    basecoordinat system.
    !*    IF not error:=clockw.
    !*
    !********************************************************)
    VAR pos vect;

    !Write outfile,  "*********** clockwise() ***********";
    cross_calc pos_a,pos_b,pos_c,vect;
    IF mounting=standing THEN
      IF vect.z<0 RAISE aclockw_err;
    ELSE
      IF vect.z>0 RAISE clockw_err;
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  !******************************************************)
  PROC posdist()
    !*  
    !*  ABSTRACT:
    !*      The procedure test if the distances pb-pa and
    !*      pb-pc are greater than 100 mm.
    !*      If not error:=pos_dist.
    !********************************************************)
    CONST num min_dist:=100;
    !* 100 mm *)
    VAR num magn;
    VAR pos tmp;

    !Write outfile,  "*********** posdist() ***********";
    tmp.x:=(pos_a.trans.x-pos_b.trans.x);
    tmp.y:=(pos_a.trans.y-pos_b.trans.y);
    tmp.z:=(pos_a.trans.z-pos_b.trans.z);
    magn:=Sqrt(Pow(tmp.x,2)+Pow(tmp.y,2)+Pow(tmp.z,2));
    IF magn<min_dist THEN
      !* Distance pa -pb *)
      RAISE posdist_err;
    ELSE
      tmp.x:=(pos_c.trans.x-pos_b.trans.x);
      tmp.y:=(pos_c.trans.y-pos_b.trans.y);
      tmp.z:=(pos_c.trans.z-pos_b.trans.z);
      magn:=Sqrt(Pow(tmp.x,2)+Pow(tmp.y,2)+Pow(tmp.z,2));
      IF magn<min_dist RAISE posdist_err;
      !* Distance pc -pb *)
    ENDIF
  ERROR
    RAISE;
  ENDPROC

  !*******************************************************)
  PROC yaxis()
    !*  
    !*  ABSTRACT:
    !*    The function tests if angle between the y-axis of the 
    !*    basecoordinat system and pbpa is less than 30 deg.
    !*    If not error:=paraly
    !*
    !********************************************************)
    VAR num cosang;
    VAR num magn;
    VAR pos tmp;

    !Write outfile,  "*********** yaxis() ***********";
    tmp.x:=(pos_a.trans.x-pos_b.trans.x);
    tmp.y:=(pos_a.trans.y-pos_b.trans.y);
    tmp.z:=(pos_a.trans.z-pos_b.trans.z);
    magn:=Sqrt(tmp.x*tmp.x+tmp.y*tmp.y+tmp.z*tmp.z);
    cosang:=tmp.y/magn;
    IF Abs(cosang)<Cos(180/6) RAISE paraly_err;
  ERROR
    RAISE;
  ENDPROC

  !*******************************************************)
  PROC xaxis()
    !*  
    !*  ABSTRACT:
    !*    The function tests if angle between the x-axis of the 
    !*    basecoordinat system and pcpb is less than 30 deg.
    !*    If not error:=paralx.
    !********************************************************)
    VAR num cosang;
    VAR num magn;
    VAR pos tmp;

    !Write outfile,  "*********** xaxis() ***********";
    tmp.x:=pos_c.trans.x-pos_b.trans.x;
    tmp.y:=-(pos_b.trans.y-pos_c.trans.y);
    tmp.z:=-(pos_b.trans.z-pos_c.trans.z);
    magn:=Sqrt(tmp.x*tmp.x+tmp.y*tmp.y+tmp.z*tmp.z);
    cosang:=tmp.x/magn;
    IF Abs(cosang)<Cos(180/6) RAISE paralx_err;
  ERROR
    RAISE;
  ENDPROC

  !*******************************************************)
  PROC testaxis4()
    !*  
    !*  ABSTRACT:
    !*    The function tests if axis 4 is less than +- 30 deg
    !*    from syncposition.
    !*    If not error:=axis4er
    !********************************************************)
    !  WRITELN('30GRAD',incalib.gear_ptr@.g4*pi/6);               
    !  WRITELN('ABS AXIS4',abs(incalib.pos_ptr@.axis.r4));      
    !  IF  incalib.gear_ptr@.g4*pi/6 < abs(incalib.pos_ptr@.axis.r4) THEN error:=axis4er
    !Write outfile,  "*********** testaxis4() ***********";
    !Write outfile,  " Test axis 4 ok";
  ENDPROC

  !*******************************************************)
  PROC cross_calc(
    robtarget p1,
    robtarget p2,
    robtarget p3,
    VAR pos vect)

    !*
    !*  
    !*  ABSTRACT:
    !*      The procedure calculates the cross-product 
    !*      vect:=vect1 x vect2.
    !*      vect1 = p1-p2.
    !*      vect2 = p3-p2.
    !********************************************************
    VAR pos vect1;
    VAR pos vect2;

    !Write outfile,  "*********** cross_calc() ***********";
    vect1.x:=p1.trans.x-p2.trans.x;
    vect1.y:=p1.trans.y-p2.trans.y;
    vect1.z:=p1.trans.z-p2.trans.z;
    vect2.x:=p3.trans.x-p2.trans.x;
    vect2.y:=p3.trans.y-p2.trans.y;
    vect2.z:=p3.trans.z-p2.trans.z;
    vect.x:=vect1.y*vect2.z-vect2.y*vect1.z;
    vect.y:=vect1.z*vect2.x-vect2.z*vect1.x;
    vect.z:=vect1.x*vect2.y-vect2.x*vect1.y;
  ENDPROC

  !*******************************************************)
  FUNC num avrage(
    num vector{*})

    !*  ABSTRACT:
    !*      The largest and smallest post in vector
    !*      are omitted, and the function returns a meanvalue
    !*      of the other three.
    !*      The elements are first sorted with the largest
    !*      element in vect{1}.
    !*      The meanvalue is then calculated using elements
    !*      2,3 and 4.
    !*
    !********************************************************)
    VAR num index1;
    VAR num index2;
    VAR num max_indx;
    VAR num max;

    !Write outfile,  "*********** avrage() ***********";
    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

  !*******************************************************)
  PROC unit(
    pos in_par,
    VAR pos out_par)

    !*       ABSTRACT:
    !*       The procedure calculate a vector with magnitude
    !*       1 from in_par, and returns the rusult in out_par.
    !*          
    !********************************************************)
    VAR num magn;

    magn:=Sqrt(in_par.x*in_par.x+in_par.y*in_par.y+in_par.z*in_par.z);
    out_par.x:=in_par.x/magn;
    out_par.y:=in_par.y/magn;
    out_par.z:=in_par.z/magn;
  ENDPROC

  !*******************************************************)
  PROC vect_calc(
    VAR pos vect_pba,
    VAR pos vect_pbc,
    VAR pos vect_norm)

    !*  ABSTRACT:
    !*       The procedure calculate the following vectors.
    !*
    !*       vect_pba:  The vector from pb to pa in 
    !*                  handcoordinates.
    !*       vect_pbc:  The vector from pb to pc in 
    !*                  handcoordinates.
    !*       vect_norm: A vector perpendicular to the calibration-
    !*                  plane, with origin in pb.
    !*       
    !*       All the vectors have the magnitude 1.
    !*          
    !********************************************************)
    VAR pose hand_pa;
    VAR pose hand_pb;
    VAR pose hand_pc;
    VAR num magn;
    VAR pos vect;

    !* Calculate the positions of the corners of the    *)
    !* calibration plane in the hand coordinate system. *)
    !Write outfile,  "*********** vect_calc() ***********";
    wobjhand pos_a,hand_pa,ctool;
    wobjhand pos_b,hand_pb,ctool;
    wobjhand pos_c,hand_pc,ctool;
    !Write outfile,  " hand_pa="\pos:=hand_pa.trans;
    !Write outfile,  " hand_pb="\pos:=hand_pb.trans;
    !Write outfile,  " hand_pc="\pos:=hand_pc.trans; 
    !!Break;
    !* Calculate the vectors *)
    vect.x:=hand_pa.trans.x-hand_pb.trans.x;
    vect.y:=hand_pa.trans.y-hand_pb.trans.y;
    vect.z:=hand_pa.trans.z-hand_pb.trans.z;
    unit vect,vect_pba;
    !Write outfile,  " vect_pba.x="\Num:=vect_pba.x;
    !Write outfile,  " vect_pba.y="\Num:=vect_pba.y;
    !Write outfile,  " vect_pba.z="\Num:=vect_pba.z; 
    !  !Break;
    vect.x:=hand_pc.trans.x-hand_pb.trans.x;
    vect.y:=hand_pc.trans.y-hand_pb.trans.y;
    vect.z:=hand_pc.trans.z-hand_pb.trans.z;
    unit vect,vect_pbc;
    !Write outfile,  " vect_pbc.x="\Num:=vect_pbc.x;
    !Write outfile,  " vect_pbc.y="\Num:=vect_pbc.y;
    !Write outfile,  " vect_pbc.z="\Num:=vect_pbc.z; 
    !  !Break;
    !  cross_calc hand_pa,hand_pb,hand_pc,vect;
    !  unit vect,vect_norm;
    !Write outfile,  " vect_norm.x="\Num:=vect_norm.x;
    !Write outfile,  " vect_norm.y="\Num:=vect_norm.y;
    !Write outfile,  " vect_norm.z="\Num:=vect_norm.z; 
    !!Break;
  ENDPROC

  !*******************************************************)
  PROC calib(
    num calibno)

    !*  
    !*  ABSTRACT:
    !*     The procedure calculates the components of the 
    !*     sensorcalibration for an laser sensor.
    !*
    !********************************************************)
    CONST num offset_z:=25;
    !* 25 mm  *)
    VAR num mea_pos1{2};
    !* Corner edge coordinates    *)
    VAR num mea_neg1{2};
    !* {1}=dist {2}=side          *) 
    VAR num mea_pos2{2};
    VAR num mea_neg2{2};
    !* in pos Pd and Pd+offset_z. *)
    VAR num scan_ang;
    VAR num ang_1;
    VAR num ang_4;
    VAR num add_2;
    VAR robtarget pos_off;
    !* pos_corn+offsetz(0,0,1)    *)
    VAR pos hnd1_p3;
    VAR pos hnd2_p3;
    VAR num diffdist;
    VAR num diffside;

    !Write outfile,  "*********** calib() ***********";
    !* TCP position with scan over Pb. *)
    
    !Start the sensor
    IF simulate = FALSE THEN
      WriteVar 6,1;
      WriteVar 7,0;
      WaitTime 5;
    ENDIF
    
    mcount:=0;
    zoff_meas calibno;
    !* Calculate sensor origo  *)
    mcount:=0;
    !* Reset counter for simulation *)
    cal_ang1 ang_1;
    !* Calculate the corner  *)
    !* angle in pos Pb.      *)
    corn_meas mea_pos1,mea_neg1,calibno;
    !Write outfile,  "mea_pos1{1}="\Num:=mea_pos1{1};
    !Write outfile,  "mea_pos1{2}="\Num:=mea_pos1{2};
    !Write outfile,  "mea_neg1{1}="\Num:=mea_neg1{1};
    !Write outfile,  "mea_neg1{2}="\Num:=mea_neg1{2};
    ! !Break;
    diffdist:=mea_pos1{1}-mea_neg1{1};
    diffside:=mea_pos1{2}-mea_neg1{2};
    ang_4:=ATan(Abs(diffdist)/Abs(diffside));
    cal_scanang scan_ang,ang_4,add_2,calibno;
    MoveL pos_d,v100,fine,ctool;
    WaitTime 0.5;
    IF add_2=0 THEN
      inter_pt mea_pos1,mea_neg1,hnd1_p3,scan_ang,ang_1,ang_4,add_2;
    ELSE
      inter_pt mea_neg1,mea_pos1,hnd1_p3,scan_ang,ang_1,ang_4,add_2;
    ENDIF
    pos_off:=pos_d;
    IF mounting=standing THEN
      pos_off.trans.z:=pos_off.trans.z+(offset_z);
      !* standing robot *)
    ELSE
      pos_off.trans.z:=pos_off.trans.z-(offset_z);
      !* suspended robot *)
    ENDIF
    MoveL pos_off,v100,fine,ctool;
    WaitTime 0.5;
    corn_meas mea_pos2,mea_neg2,calibno;
    IF add_2=0 THEN
      !Write outfile,  "add_2=0";   
      inter_pt mea_pos2,mea_neg2,hnd2_p3,scan_ang,ang_1,ang_4,add_2;
    ELSE
      !Write outfile,  "add_2=180";   
      inter_pt mea_neg2,mea_pos2,hnd2_p3,scan_ang,ang_1,ang_4,add_2;
    ENDIF
    calc_mat hnd1_p3,hnd2_p3,scan_ang,add_2;
    calc_trn hnd1_p3,hnd2_p3,mea_pos1,mea_neg1;
    !writ_res;
    calpose calibno;
    Write outfile,"Calibration result";
    Write outfile,"";
    Write outfile,"scal_pose{calibno}.trans  "\Pos:=scal_pose{calibno}.trans;
    Write outfile,"scal_pose{calibno}.rot  "\Orient:=scal_pose{calibno}.rot;
    Write outfile,"z_offset  "\Num:=scal_pos{calibno}.z;
    Write outfile,"ctool.trans  "\Pos:=ctool.tframe.trans;
    Write outfile,"ctool.rot  "\Orient:=ctool.tframe.rot;
    Write outfile,"EulerZYX(X,scal_pose{calibno}.rot  )"\Num:=EulerZYX(\X,scal_pose{calibno}.rot);
    Write outfile,"EulerZYX(Y,scal_pose{calibno}.rot  )"\Num:=EulerZYX(\Y,scal_pose{calibno}.rot);
    Write outfile,"EulerZYX(Z,scal_pose{calibno}.rot  )"\Num:=EulerZYX(\Z,scal_pose{calibno}.rot);
    Write outfile,"";
    MoveL pos_d,v100,fine,ctool;
    WaitTime 0.5;
    
    !Suspend the sensor
    IF simulate = FALSE THEN
      WriteVar 6,0;
    ENDIF
    
  ERROR
    TEST ERRNO
    CASE ERR_DIVZERO:
      diffdist:=0;
      diffside:=1;
      RETRY;
    CASE SEN_NO_MEAS:
      Write outfile,"SEN_NO_MEAS error reported";
      TPWrite "SEN_NO_MEAS error reported";
    CASE SEN_NOREADY:
      Write outfile,"SEN_NOREADY error reported";
      TPWrite "SEN_NOREADY error reported";
    CASE SEN_GENERRO:
      Write outfile,"SEN_GENERRO error reported";
      TPWrite "SEN_GENERRO error reported";
    CASE SEN_BUSY:
      Write outfile,"SEN_BUSY error reported";
      TPWrite "SEN_BUSY error reported";
    CASE SEN_UNKNOWN:
      Write outfile,"SEN_UNKNOWN error reported";
      TPWrite "SEN_UNKNOWN error reported";
    CASE SEN_ILLEGAL:
      Write outfile,"SEN_ILLEGAL error reported";
      TPWrite "SEN_ILLEGAL error reported";
    CASE SEN_EXALARM:
      Write outfile,"SEN_EXALARM error reported";
      TPWrite "SEN_EXALARM error reported";
    CASE SEN_CAALARM:
      Write outfile,"SEN_CAALARM error reported";
      TPWrite "SEN_CAALARM error reported";
    CASE SEN_TEMP:
      Write outfile,"SEN_TEMP error reported";
      TPWrite "SEN_TEMP error reported";
    CASE SEN_VALUE:
      Write outfile,"SEN_VALUE error reported";
      TPWrite "SEN_VALUE error reported";
    CASE SEN_CAMCHECK:
      Write outfile,"SEN_CAMCHECK error reported";
      TPWrite "SEN_CAMCHECK error reported";
    CASE SEN_TIMEOUT:
      Write outfile,"SEN_TIMEOUT error reported";
      TPWrite "SEN_TIMEOUT error reported";
    DEFAULT:
      RAISE;
    ENDTEST
  ENDPROC

  !*******************************************************)
  PROC inter_pt(
    num mp1{*},
    num mp2{*},
    VAR pos hnd_p3,
    num scan_ang,
    num ang_1,
    num ang_4,
    num add_2)

    !*  ABSTRACT:
    !*      The procedure calculates the intersection of the 
    !*      calibration plane and sensor scan at scanangle 0.
    !*      The projection of the scan on the plane is defined 
    !*      by inputs mp1 and mp2 which are the sensor coordinates
    !*      of the intersections of the scan and the edges of 
    !*      the plane.
    !*      Seamdata.ang_a1 the angle of the corner of 
    !*      Pb on the calibration plane, and scan_ang,
    !*      the angle between the x-coordinat of the sensor's coordinat 
    !*      sytem scan and the plane edge in Pg, are used in the calculations.
    !*      
    !********************************************************)
    VAR pos hvect1;
    !* Normalized vector pb pa in handcoordinates *)
    VAR pos hvect2;
    !* Normalized vector pb pc in handcoordinates *)
    VAR pos hvect3;
    !* Normalized vector p1 p2 in handcoordinates *)
    VAR pose tmp_pb;
    !* The corner of the plane in handcoordinates *)
    VAR pos hnd_pb;
    !* The corner of the plane in handcoordinates *)
    VAR pos hnd_p1;
    !* The instrsection of the scan and the plane *)
    VAR pos hnd_p2;
    !* edge corresponding to mp1 and mp2.         *)
    VAR num ang_2;
    !* Angle between the scan and vector bc       *)   
    VAR num ang_3;
    !* Angle between the scan and vector ba       *)   
    VAR num l1;
    !* Length of the scan projection on the plane *)
    VAR num l2;
    !* L2 and l3 are the "kateterar" in the       *)
    VAR num l3;

    !* triangle  where l1 is the "hypertenusa".   *)
    !* L2 and l3 are parallell with the vectors   *)
    !* ba nad bc respectively.                    *)   
    !Write outfile,  "*********** interpt() ***********";
    ang_2:=Abs(scan_ang);
    ang_3:=Abs(180-Abs(ang_1)-Abs(ang_2));
    l1:=(Abs(mp1{2}-mp2{2}));
    l2:=Abs(Sin(ang_2)*l1/Sin(ang_1));
    l3:=Abs(Sin(ang_3)*l1/Sin(ang_1));
    !Write outfile,  "mp1{2}= "\Num:=mp1{2};
    !Write outfile,  "mp2{2}= "\Num:=mp2{2};
    !Write outfile,  "ang_2= "\Num:=ang_2;
    !Write outfile,  "ang_3= "\Num:=ang_3;
    !Write outfile,  "l1= "\Num:=l1;
    !Write outfile,  "l2= "\Num:=l2;
    !Write outfile,  "l3= "\Num:=l3;
    vect_calc hvect1,hvect2,hvect3;
    !* Get hvect1 and hvect2.     *)
    !* The crossproduct returned  *)
    !* in hvect3 is not used.     *)
    wobjhand pos_b,tmp_pb,ctool;
    !* Get Pb in hand coordinates *)
    hnd_pb.x:=tmp_pb.trans.x;
    hnd_pb.y:=tmp_pb.trans.y;
    hnd_pb.z:=tmp_pb.trans.z;
    !Write outfile,  "hnd_pb.x="\Num:= hnd_pb.x;
    !Write outfile,  "hnd_pb.y="\Num:= hnd_pb.y;
    !Write outfile,  "hnd_pb.z="\Num:= hnd_pb.z;
    !  !Break;
    hnd_p1.x:=hvect1.x*l2+hnd_pb.x;
    hnd_p1.y:=hvect1.y*l2+hnd_pb.y;
    hnd_p1.z:=hvect1.z*l2+hnd_pb.z;
    !Write outfile,  "hnd_p1.x="\Num:= hnd_p1.x;
    !Write outfile,  "hnd_p1.y="\Num:= hnd_p1.y;
    !Write outfile,  "hnd_p1.z="\Num:= hnd_p1.z;
    !  !Break;
    hnd_p2.x:=hvect2.x*l3+hnd_pb.x;
    hnd_p2.y:=hvect2.y*l3+hnd_pb.y;
    hnd_p2.z:=hvect2.z*l3+hnd_pb.z;
    !Write outfile,  "hnd_p2.x="\Num:= hnd_p2.x;
    !Write outfile,  "hnd_p2.y="\Num:= hnd_p2.y;
    !Write outfile,  "hnd_p2.z="\Num:= hnd_p2.z;
    !  !Break;
    hvect3.x:=hnd_p2.x-hnd_p1.x;
    hvect3.y:=hnd_p2.y-hnd_p1.y;
    hvect3.z:=hnd_p2.z-hnd_p1.z;
    !Write outfile,  "hvect3.x="\Num:= hvect3.x;
    !Write outfile,  "hvect3.y="\Num:= hvect3.y;
    !Write outfile,  "hvect3.z="\Num:= hvect3.z;
    !  !Break;
    unit hvect3,hvect3;
    !* Normalize hvect3. *)
    IF add_2=0 THEN
      hnd_p3.x:=hnd_p1.x-hvect3.x*(mp1{2}*Cos(ang_4));
      hnd_p3.y:=hnd_p1.y-hvect3.y*(mp1{2}*Cos(ang_4));
      hnd_p3.z:=hnd_p1.z-hvect3.z*(mp1{2}*Cos(ang_4));
    ELSE
      hnd_p3.x:=hnd_p1.x+hvect3.x*(mp1{2}*Cos(ang_4));
      hnd_p3.y:=hnd_p1.y+hvect3.y*(mp1{2}*Cos(ang_4));
      hnd_p3.z:=hnd_p1.z+hvect3.z*(mp1{2}*Cos(ang_4));
    ENDIF
    !Write outfile,  "hvect3.y="\Num:= hvect3.y;
    !Write outfile,  "mp1{2}="\Num:=mp1{2};
    !Write outfile,  "ang_4="\Num:=ang_4;  
    !Write outfile,  "hnd_p3.x="\Num:= hnd_p3.x;
    !Write outfile,  "hnd_p3.y="\Num:= hnd_p3.y;
    !Write outfile,  "hnd_p3.z="\Num:= hnd_p3.z;
    !  !Break;
  ENDPROC

  !*******************************************************)
  PROC edgemess(
    pos vect,
    num instepd,
    VAR num meas{*},
    num calibno)

    !*  
    !*  ABSTRACT:
    !*      The procedure returns the side and hight coordinats of 
    !*      an edge of the calibration plane.
    !*      The function meassures the coordinate in five 
    !*      positions, 
    !*      original position + 0.1mm(-2,-1,0,+1,+2) * vect.
    !*      The largest and smallest coordinate meassured 
    !*      are omitted, and the function returns a meanvalue
    !*      of the other three.
    !*        
    !********************************************************)
    CONST num m_offset:=0.1;
    !* 0.1 mm *)
    VAR num index;
    VAR num side_arr{5};
    VAR num dist_arr{5};
    VAR robtarget act_p;
    VAR robtarget send_p;
    VAR pos mea_data;
    !* measure data  OBS CHANGE TO STRUCT
    VAR num status;

    !Write outfile,  "*********** edgemess() ***********";
    act_p:=CRobT();
    send_p:=act_p;
    FOR index FROM 0 TO 4 DO
      send_p.trans.x:=(act_p.trans.x+(index-2)*(m_offset)*vect.x);
      send_p.trans.y:=(act_p.trans.y+(index-2)*(m_offset)*vect.y);
      send_p.trans.z:=(act_p.trans.z+(index-2)*(m_offset)*vect.z);
      MoveL send_p,v100,fine,ctool;
      WaitTime 0.5;
      
      !Run Measuresim if no sensor is connected.
      IF simulate = TRUE THEN
        mea_data:=Measuresim(send_p,act_p,calibno);
      ELSE
        mea_data:=Measure(instepd,calibno);
      ENDIF
      
      side_arr{index+1}:=mea_data.y;
      dist_arr{index+1}:=mea_data.z;
      !  !Break; 
    ENDFOR
    meas{1}:=avrage(dist_arr);
    meas{2}:=avrage(side_arr);
    !  Write outfile,  "meas{1}="\Num:=meas{1};
    !  Write outfile,  "meas{2}="\Num:=meas{2};
    !!Break; 
    MoveL act_p,v100,fine,ctool;
    WaitTime 0.5;
  ERROR
    RAISE;
  ENDPROC

  !* END edgemess *)
  !*******************************************************)
  PROC scan_tst(
    VAR bool posedge,
    VAR bool negedge,
    num calibno)

    !*  
    !*  ABSTRACT:
    !*      The procedure tests the direction of step(s) within 
    !*      the sensor scan.
    !*      Edgemess is called two times for each step direction.
    !*      If the result in side and distance are repeated within
    !*      max_error the parameter corresponding to tested direction
    !*      is returned true.
    !*
    !********************************************************)
    posedge:=scan_sub(right_lap,calibno);
    !* Look for positive lap *) 
    negedge:=scan_sub(left_lap,calibno);
    !* Look for negative lap *)  
  ENDPROC

  !*******************************************************)
  FUNC bool scan_sub(
    num direction,
    num calibno)

    !*  Subroutione in scan_tst.
    !*  ABSTRACT:
    !*      The procedure tests the direction of step(s) within 
    !*      the sensor scan.
    !*      Edgemess is called two times.
    !*      If the result in side and distance are repeated within
    !*      max_error 'TRUE' is returned.
    !*
    !********************************************************)
    CONST num max_error:=1;
    !* 1 mm *)
    VAR num meas1{2};
    VAR num meas2{2};
    VAR pos vector;

    vector.x:=1;
    vector.y:=0;
    vector.z:=0;
    !Write outfile,  "*********** scan_tst() ***********";
    ! !Break; 
    edgemess vector,direction,meas1,calibno;
    edgemess vector,direction,meas2,calibno;
    !!Break;
    
    ! For simulation. Test if measurement is invalid then false.
    IF simulate = TRUE THEN
      IF invalid RETURN FALSE; 
    ENDIF
     
    IF (Abs(meas1{2}-meas2{2})<max_error) AND (Abs(meas1{1}-meas2{1})<max_error) THEN
      RETURN TRUE;
    ELSE
      Write outfile,"scan_tst() FAILED ";
      RETURN FALSE;
    ENDIF
  ERROR
    TEST ERRNO
    CASE SEN_NO_MEAS:
      RETURN FALSE;
    DEFAULT:
      RAISE;
    ENDTEST
  ENDFUNC

  !*******************************************************)
  PROC calc_trn(
    pos hnd1_p3,
    pos hnd2_p3,
    num mea_pos1{*},
    num mea_neg1{*})

    !*  
    !*  ABSTRACT:
    !*      The procedure calculates the translation from the origin of
    !*      the sensor's coordinat system to the origin of the
    !*      hand system.
    !********************************************************)
    VAR pos vect;
    !* The z' vector.         *)
    VAR pos s_org;
    !* Origin of the sensor.  *)
    VAR num dist1;

    !* Calculated distance to *)
    !* hnd_p3.                *)
    !Write outfile,  "*********** calc_trn() ***********";
    !* Calculate the z' vector *)
    vect.x:=hnd1_p3.x-hnd2_p3.x;
    vect.y:=hnd1_p3.y-hnd2_p3.y;
    vect.z:=hnd1_p3.z-hnd2_p3.z;
    unit vect,vect;
    !* Normalize the vector *)
    dist1:=((mea_pos1{1}+mea_neg1{1})/2);
    !Write outfile,  "hnd1_p3.x="\Num:=hnd1_p3.x;
    !Write outfile,  "hnd2_p3.x="\Num:=hnd2_p3.x;
    !Write outfile,  "vect.x="\Num:=vect.x;
    !Write outfile,  "mea_pos1{1}="\Num:=mea_pos1{1};
    !Write outfile,  "mea_neg1{1}="\Num:=mea_neg1{1};
    !Write outfile,  "dist1="\Num:=dist1;
    !  !Break;
    !*  se_cal.trtx:=  (hnd1_p3.x - vect.x * dist1);
    !*  se_cal.trty:=  (hnd1_p3.y - vect.y * dist1);      
    !*  se_cal.trtz:=  (hnd1_p3.z - vect.z * dist1);  
    se_cal{10}:=(hnd1_p3.x-vect.x*dist1);
    se_cal{11}:=(hnd1_p3.y-vect.y*dist1);
    se_cal{12}:=(hnd1_p3.z-vect.z*dist1);
    !Write outfile,  "********************************";
    !Write outfile,  "dist1"\Num:=dist1;
    !Write outfile,  "hnd1_p3.x"\Num:=hnd1_p3.x;
    !Write outfile,  "hnd1_p3.y"\Num:=hnd1_p3.y;
    !Write outfile,  "hnd1_p3.z"\Num:=hnd1_p3.z;
    !Write outfile,  "vect.x"\Num:=vect.x;
    !Write outfile,  "vect.y"\Num:=vect.y;
    !Write outfile,  "vect.z"\Num:=vect.z;
    !Write outfile,  "trtx"\Num:=se_cal{10};
    !Write outfile,  "trty"\Num:=se_cal{11};
    !Write outfile,  "trtz"\Num:=se_cal{12};
    !Write outfile,  "********************************";
  ENDPROC

  !*******************************************************)
  PROC calc_mat(
    pos hnd1_p3,
    pos hnd2_p3,
    num scan_ang,
    num add_2)

    !*  
    !*  ABSTRACT:
    !*      The procedure calculates the rotation matrix
    !*      which transforms sensor coordinates into the 
    !*      hand system.
    !*      The z-axis of the sensor system is first imaged on
    !*      the z axis of the hand system.
    !*      This transformation is done by rotations around the
    !*      hand x and y -axis.
    !*      The angle required to image the sensor y-axis on the hand 
    !*      y-axis, by rotation around the hand z-axis, is calculated
    !*      by transforming the calibrationplane vector pbc_hand with the 
    !*      partial xy transformation.
    !*      Rotz, rotation around the hand z-axis, is calculated as 
    !*      scan_ang - "the angle between the transformated pbc_hand and
    !*      the hand y-axis".
    !*
    !********************************************************)
    VAR num vector{3};
    VAR num matrix{3,3};
    VAR pos tmp_v1;
    VAR pos tmp_v2;
    !* Temporary variables.    *)
    VAR num s;
    VAR num t;
    VAR num temp;
    VAR num rotz;
    !* Rotation angle about z. *)
    VAR num trans{3,3};
    !* Resulting transformation *)
    VAR num xrot{3,3};
    VAR num yrot{3,3};
    VAR num zrot{3,3};
    !* Rotation matixes         *)
    VAR pos sensor_z;
    !* z-axis of the sensor     *)
    VAR pos pbc_hand;

    !* Calibration plane vector *)
    !* pbc in hand coordinates  *)
    !Write outfile,  "*********** calc_mat() ***********";
    sensor_z.x:=hnd1_p3.x-hnd2_p3.x;
    sensor_z.y:=hnd1_p3.y-hnd2_p3.y;
    sensor_z.z:=hnd1_p3.z-hnd2_p3.z;
    !Write outfile,  " sensor_z.x"\Num:=sensor_z.x;
    !Write outfile,  " sensor_z.y"\Num:=sensor_z.y;
    !Write outfile,  " sensor_z.z"\Num:=sensor_z.z; 
    !  !Break;
    unit sensor_z,sensor_z;
    !Write outfile,  " sensor_z.x"\Num:=sensor_z.x;
    !Write outfile,  " sensor_z.y"\Num:=sensor_z.y;
    !Write outfile,  " sensor_z.z"\Num:=sensor_z.z; 
    !  !Break;
    
    !IF Abs(sensor_z.x)< Abs(sensor_z.y) THEN
      !Write outfile,  "x-komponent var minst";
      temp:=Sqrt(Pow(sensor_z.y,2)+Pow(sensor_z.z,2));
      sin_xrot:=-sensor_z.y/temp;
      cos_xrot:=sensor_z.z/temp;
      sin_yrot:=sensor_z.x;
      cos_yrot:=temp;
      fill_xymat yrot,xrot;
      mat_mul xrot,yrot,trans;
    !ELSE
      !Write outfile,  "y-komponent var minst";
      !temp:=Sqrt(Pow(sensor_z.x,2)+Pow(sensor_z.z,2));
      !sin_xrot:=-sensor_z.y;
      !cos_xrot:=temp;
      !sin_yrot:=sensor_z.x/temp;
      !cos_yrot:=sensor_z.z/temp;
      !fill_xymat yrot,xrot;
      !mat_mul yrot,xrot,trans;
    !ENDIF
    !Write outfile,  " temp="\Num:=temp;
    !Write outfile,  " sin_xrot="\Num:=sin_xrot;
    !Write outfile,  " cos_xrot="\Num:=cos_xrot;
    !Write outfile,  " sin_yrot="\Num:=sin_yrot;
    !Write outfile,  " cos_yrot="\Num:=cos_yrot;
    !  !Break;  
    vect_calc tmp_v1,pbc_hand,tmp_v2;
    !Write outfile,  " trans{1,1}="\Num:=trans{1,1};
    !Write outfile,  " trans{1,2}="\Num:=trans{1,2};
    !Write outfile,  " trans{1,3}="\Num:=trans{1,3};
    !Write outfile,  " trans{2,1}="\Num:=trans{2,1};
    !Write outfile,  " trans{2,2}="\Num:=trans{2,2};
    !Write outfile,  " trans{2,3}="\Num:=trans{2,3};
    !Write outfile,  " trans{3,1}="\Num:=trans{3,1};
    !Write outfile,  " trans{3,2}="\Num:=trans{3,2};
    !Write outfile,  " trans{3,3}="\Num:=trans{3,3};
    !  !Break;  
    !Write outfile,  "pbc_hand"\Num:=pbc_hand.x;
    !Write outfile,  "pbc_hand"\Num:=pbc_hand.y;
    !Write outfile,  "pbc_hand"\Num:=pbc_hand.z; 
    vect_mul pbc_hand,trans,pbc_hand;
    !Write outfile,  "pbc_hand"\Num:=pbc_hand.x;
    !Write outfile,  "pbc_hand"\Num:=pbc_hand.y;
    !Write outfile,  "pbc_hand"\Num:=pbc_hand.z;  
    !  !Break;
    unit pbc_hand,pbc_hand;
    !Write outfile,  "pbc_hand"\Num:=pbc_hand.x;
    !Write outfile,  "pbc_hand"\Num:=pbc_hand.y;
    !Write outfile,  "pbc_hand"\Num:=pbc_hand.z; 
    mat_zer zrot;
    rotz:=ASin(pbc_hand.x);
    !Write outfile,  "rotz: " \Num:=rotz;
    IF pbc_hand.y<0 rotz:=180-rotz;
    !Write outfile,  "rotz: " \Num:=rotz;
    sin_zrot:=Sin(scan_ang+add_2-rotz);
    cos_zrot:=Cos(scan_ang+add_2-rotz);
    zrot{3,3}:=1;
    zrot{1,1}:=cos_zrot;
    zrot{1,2}:=-sin_zrot;
    zrot{2,1}:=sin_zrot;
    zrot{2,2}:=cos_zrot;
    !Write outfile,  " ";
    !Write outfile,  "--- zrot ---- ";
    !Write outfile,  " ";
    !FOR s FROM 1 TO 3 DO  
    !Write outfile,  ""\Num:=zrot{s,1} \Nonewline;
    !Write outfile,  "  "\Num:=zrot{s,2} \Nonewline;
    !Write outfile,  "  "\Num:=zrot{s,3};
    !ENDFOR
    mat_mul trans,zrot,trans;
    fill_secal trans;
  ENDPROC

  !************************************************************************)
  PROC mat_mul(
    num mat1{*,*},
    num mat2{*,*},
    VAR num res_mat{*,*})

    !*  Subroutione to calc_mat
    !*  ABSTRACT:
    !*      The function multiplies mat1 by mat2.
    !*
    !********************************************************)
    VAR num r;
    VAR num s;
    VAR num t;

    FOR r FROM 1 TO 3 DO
      FOR s FROM 1 TO 3 DO
        res_mat{r,s}:=0;
        FOR t FROM 1 TO 3 DO
          res_mat{r,s}:=res_mat{r,s}+mat1{r,t}*mat2{t,s};
        ENDFOR
      ENDFOR
    ENDFOR
  ENDPROC

  !********************************************************************)
  PROC vect_mul(
    pos vekt,
    num mat{*,*},
    VAR pos real_res)

    !*  Subroutione to calc_mat
    !*  ABSTRACT:
    !*      The function multiplies a vector by a matrix.
    !*
    !*********************************************************************)
    VAR num arr_vect{3};
    VAR num res_vect{3};
    VAR num s;
    VAR num t;

    arr_vect{1}:=vekt.x;
    arr_vect{2}:=vekt.y;
    arr_vect{3}:=vekt.z;
    !Write outfile,  "*********** vect_mult() ***********";
    FOR s FROM 1 TO 3 DO
      res_vect{s}:=0;
      FOR t FROM 1 TO 3 DO
        res_vect{s}:=res_vect{s}+mat{t,s}*arr_vect{t};
      ENDFOR
    ENDFOR
    !Write outfile,  "res_vect{1},res_vect{2},res_vect{3}";
    real_res.x:=res_vect{1};
    real_res.y:=res_vect{2};
    real_res.z:=res_vect{3};
  ENDPROC

  !********************************************************************)
  PROC mat_zer(
    VAR num mat{*,*})

    !*  Subroutione to calc_mat
    !*  ABSTRACT:
    !*      The procedure zero sets all elements in a matrix
    !*
    !********************************************************************)
    VAR num s;
    VAR num t;

    FOR s FROM 1 TO 3 DO
      FOR t FROM 1 TO 3 DO
        mat{t,s}:=0;
      ENDFOR
    ENDFOR
  ENDPROC

  !********************************************************************)
  PROC fill_xymat(
    VAR num yrot{*,*},
    VAR num xrot{*,*})

    !*  Subroutione to calc_mat
    !*  ABSTRACT:
    !*      The procedure builds rotation matrixes for rotation
    !*      about the x and y -axes.
    !*
    !********************************************************************)
    mat_zer xrot;
    mat_zer yrot;
    !Write outfile,  "*********** fill_xymat() ***********";
    xrot{1,1}:=1;
    xrot{2,2}:=cos_xrot;
    xrot{2,3}:=-sin_xrot;
    xrot{3,2}:=sin_xrot;
    xrot{3,3}:=cos_xrot;
    !Write outfile,  "xrot{1,1}"\Num:=xrot{1,1};
    !Write outfile,  "xrot{2,2}"\Num:=xrot{2,2};
    !Write outfile,  "xrot{2,3}"\Num:=xrot{2,3};
    !Write outfile,  "xrot{3,2}"\Num:=xrot{3,2};
    !Write outfile,  "xrot{3,3}"\Num:=xrot{3,3};
    !  !Break;
    yrot{2,2}:=1;
    yrot{1,1}:=cos_yrot;
    yrot{1,3}:=sin_yrot;
    yrot{3,1}:=-sin_yrot;
    yrot{3,3}:=cos_yrot;
    !Write outfile,  "yrot{1,1}"\Num:=yrot{1,1};
    !Write outfile,  "yrot{1,3}"\Num:=yrot{1,3};
    !Write outfile,  "yrot{2,2}"\Num:=yrot{2,2};
    !Write outfile,  "yrot{3,1}"\Num:=yrot{3,1};
    !Write outfile,  "yrot{3,3}"\Num:=yrot{3,3};
    !  !Break;
  ENDPROC

  !********************************************************************)
  PROC fill_secal(
    num trans{*,*})

    !*  Subroutione to calc_mat
    !*  ABSTRACT:
    !*      The procedure builds rotation matrixes for rotation
    !*      about the x and y -axes.
    !*
    !********************************************************************)
    !    se_cal.dkxx:=trans{1,1};
    !    se_cal.dkxy:=trans{1,2};
    !    se_cal.dkxz:=trans{1,3};
    !    se_cal.dkyx:=trans{2,1};
    !    se_cal.dkyy:=trans{2,2};
    !    se_cal.dkyz:=trans{2,3};
    !    se_cal.dkzx:=trans{3,1};
    !    se_cal.dkzy:=trans{3,2};
    !    se_cal.dkzz:=trans{3,3};
    se_cal{1}:=trans{1,1};
    se_cal{2}:=trans{1,2};
    se_cal{3}:=trans{1,3};
    se_cal{4}:=trans{2,1};
    se_cal{5}:=trans{2,2};
    se_cal{6}:=trans{2,3};
    se_cal{7}:=trans{3,1};
    se_cal{8}:=trans{3,2};
    se_cal{9}:=trans{3,3};
  ENDPROC

  !*******************************************************)
  PROC cal_ang1(
    VAR num ang_1)

    !*  
    !*  ABSTRACT:
    !*      The procedure calculates the angle of the corner 
    !*      of the calibration plane in pos Pb.
    !*
    !********************************************************)
    VAR pos vectba;
    VAR pos vectbc;
    VAR pos vect;

    !* Calculate vector Pb to Pa *)
    !Write outfile,  "*********** cal_ang1() ***********";
    vectba.x:=pos_a.trans.x-pos_b.trans.x;
    vectba.y:=pos_a.trans.y-pos_b.trans.y;
    vectba.z:=pos_a.trans.z-pos_b.trans.z;
    unit vectba,vectba;
    !* Normalize the vector.     *)
    !* Calculate vector Pb to Pa *)
    vectbc.x:=pos_c.trans.x-pos_b.trans.x;
    vectbc.y:=pos_c.trans.y-pos_b.trans.y;
    vectbc.z:=pos_c.trans.z-pos_b.trans.z;
    unit vectbc,vectbc;
    !* Normalize the vector.     *)
    ang_1:=ACos((vectba.x*vectbc.x)+(vectba.y*vectbc.y)+(vectba.z*vectbc.z));
  ENDPROC

  !*******************************************************)
  PROC cal_scanang(
    VAR num scan_ang,
    num ang_4,
    VAR num add_2,
    num calibno)

    !*  
    !*  ABSTRACT:
    !*      The procedure first positions the robot TCP to pos Pg.
    !*      The angle between the x-axis of the sensor's koordinat 
    !*      system and the plane edge is then calculated and stored in scan_ang.
    !*      The x-axis of the sensor system is perpendicular to the 
    !*      sensor scan.
    !*
    !*               .      !Plane edge   
    !*		  .	!
    !*		   .	!
    !*		    .	!
    !*		     .	!
    !*		      . !
    !*		       .!	
    !*			.	
    !*		      .	! .	
    !*		    .	!  . 	
    !*	          .	!   .Senor scan	
    !*	x-axis	. ----->!	
    !*                scan_ang
    !*
    !********************************************************)
    CONST num offsetr:=10;
    !* 10 mm   *)
    VAR robtarget pos_g;
    !* Pd + 0.5(Pc-Pb) in base coordinates *)
    VAR robtarget off_g;
    !* pod_g + offset                      *)
    VAR pos vect;
    !* Normalized vector PaPb              *)
    VAR num stepd;
    !* Step direction                      *)
    VAR num meas1{2};
    !* Storage for the meassured edge      *)
    VAR num meas2{2};
    !* coordinates in pod_g and off_g.     *)
    VAR bool posstep;
    !* Is there a pos step in the sensor range *)
    VAR bool negstep;

    !* Is there a neg step in the sensor range *)
    !* Calculate pos Pg *)
    !Write outfile,  "*********** cal_scanang() ***********";
    pos_g:=pos_d;
    pos_g.trans.x:=pos_d.trans.x+0.5*(pos_c.trans.x-pos_b.trans.x);
    pos_g.trans.y:=pos_d.trans.y+0.5*(pos_c.trans.y-pos_b.trans.y);
    pos_g.trans.z:=pos_d.trans.z+0.5*(pos_c.trans.z-pos_b.trans.z);
    MoveL pos_g,v100,fine,ctool;
    WaitTime 0.5;
    scan_tst posstep,negstep,calibno;
    IF posstep AND negstep THEN
      Break;
      RAISE mea_err;
    ENDIF
    IF (NOT posstep) AND (NOT negstep) THEN
      Break;
      RAISE mea_err;
    ENDIF
    IF posstep AND (NOT negstep) THEN
      add_2:=180;
      stepd:=right_lap;
    ENDIF
    IF negstep AND (NOT posstep) THEN
      add_2:=0;
      stepd:=left_lap;
    ENDIF
    !* Calculate vect *)
    vect.x:=pos_b.trans.x-pos_a.trans.x;
    vect.y:=pos_b.trans.y-pos_a.trans.y;
    vect.z:=pos_b.trans.z-pos_a.trans.z;
    unit vect,vect;
    !* Normalize vect *)
    !!Break;
    edgemess vect,stepd,meas1,calibno;
    off_g:=pos_g;
    off_g.trans.x:=pos_g.trans.x+vect.x*(offsetr);
    off_g.trans.y:=pos_g.trans.y+vect.y*(offsetr);
    off_g.trans.z:=pos_g.trans.z+vect.z*(offsetr);
    MoveL off_g,v100,fine,ctool;
    WaitTime 0.5;
    edgemess vect,stepd,meas2,calibno;
    scan_ang:=ASin(Cos(ang_4)*(offsetr)/(Abs(meas1{2}-meas2{2})));
    !Write outfile,  "scan_ang"\Num:=scan_ang;
  ERROR
    TEST ERRNO
    CASE SEN_NO_MEAS:
      Write outfile,"SEN_NO_MEAS error reported";
      TPWrite "SEN_NO_MEAS error reported";
    DEFAULT:
      RAISE;
    ENDTEST
  ENDPROC

  !*******************************************************)
  PROC zoff_meas(
    num calibno)

    !*  
    !*  ABSTRACT:
    !*      The procedure returns avrage z values for positive and
    !*      negative edge. This is considered to be origo of the
    !*      sensor coordinate system.
    !*
    !********************************************************)
    VAR pos vect;
    VAR pos vectba;
    VAR pos vectbc;
    VAR num meas_pos{2};
    !* Corner edge coordinates    *)
    VAR num meas_neg{2};

    !* {1}=dist {2}=side          *) 
    !Write outfile,  "*********** zoff_meas() ***********";
    !* Calculate vector Pb to Pa
    vectba.x:=pos_a.trans.x-pos_b.trans.x;
    vectba.y:=pos_a.trans.y-pos_b.trans.y;
    vectba.z:=pos_a.trans.z-pos_b.trans.z;
    unit vectba,vectba;
    !* Normalize the vector.     *)
    !* Calculate vector Pb to Pc *)  
    vectbc.x:=pos_c.trans.x-pos_b.trans.x;
    vectbc.y:=pos_c.trans.y-pos_b.trans.y;
    vectbc.z:=pos_c.trans.z-pos_b.trans.z;
    unit vectbc,vectbc;
    !* Normalize the vector.     *)
    !* Calculate a diagonal vector *)
    vect.x:=vectba.x+vectbc.x;
    vect.y:=vectba.y+vectbc.y;
    vect.z:=vectba.z+vectbc.z;
    unit vect,vect;
    !* Normalize the vector.     *)
    scal_pos{calibno}:=[0,0,0];
    edgemess vect,right_lap,meas_pos,calibno;
    edgemess vect,left_lap,meas_neg,calibno;
    scal_pos{calibno}.z:=(meas_neg{1}+meas_pos{1})/2;
  ERROR
    TEST ERRNO
    CASE SEN_NO_MEAS:
      Write outfile,"SEN_NO_MEAS error reported";
      TPWrite "SEN_NO_MEAS error reported";
    DEFAULT:
      RAISE;
    ENDTEST
  ENDPROC

  !*******************************************************)
  PROC corn_meas(
    VAR num meas_pos{*},
    VAR num meas_neg{*},
    num calibno)

    !*  
    !*  ABSTRACT:
    !*      The procedure returns side and distance coordinates
    !*      for the intersection of the sensorscan and 
    !*      the two plane edges of the calibration plane 
    !*      in pos Pb.
    !*
    !********************************************************)
    VAR pos vect;
    VAR pos vectba;
    VAR pos vectbc;

    !Write outfile,  "*********** corn_meas() ***********";
    !* Calculate vector Pb to Pa
    vectba.x:=pos_a.trans.x-pos_b.trans.x;
    vectba.y:=pos_a.trans.y-pos_b.trans.y;
    vectba.z:=pos_a.trans.z-pos_b.trans.z;
    unit vectba,vectba;
    !* Normalize the vector.     *)
    !* Calculate vector Pb to Pc *)  
    vectbc.x:=pos_c.trans.x-pos_b.trans.x;
    vectbc.y:=pos_c.trans.y-pos_b.trans.y;
    vectbc.z:=pos_c.trans.z-pos_b.trans.z;
    unit vectbc,vectbc;
    !* Normalize the vector.     *)
    !* Calculate a diagonal vector *)
    vect.x:=vectba.x+vectbc.x;
    vect.y:=vectba.y+vectbc.y;
    vect.z:=vectba.z+vectbc.z;
    unit vect,vect;
    !* Normalize the vector.     *)
    !!Break;
    edgemess vect,right_lap,meas_pos,calibno;
    edgemess vect,left_lap,meas_neg,calibno;
  ERROR
    TEST ERRNO
    CASE SEN_NO_MEAS:
      Write outfile,"SEN_NO_MEAS error reported";
      TPWrite "SEN_NO_MEAS error reported";
    DEFAULT:
      RAISE;
    ENDTEST
  ENDPROC

  !*******************************************************)
  PROC writ_res()
    !*  
    !*  ABSTRACT:
    !*      The procedure writes the response from calib.
    !*
    !********************************************************)
    Write outfile,"Calibration result";
    Write outfile,"dkxx "\Num:=se_cal{1};
    Write outfile,"dkxy "\Num:=se_cal{2};
    Write outfile,"dkxz "\Num:=se_cal{3};
    Write outfile,"dkyx "\Num:=se_cal{4};
    Write outfile,"dkyy "\Num:=se_cal{5};
    Write outfile,"dkyz "\Num:=se_cal{6};
    Write outfile,"dkzx "\Num:=se_cal{7};
    Write outfile,"dkzy "\Num:=se_cal{8};
    Write outfile,"dkzz "\Num:=se_cal{9};
    Write outfile,"trtx "\Num:=se_cal{10};
    Write outfile,"trty "\Num:=se_cal{11};
    Write outfile,"trtz "\Num:=se_cal{12};
  ENDPROC

  !*******************************************************)
  PROC wobjhand(
    robtarget target1,
    VAR pose handpose,
    tooldata tool)

    !*  
    !*  ABSTRACT:
    !*      The procedure calculates hand_coordinates from WObj_coordinates.
    !*
    !********************************************************)
    VAR robtarget target0;
    VAR pose pose0;
    VAR pose pose0inv;
    VAR pose pose1;

    !Write outfile,  "*********** wobjhand() ***********";
    target0:=CRobT();
    pose0.trans:=target0.trans;
    pose0.rot:=target0.rot;
    !Break;  
    pose0inv:=PoseInv(pose0);
    pose1.trans:=target1.trans;
    pose1.rot:=target1.rot;
    handpose:=PoseMult(tool.tframe,pose0inv);
    handpose:=PoseMult(handpose,pose1);
  ENDPROC

  !*******************************************************)
  PROC calpose(
    num calibno)

    !*  
    !*  ABSTRACT:
    !*  Put the coordinate transformation from hand to sensor 
    !*  in the pose variable scal_pose.
    !*
    !*  Calculate the quaternions from the rotation matrix.
    !*  and put them in scal_pose.rot
    !*
    !*  q1=Sqrt(x1+y2+z3+1)/2     
    !*  q2=Sqrt(x1-y2-z3+1)/2     sign q2=sign(y3-z2)
    !*  q3=Sqrt(y2-x1-z3+1)/2     sign q3=sign(z1-x3)
    !*  q4=Sqrt(z3-x1-y2+1)/2     sign q4=sign(x2-y1)
    !*
    !*  Put the translation in sensorl.trans
    !*
    !********************************************************)
    !Write outfile,  "*********** Calpose() ***********";

    scal_pose{calibno}.rot.q1:=Sqrt(se_cal{1}+se_cal{5}+se_cal{9}+1)/2;
    scal_pose{calibno}.rot.q2:=Sqrt(se_cal{1}-se_cal{5}-se_cal{9}+1)/2;
    scal_pose{calibno}.rot.q3:=Sqrt(se_cal{5}-se_cal{1}-se_cal{9}+1)/2;
    scal_pose{calibno}.rot.q4:=Sqrt(se_cal{9}-se_cal{1}-se_cal{5}+1)/2;

    IF se_cal{8}-se_cal{6}<0 scal_pose{calibno}.rot.q2:=-scal_pose{calibno}.rot.q2;
    IF se_cal{3}-se_cal{7}<0 scal_pose{calibno}.rot.q3:=-scal_pose{calibno}.rot.q3;
    IF se_cal{4}-se_cal{2}<0 scal_pose{calibno}.rot.q4:=-scal_pose{calibno}.rot.q4;
    
    
    scal_pose{calibno}.trans.x:=se_cal{10};
    scal_pose{calibno}.trans.y:=se_cal{11};
    scal_pose{calibno}.trans.z:=se_cal{12};
  ENDPROC

  !*******************************************
  FUNC pos Measure(
    num jointno,
    num calibno)

    !*
    !* ABSTRACT:
    !*    Measure with a Laser Sensor 
    !*
    !* RETURN:
    !* IF calib is present then x,y,z in WObj coordinates.
    !*
    !* ELSE x=0 ,y,z in sensor coordinates.
    !*
    !*******************************************
    CONST num start:=6;
    CONST num suspend:=7;
    CONST num on:=1;
    CONST num off:=0;
    CONST num yes:=1;
    CONST num no:=0;
    CONST num y:=9;
    CONST num z:=10;
    CONST num joint:=16;
    CONST num timeout:=10;
    VAR num err_count;
    VAR pos meapos;
    VAR robtarget target0;
    VAR pose objinv;
    VAR pose userinv;
    VAR pose toolinv;
    VAR pose calibinv;
    VAR pose targettmp;
    VAR pose target1;
    VAR pose pose0;
    VAR num ry;
    VAR num rz;
    VAR num ry_raw{5}:=[0,0,0,0,0];
    VAR num rz_raw{5}:=[0,0,0,0,0];
    VAR num ry_avr{5}:=[0,0,0,0,0];
    VAR num rz_avr{5}:=[0,0,0,0,0];
    VAR num i;
    VAR num j;

    !* Write new joint number
    WriteVar joint,jointno;
    WriteVar 7,0;
    WaitTime 2;
    !* Read data
    ry:=0;
    rz:=0;
    FOR i FROM 1 TO 5 DO
      FOR j FROM 1 TO 5 DO
        ry_raw{j}:=ReadVar(y);
        IF ry_raw{j}>32767 ry_raw{j}:=ry_raw{j}-65536;
        ry_raw{j}:=ry_raw{j}/mea_unit;          
        err_count:=0;
        rz_raw{j}:=ReadVar(z);
        rz_raw{j}:=rz_raw{j}/mea_unit;      
        err_count:=0;      
      ENDFOR
      ry_avr{i}:=avrage(ry_raw);
      rz_avr{i}:=avrage(rz_raw);
    ENDFOR
    ry:=avrage(ry_avr);
    rz:=avrage(rz_avr);
    !Write outfile,  "ry="\Num:=ry;
    !Write outfile,  "rz="\Num:=rz;

    meapos.x:=0;
    meapos.y:=ry;
    meapos.z:=rz-scal_pos{calibno}.z;
    RETURN meapos;
  ERROR
    TEST ERRNO
    CASE SEN_NO_MEAS:
      !Write outfile,  "SEN_NO_MEAS error reported in Measure()";
      IF (err_count<4) THEN
        WaitTime 0.1;
        err_count:=err_count+1;
        RETRY;
      ELSE
        RAISE;
      ENDIF
    CASE SEN_NOREADY:
      Write outfile,"SEN_NOREADY error reported";
      TPWrite "SEN_NOREADY error reported";
    CASE SEN_GENERRO:
      Write outfile,"SEN_GENERRO error reported";
      TPWrite "SEN_GENERRO error reported";
    CASE SEN_BUSY:
      Write outfile,"SEN_BUSY error reported";
      TPWrite "SEN_BUSY error reported";
    CASE SEN_UNKNOWN:
      Write outfile,"SEN_UNKNOWN error reported";
      TPWrite "SEN_UNKNOWN error reported";
    CASE SEN_ILLEGAL:
      Write outfile,"SEN_ILLEGAL error reported";
      TPWrite "SEN_ILLEGAL error reported";
    CASE SEN_EXALARM:
      Write outfile,"SEN_EXALARM error reported";
      TPWrite "SEN_EXALARM error reported";
    CASE SEN_CAALARM:
      Write outfile,"SEN_CAALARM error reported";
      TPWrite "SEN_CAALARM error reported";
    CASE SEN_TEMP:
      Write outfile,"SEN_TEMP error reported";
      TPWrite "SEN_TEMP error reported";
    CASE SEN_VALUE:
      Write outfile,"SEN_VALUE error reported";
      TPWrite "SEN_VALUE error reported";
    CASE SEN_CAMCHECK:
      Write outfile,"SEN_CAMCHECK error reported";
      TPWrite "SEN_CAMCHECK error reported";
    CASE SEN_TIMEOUT:
      Write outfile,"SEN_TIMEOUT error reported";
      TPWrite "SEN_TIMEOUT error reported";
    DEFAULT:
      Write outfile,"Unknown error reported"\Num:=ERRNO;
      TPWrite "Unknown error reported";
    ENDTEST
  ENDFUNC

  !*******************************************
  FUNC robtarget Search(
    num jointno,
    tooldata tool,
    num calibno)

    !*
    !* ABSTRACT:
    !*    Measure with a Laser Sensor 
    !*
    !* RETURN:
    !* IF calib is present then x,y,z in WObj coordinates.
    !*
    !* ELSE x=0 ,y,z in sensor coordinates.
    !*
    !*******************************************
    CONST num start:=6;
    CONST num suspend:=7;
    CONST num on:=1;
    CONST num off:=0;
    CONST num yes:=1;
    CONST num no:=0;
    CONST num y:=9;
    CONST num z:=10;
    CONST num joint:=16;
    CONST num timeout:=10;
    VAR num err_count;
    VAR pos meapos;
    VAR robtarget target0;
    VAR robtarget searchp;
    VAR pose objinv;
    VAR pose userinv;
    VAR pose toolinv;
    VAR pose calibinv;
    VAR pose targettmp;
    VAR pose target1;
    VAR pose pose0;
    VAR num ry;
    VAR num rz;
    VAR num ry_raw{5}:=[0,0,0,0,0];
    VAR num rz_raw{5}:=[0,0,0,0,0];
    VAR num ry_avr{5}:=[0,0,0,0,0];
    VAR num rz_avr{5}:=[0,0,0,0,0];
    VAR num i;
    VAR num j;

    !* Selection of joint number
    WriteVar joint,jointno;
    !* Measure on
    WriteVar start,on;
    WriteVar suspend,no;
    WaitTime 2;
    !* Read data
    ry:=0;
    rz:=0;
    
    ! Read raw data from the sensor and
    ! change from unsign to sign word and change to right unit.    
    FOR j FROM 1 TO 5 DO
      ry_raw{j}:=ReadVar(y);
      IF ry_raw{j}>32767 ry_raw{j}:=ry_raw{j}-65536;
      ry_raw{j}:=ry_raw{j}/mea_unit;          
      err_count:=0;
      rz_raw{j}:=ReadVar(z);
      rz_raw{j}:=rz_raw{j}/mea_unit;      
      err_count:=0;
    ENDFOR    
    
    
    ry:=avrage(ry_raw);
    rz:=avrage(rz_raw);
    
    !Write outfile,  "ry="\Num:=ry;
    !Write outfile,  "rz="\Num:=rz;

    meapos.x:=0;
    meapos.y:=ry;
    meapos.z:=rz-scal_pos{calibno}.z;
      
    
    !* Measure off
    WriteVar suspend,yes;
    
    
    !* Transform sensor coordinates to WObj coordinates with calib
    !*  
    !* Calculate frame to sensor origo.
    !* target1:=target0*PoseInv(tool)*calib
    !* Calculate the measured sensor pos to WObj coordinates.
    !* meapos:=target1*meapos.
    target0:=CRobT();
    pose0.trans:=target0.trans;
    pose0.rot:=target0.rot;
    toolinv:=PoseInv(tool.tframe);
    target1:=PoseMult(pose0,toolinv);
    target1:=PoseMult(target1,scal_pose{calibno});
    meapos:=PoseVect(target1,meapos);
    searchp:=target0;
    searchp.trans:=meapos;
    RETURN searchp;
  ERROR
    TEST ERRNO
    CASE SEN_NO_MEAS:
      !Write outfile,  "SEN_NO_MEAS error reported in Measure()";
      IF (err_count<4) THEN
        WaitTime 0.1;
        err_count:=err_count+1;
        RETRY;
      ELSE
        RAISE;
      ENDIF
    CASE SEN_NOREADY:
      Write outfile,"SEN_NOREADY error reported";
      TPWrite "SEN_NOREADY error reported";
    CASE SEN_GENERRO:
      Write outfile,"SEN_GENERRO error reported";
      TPWrite "SEN_GENERRO error reported";
    CASE SEN_BUSY:
      Write outfile,"SEN_BUSY error reported";
      TPWrite "SEN_BUSY error reported";
    CASE SEN_UNKNOWN:
      Write outfile,"SEN_UNKNOWN error reported";
      TPWrite "SEN_UNKNOWN error reported";
    CASE SEN_ILLEGAL:
      Write outfile,"SEN_ILLEGAL error reported";
      TPWrite "SEN_ILLEGAL error reported";
    CASE SEN_EXALARM:
      Write outfile,"SEN_EXALARM error reported";
      TPWrite "SEN_EXALARM error reported";
    CASE SEN_CAALARM:
      Write outfile,"SEN_CAALARM error reported";
      TPWrite "SEN_CAALARM error reported";
    CASE SEN_TEMP:
      Write outfile,"SEN_TEMP error reported";
      TPWrite "SEN_TEMP error reported";
    CASE SEN_VALUE:
      Write outfile,"SEN_VALUE error reported";
      TPWrite "SEN_VALUE error reported";
    CASE SEN_CAMCHECK:
      Write outfile,"SEN_CAMCHECK error reported";
      TPWrite "SEN_CAMCHECK error reported";
    CASE SEN_TIMEOUT:
      Write outfile,"SEN_TIMEOUT error reported";
      TPWrite "SEN_TIMEOUT error reported";
    DEFAULT:
      Write outfile,"Unknown error reported"\Num:=ERRNO;
      TPWrite "Unknown error reported";
    ENDTEST
  ENDFUNC
  
  !*******************************************
  FUNC robtarget Searchtr(
    tooldata tool,
    trackdata trackd,
    num calibno)

    !*
    !* ABSTRACT:
    !*    Measure with a Laser Sensor 
    !*
    !* RETURN:
    !* IF calib is present then x,y,z in WObj coordinates.
    !*
    !* ELSE x=0 ,y,z in sensor coordinates.
    !*
    !*******************************************
    CONST num start:=6;
    CONST num suspend:=7;
    CONST num on:=1;
    CONST num off:=0;
    CONST num yes:=1;
    CONST num no:=0;
    CONST num y:=9;
    CONST num z:=10;
    CONST num joint:=16;
    CONST num timeout:=10;
    VAR num err_count;
    VAR pos meapos;
    VAR robtarget target0;
    VAR robtarget searchp;
    VAR pose objinv;
    VAR pose userinv;
    VAR pose toolinv;
    VAR pose calibinv;
    VAR pose targettmp;
    VAR pose target1;
    VAR pose pose0;
    VAR num ry;
    VAR num rz;
    VAR num ry_raw{5}:=[0,0,0,0,0];
    VAR num rz_raw{5}:=[0,0,0,0,0];
    VAR num ry_avr{5}:=[0,0,0,0,0];
    VAR num rz_avr{5}:=[0,0,0,0,0];
    VAR num i;
    VAR num j;

    !* Selection of joint number
    WriteVar joint,trackd.joint_no;
    !* Measure on
    WriteVar start,on;
    WriteVar suspend,no;
    WaitTime 2;
    !* Read data
    ry:=0;
    rz:=0;

    
    ! Read raw data from the sensor and
    ! change from unsign to sign word and change to right unit.    
    FOR j FROM 1 TO 5 DO
      ry_raw{j}:=ReadVar(y);
      IF ry_raw{j}>32767 ry_raw{j}:=ry_raw{j}-65536;
      ry_raw{j}:=ry_raw{j}/mea_unit;          
      err_count:=0;
      rz_raw{j}:=ReadVar(z);
      rz_raw{j}:=rz_raw{j}/mea_unit;      
      err_count:=0;
    ENDFOR
    ry:=avrage(ry_raw);
    rz:=avrage(rz_raw);
    !Write outfile,  "ry="\Num:=ry;
    !Write outfile,  "rz="\Num:=rz;  
    

    !Write outfile,  "meapos.y="\Num:=ry \Nonewline;
    !Write outfile,  "  meapos.z="\Num:=rz;
    !!Break;
    
    meapos.x:=0;
    meapos.y:=ry;
    meapos.z:=rz-scal_pos{calibno}.z;
    
    
    !* Offsets from trackdata
    meapos.y:=meapos.y+trackd.hor_offset;
    meapos.z:=meapos.z+trackd.vert_offset; 
        
    !* Measure off
    !* Transform sensor coordinates to WObj coordinates with calib
    !*  
    !* Calculate frame to sensor origo.
    !* target1:=target0*PoseInv(tool)*calib
    !* Calculate the measured sensor pos to WObj coordinates.
    !* meapos:=target1*meapos.
    target0:=CRobT();
    pose0.trans:=target0.trans;
    pose0.rot:=target0.rot;
    toolinv:=PoseInv(tool.tframe);
    target1:=PoseMult(pose0,toolinv);
    target1:=PoseMult(target1,scal_pose{calibno});
    meapos:=PoseVect(target1,meapos);
    searchp:=target0;
    searchp.trans:=meapos;
    RETURN searchp;
  ERROR
    TEST ERRNO
    CASE SEN_NO_MEAS:
      !Write outfile,  "SEN_NO_MEAS error reported in Measure()";
      IF (err_count<4) THEN
        WaitTime 0.1;
        err_count:=err_count+1;
        RETRY;
      ELSE
        RAISE;
      ENDIF
    CASE SEN_NOREADY:
      Write outfile,"SEN_NOREADY error reported";
      TPWrite "SEN_NOREADY error reported";
    CASE SEN_GENERRO:
      Write outfile,"SEN_GENERRO error reported";
      TPWrite "SEN_GENERRO error reported";
    CASE SEN_BUSY:
      Write outfile,"SEN_BUSY error reported";
      TPWrite "SEN_BUSY error reported";
    CASE SEN_UNKNOWN:
      Write outfile,"SEN_UNKNOWN error reported";
      TPWrite "SEN_UNKNOWN error reported";
    CASE SEN_ILLEGAL:
      Write outfile,"SEN_ILLEGAL error reported";
      TPWrite "SEN_ILLEGAL error reported";
    CASE SEN_EXALARM:
      Write outfile,"SEN_EXALARM error reported";
      TPWrite "SEN_EXALARM error reported";
    CASE SEN_CAALARM:
      Write outfile,"SEN_CAALARM error reported";
      TPWrite "SEN_CAALARM error reported";
    CASE SEN_TEMP:
      Write outfile,"SEN_TEMP error reported";
      TPWrite "SEN_TEMP error reported";
    CASE SEN_VALUE:
      Write outfile,"SEN_VALUE error reported";
      TPWrite "SEN_VALUE error reported";
    CASE SEN_CAMCHECK:
      Write outfile,"SEN_CAMCHECK error reported";
      TPWrite "SEN_CAMCHECK error reported";
    CASE SEN_TIMEOUT:
      Write outfile,"SEN_TIMEOUT error reported";
      TPWrite "SEN_TIMEOUT error reported";
    DEFAULT:
      Write outfile,"Unknown error reported"\Num:=ERRNO;
      TPWrite "Unknown error reported";
    ENDTEST
  ENDFUNC  

  !*******************************************
  FUNC pos meaq(
    num jointno,
    tooldata tool,
    num calibno)

    !*
    !* ABSTRACT:
    !*    Measure with a Laser Sensor 
    !*
    !* RETURN:
    !* IF calib is present then x,y,z in WObj coordinates.
    !*
    !* ELSE x=0 ,y,z in sensor coordinates.
    !*
    !*******************************************
    CONST num start:=6;
    CONST num suspend:=7;
    CONST num on:=1;
    CONST num off:=0;
    CONST num yes:=1;
    CONST num no:=0;
    CONST num y:=9;
    CONST num z:=10;
    CONST num joint:=16;
    VAR num err_count;
    VAR pos meapos;
    VAR robtarget target0;
    VAR pose objinv;
    VAR pose userinv;
    VAR pose toolinv;
    VAR pose calibinv;
    VAR pose targettmp;
    VAR pose target1;
    VAR pose pose0;
    VAR num ry;
    VAR num rz;
    VAR num i;
    VAR num j;

    !* Selection of joint number
    WriteVar joint,jointno;
    !* Measure on
    WriteVar suspend,no;
    WaitTime 2;
    !* Read data
    ry:=0;
    rz:=0;
    ry:=ReadVar(y);
    err_count:=0;
    rz:=ReadVar(z);
    err_count:=0;
    !Write outfile,  "ry="\Num:=ry;
    !Write outfile,  "rz="\Num:=rz;
    ! Change from unsign to sign word. And change to right unit.
    IF ry>32767 ry:=ry-65536;
    ry:=ry/mea_unit;
    IF rz>32767 rz:=rz-65536;
    rz:=rz/mea_unit;
    !Write outfile,  "meapos.y="\Num:=ry \Nonewline;
    !Write outfile,  "  meapos.z="\Num:=rz;
    !!Break;
    meapos.x:=0;
    meapos.y:=ry;
    meapos.z:=rz-scal_pos{calibno}.z;
    !* Measure off
    WriteVar suspend,yes;
    !* Transform sensor coordinates to WObj coordinates with calib
    !*  
    !* Calculate frame to sensor origo.
    !* target1:=target0*PoseInv(tool)*calib
    !* Calculate the measured sensor pos to WObj coordinates.
    !* meapos:=target1*meapos.
    target0:=CRobT();
    pose0.trans:=target0.trans;
    pose0.rot:=target0.rot;
    toolinv:=PoseInv(tool.tframe);
    target1:=PoseMult(pose0,toolinv);
    target1:=PoseMult(target1,scal_pose{calibno});
    meapos:=PoseVect(target1,meapos);
    RETURN meapos;
  ERROR
    TEST ERRNO
    CASE SEN_NO_MEAS:
      !Write outfile,  "SEN_NO_MEAS error reported in Measure()";
      IF (err_count<4) THEN
        WaitTime 0.1;
        err_count:=err_count+1;
        RETRY;
      ELSE
        RAISE;
      ENDIF
    CASE SEN_NOREADY:
      Write outfile,"SEN_NOREADY error reported";
      TPWrite "SEN_NOREADY error reported";
    CASE SEN_GENERRO:
      Write outfile,"SEN_GENERRO error reported";
      TPWrite "SEN_GENERRO error reported";
    CASE SEN_BUSY:
      Write outfile,"SEN_BUSY error reported";
      TPWrite "SEN_BUSY error reported";
    CASE SEN_UNKNOWN:
      Write outfile,"SEN_UNKNOWN error reported";
      TPWrite "SEN_UNKNOWN error reported";
    CASE SEN_ILLEGAL:
      Write outfile,"SEN_ILLEGAL error reported";
      TPWrite "SEN_ILLEGAL error reported";
    CASE SEN_EXALARM:
      Write outfile,"SEN_EXALARM error reported";
      TPWrite "SEN_EXALARM error reported";
    CASE SEN_CAALARM:
      Write outfile,"SEN_CAALARM error reported";
      TPWrite "SEN_CAALARM error reported";
    CASE SEN_TEMP:
      Write outfile,"SEN_TEMP error reported";
      TPWrite "SEN_TEMP error reported";
    CASE SEN_VALUE:
      Write outfile,"SEN_VALUE error reported";
      TPWrite "SEN_VALUE error reported";
    CASE SEN_CAMCHECK:
      Write outfile,"SEN_CAMCHECK error reported";
      TPWrite "SEN_CAMCHECK error reported";
    CASE SEN_TIMEOUT:
      Write outfile,"SEN_TIMEOUT error reported";
      TPWrite "SEN_TIMEOUT error reported";
    DEFAULT:
      Write outfile,"Unknown error reported"\Num:=ERRNO;
      TPWrite "Unknown error reported";
    ENDTEST
  ENDFUNC

  FUNC pos Measuresim(
    robtarget send_p,
    robtarget act_p,
    num calibno)

    VAR pos meapos;
    VAR num mdata{2};

    mcount:=mcount+1;
    invalid:=FALSE;
    ! corn_meas 2Xedgemess pos/neg
    TEST mcount
    CASE 1,2,3,4,5:
      mdata{1}:=200;
      mdata{2}:=send_p.trans.y-act_p.trans.y-(Sqrt(2)*20);
    CASE 6,7,8,9,10:
      mdata{1}:=200;
      mdata{2}:=send_p.trans.y-act_p.trans.y+(Sqrt(2)*20);
      ! cal_scanang 2Xedgemess pos/pos 
    CASE 11,12,13,14,15,16,17,18,19,20:
      mdata{1}:=200;
      mdata{2}:=send_p.trans.x-act_p.trans.x-(Sqrt(2)*20);
      !             2Xedgemess neg/neg 
    CASE 21,22,23,24,25,26,27,28,29,30:
      mdata{1}:=0;
      mdata{2}:=0;
      invalid:=TRUE;
      !             edgemess pos
    CASE 31,32,33,34,35:
      mdata{1}:=200;
      mdata{2}:=send_p.trans.y-act_p.trans.y-(Sqrt(2)*20);
      !    20mm ->  edgemess pos
    CASE 36,37,38,39,40:
      mdata{1}:=200;
      mdata{2}:=send_p.trans.y-act_p.trans.y;
      ! offset 25 basZ 2Xedgemess pos/neg 
    CASE 41,42,43,44,45:
      mdata{1}:=175;
      mdata{2}:=send_p.trans.x-act_p.trans.x-(Sqrt(2)*20);
    CASE 46,47,48,49,50:
      mdata{1}:=175;
      mdata{2}:=send_p.trans.x-act_p.trans.x+(Sqrt(2)*20);
    DEFAULT:
      mdata{1}:=0;
      mdata{2}:=0;
      invalid:=TRUE;
    ENDTEST
    meapos.x:=0;
    meapos.y:=mdata{2};
    IF NOT invalid THEN
      meapos.z:=mdata{1}-scal_pos{calibno}.z;
    ELSE
      meapos.z:=mdata{1};
    ENDIF
    TPWrite "mcount "\Num:=mcount;
    TPWrite "meapos  "\Pos:=meapos;
    RETURN meapos;
  ENDFUNC
ENDMODULE
