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

MODULE SENCAL

  !*****************************************************************
  ! Example on how to use SENCALIB.SYS for Laser Sensor Calibration
  ! Copyright (c) ABB Robotics Products AB 1992  All rights reserved
  !*****************************************************************

  PERS tooldata torch:=[TRUE,[[-2.99622,223.601,181.291],[0.709626,-0.704579,-0.000538,0.000535]],[5,[110,0,90],[1,0,0,0],0,0,0]];  
  
  PROC main()
  
    CONST num calib1:=1;
    VAR robtarget pos_a;
    VAR robtarget pos_b;
    VAR robtarget pos_c;
    VAR robtarget pos_d;
    VAR robtarget pos_e;
    CONST num standing:=1;
    CONST num hanging:=0;
    VAR num mounting:=standing;
    VAR num answer;
    VAR bool simulate:=FALSE;
    CONST num left_lap:=1;
    CONST num right_lap:=2;

    TPErase;
    TPReadFK answer,"Do you want to calibrate ?","Yes","","","","No";
    IF answer = 5 EXIT;

    TPErase;
    TPReadFK answer,"Sensor mounted on robot ?","Yes","","","","No";
    IF answer = 5 THEN 
      simulate := TRUE;
      TPErase;
      TPWrite "The sensor will be simulated";
      TPWrite "The robot can not use the calibration";
      TPWrite "result";
      TPReadFK answer,"Start simulation ?","Yes","","","","No";
      IF answer = 5 EXIT;
    ELSE
      simulate:=FALSE;
      TPErase;
      TPWrite "ModPos the MoveL instr. from pos_a";
      TPWrite "to pos_d on the calibration plate.";
      TPReadFK answer,"Done ?","Yes","","","","No";
      IF answer = 5 EXIT;
    ENDIF
    
    TPErase;
    TPWrite "Moving towards pos_a";
    MoveL [[838.08,392.19,417.35],[0.015188,-0.688088,0.72517,0.02084],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,fine,torch;
    WaitTime 0.1;
    pos_a:=CRobT();

    TPWrite "Moving towards pos_b";
    MoveL [[838.03,642.49,417.12],[0.015137,-0.688071,0.725188,0.020849],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,fine,torch;
    WaitTime 0.1;
    pos_b:=CRobT();

    TPWrite "Moving towards pos_c";
    MoveL [[1087.56,642.04,415.01],[0.015146,-0.688069,0.725188,0.020891],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,fine,torch;
    WaitTime 0.1;
    pos_c:=CRobT();

    TPWrite "Moving towards pos_d";
    MoveL [[873.53,613.36,417.99],[0.021309,-0.939404,0.341571,-0.019947],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,fine,torch;
    WaitTime 0.1;
    pos_d:=CRobT();
        
    calibrate torch, calib1, pos_a, pos_b, pos_c, pos_d, mounting \sim_sensor:=simulate;
    
    TPErase;
    TPWrite "Calibration is ready";
    TPWrite "Result in file CALIB.LOG";
    TPWrite "scalpos{calib1} and scalpose{calib1}";
    
    TPReadFK answer,"Repeat calibration ?","Yes","","","","No";
    IF answer = 5 THEN
      IF simulate = FALSE THEN
        TPErase;
        TPReadFK answer,"Do you want to test your calibration ?","Yes","","","","No";
        IF answer = 1 THEN
          TPWrite "Test the calibration with a search";
          TPWrite "to the left_lap in pos_d";
          test_calib torch, calib1, left_lap, pos_d;
        ENDIF
      ENDIF
      EXIT; 
    ENDIF
    
  ENDPROC
  

  
  !*******************************************************)
  PROC test_calib(
    PERS tooldata test_tool,
    num calibno,
    num jointno,
    robtarget test_pos)
  !*  
  !*  ABSTRACT:
  !*      The procedure tests the calibration
  !*
  !********************************************************)

    VAR robtarget test_pos_s;
  
    MoveL test_pos,v100,fine,test_tool;
        
    ! Measure On, i.e. start the sensor
    WriteVar 7,0;
    WaitTime 2;    
        
    test_pos_s:=Search(jointno,test_tool,calibno);
    
    MoveL test_pos_s,v100,fine,test_tool;
    
  ENDPROC
  
ENDMODULE
