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

MODULE CAL6676
  !COPYRIGHT 2001 ABB Automation Technology Products AB Robotics
  !All rights reserved
  !2001-12-12 ATRO/PAU/SLT

  !#################################################################
  !     Revision notes:
  !     NEW IRB6600/7600 calibration program  2001-12-12/SLT
  !
  !#################################################################

  ! Definition of global variables
  VAR num answer;
  VAR bool flag;
  VAR string irbtype;

  ! Definition of jointtargets
  VAR jointtarget before_norm_pos;
  VAR jointtarget stand_norm_pos;
  VAR jointtarget stand_left_pos;
  VAR jointtarget stand_right_pos;
  VAR jointtarget CAL3;
  VAR jointtarget CAL4A;
  VAR jointtarget CAL4B;

  PROC Haupt()
    !Proc used when German is booted
    main;
  ENDPROC

  PROC Principale()
    !Proc used when Italian is booted
    main;
  ENDPROC

  PROC main()
    TPErase;
    TPWrite "CALIBRATION POSITION PROGRAM";
    TPReadFK answer,"","Start","","","","Exit";
    TEST answer
    CASE 1:
      irbtype:="IRB66007600";
      before_norm_pos:=[[-4,-4,4,4,-4,4],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
      stand_norm_pos:=[[0,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
      stand_right_pos:=[[-90,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
      stand_left_pos:=[[90,0,0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
      CAL3:=[[0,0,0,-90,0,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
      CAL4A:=[[0,0,0,-90,-90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
      CAL4B:=[[0,0,0,90,-90,90],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CASE 5:
      Exit;
    ENDTEST
    flag:=TRUE;
    WHILE flag DO
      TPErase;
      TPWrite "Calibration program for ";
      TPWrite irbtype;
      TPReadFK answer,"Select operation ...","Calib","Cal.pos","","","Exit";
      TEST answer
      CASE 1:
        calib;
      CASE 2:
        cal_pos;
      CASE 5:
        flag:=FALSE;
        EXIT;
      ENDTEST
    ENDWHILE
  ENDPROC

  PROC cal_pos()
    VAR bool cal_posflag;
    cal_posflag:=TRUE;
    WHILE cal_posflag DO
      TPErase;
      TPWrite "Calibration routines for ";
      TPWrite irbtype;
      TPReadFK answer,"Go to calibration position ","Normal","Right","Left","","Quit";
      TEST answer
      CASE 1:
        home_stand_norm;
      CASE 2:
        home_stand_right;
      CASE 3:
        home_stand_left;
      CASE 5:
        cal_posflag:=FALSE;
      ENDTEST
    ENDWHILE
  ENDPROC

  PROC home_stand_norm()
    TPErase;
    TPWrite "Moving to normal position (sync.pos 0)";
    TPWrite " ";
    TPWrite "Axis 1-6      0 deg";
    TPWrite " ";
    MoveAbsJ before_norm_pos,v200,fine,tool0;
    MoveAbsJ stand_norm_pos,v200,fine,tool0;
  ENDPROC

  PROC home_stand_right()
    TPWrite "Moving to right position (sync.pos 1)";
    TPWrite "Axis 1    -90 deg";
    TPWrite "     2      0 deg";
    TPWrite "     3      0 deg";
    MoveAbsJ stand_right_pos,v200,fine,tool0;
  ENDPROC

  PROC home_stand_left()
    TPWrite "Moving to left position (sync.pos 2)";
    TPWrite "Axis 1    +90 deg";
    TPWrite "     2      0 deg";
    TPWrite "     3      0 deg";
    MoveAbsJ stand_left_pos,v200,fine,tool0;
  ENDPROC

  PROC calib()
    VAR bool calibflag;
    calibflag:=TRUE;
  
    WHILE calibflag DO
      TPReadFK answer,"Calibration position...","CAL3","CAL4A","CAL4B","CAL5","Quit";
      TEST answer
      CASE 1:
        MoveAbsJ CAL4A,v200,fine,tool0;
        MoveAbsJ CAL3,v200,fine,tool0;
      CASE 2:
        MoveAbsJ CAL4A,v200,fine,tool0;
      CASE 3:
        MoveAbsJ stand_norm_pos,v200,fine,tool0;
        MoveAbsJ CAL4B,v200,fine,tool0;
      CASE 4:
        MoveAbsJ stand_norm_pos,v200,fine,tool0;
      CASE 5:
        calibflag:=FALSE;
      ENDTEST
    ENDWHILE
  ENDPROC
ENDMODULE
