ABB S4c Steuerung und ISRA VISION KAMERA SYSTEM

MODULE SHORVI
  CONST string CVS_ERRHEAD := "Vision Error";
  
  ! Vision Subsystem Stati
  CONST num CVS_NOTINIT := 0;
  CONST num CVS_INIT := 1;
  CONST num CVS_INPROGRESS := 2;
  CONST num CVS_RESULTOK := 3;
  CONST num CVS_ERR := 4;


  PERS num XWERT:=0;
  PERS num YWERT:=0;
  PERS num ZWERT:=0;
  PERS num XROT:=0;
  PERS num yROT:=0;
  PERS num ZROT:=0;
  PERS num TeileNr:=0;
  PERS num nVisionStatus := 0;


  PROC haupt()
    VAR num nStep;
    VAR errnum nSaveErr;
    nVisionStatus := CVS_NOTINIT;


    ORIS_open;
    ! Status der Initialisierung auswerten (vorausgesetzt ORIS_open setzt den Status !)
    IF Status <> ORIS_OK THEN
      ErrWrite CVS_ERRHEAD,"FATAL: Unable to init Vision System";
      ! Anhalten
      Exit;
    ELSE
      nVisionStatus := CVS_INIT;
    ENDIF
    
    WHILE True DO
      ! Auf Trigger warten mit Interrupt (kein Polling = schneller)
      WaitDO doVisionActive, 1;
      nVisionStatus := CVS_INPROGRESS;


      nStep := 1;
      Status := ORIS_OK;
      WHILE (nStep <= 6) AND (Status = ORIS_OK) DO
        TEST nStep
          CASE 1:
            ORISCom_RunMode\Auto;
          CASE 2:
            ORISRep_RunMode;
          CASE 3:
            ORISCom_MeaData 0,1,1;
           CASE 4:
            ORISRep_GetReslt;
            TeileNr:=Oris_PosX;
          CASE 5:
            ORISCom_MeaData ORIS_PosX,ORIS_PosY,ORIS_PosZ;
          CASE 6:
            ORISRep_GetReslt;
        ENDTEST
        
        Incr nStep;
      ENDWHILE
      
      IF Status = ORIS_OK THEN
        XWERT:=ORIS_PosX;
        YWERT:=ORIS_PosY;
        ZWERT:=ORIS_PosZ;
        XROT:=ORIS_RotX;
        YROT:=ORIS_RotY;
        ZROT:=ORIS_RotZ;
        nVisionStatus := CVS_RESULTOK;
      ELSE
        ErrWrite CVS_ERRHEAD,"ORIS Status Err: " + NumToStr(Status,0) + " at step: " + NumToStr(nStep,0);
        nVisionStatus := CVS_ERR;
      ENDIF
      
      Reset doVisionActive;
    ENDWHILE


    ORIS_Close;
    RETURN;
    
    ERROR
      nSaveErr := ERRNO;
      ErrWrite CVS_ERRHEAD,"Error: " + NumToStr(nSaveErr,0) + " at step: " + NumToStr(nStep,0);
      ORIS_Close;
      Reset doTrigVision;
      RAISE nSaveErr;
  ENDPROC
ENDMODULE

CONST num CTeilMax:=20;
  CONST string CSaege_Procs {CTeilMax} := ["SaegeTeil1","SaegeTeil2","SaegeDummy", ... ,"SaegeDummy"];


  CONST num CVS_NOTINIT := 0;
  CONST num CVS_INIT := 1;
  CONST num CVS_INPROGRESS := 2;
  CONST num CVS_RESULTOK := 3;
  CONST num CVS_ERR := 4;


  PERS num XWERT:=0;
  PERS num YWERT:=0;
  PERS num ZWERT:=0;
  PERS num XROT:=0;
  PERS num yROT:=0;
  PERS num ZROT:=0;
  PERS num TeileNr:=0;
  PERS num nVisionStatus := 0;


  ! Triggerposition für Vision System
  ! muß noch an geeigneter Stelle geteacht werden (siehe Skizze)
  VAR robtarget rtVSTrigger;


  PROC SaegeTeil1()
    ! hier den Sägeablauf "normal" teachen OHNE Verechnung von DX bzw. DY
    ! Eine evtl. Verschiebung wird über PDispSet beim Proc-Aufruf geregelt....(s.u.)
    MoveJ ...
    ...
    MoveL ...
  ENDPROC


  PROC SaegeTeil2()
    MoveJ ...
    ...
    MoveL ...
  ENDPROC


  PROC SaegeDummy()
     ! Dummy = nix machen
  ENDPROC


  PROC HomePos(bool bOpenGrip)
    IF bOpenGrip GreiferAuf;
    HOME;
  ENDPROC


  PROC GreifposAnfahren(num nOffX,num nOffY,num nRotZ)
    ! Funktion um Greifposition anzufahren
    ConfJ\Off;
    ConfL\Off;
    Greifer1.tframe.rot:=OrientZYX(nRotZ,0,0);
    ! mit obigem Befehl wird die Orientierung um Z eingestellt mit aktuellem Wert
    MoveL [[nOffX,nOffY,80],[0.000395,0.965035,-0.262121,-6.1E-05],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],vmax,fine,Greifer1\WObj:=wobjKAMERA;
    WaitTime\InPos,0;
    MoveL [[nOffX,nOffY,-30],[0.000395,0.965035,-0.262121,-6.1E-05],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v50,fine,Greifer1\WObj:=wobjKAMERA;
    ! Der erste MoveL BEfehl fährt über das Teil und der zweite bewegt den greifer senkrecht nach unten
  ENDPROC


  PROC TeilAnheben(num nOffX,num nOffY)
    GreiferZu;
    MoveL [[nOffX,nOffY,80],[0.000395,0.965035,-0.262121,-6.1E-05],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],vmax,fine,Greifer1\WObj:=wobjKAMERA;
  ENDPROC
  
  PROC Saegen()
    WaitTime 0;
    Greifer1.tframe.rot:=[1,0,0,0];  !?


    ! Vision Triggerpunkt anfahren/verschleifen; innerhalb der Zone wird automatisch das Flag
    ! zum Starten des Vision-Subsystems gesetzt
    ! ACHTUNG: unbedingt Geschwindigkeit und Zone den Gegebenheiten anpassen !!
    MoveLDO rtVSTrigger,vmax,z50,Greifer1\WObj:=wobj0, doVisionActive,1;
    ! ggfls. MoveJDO verwenden


    ! Das Anfahren von HomePos kann man sich u.U. sparen wenn am Triggerpunkt verschliffen
    ! direkt zum Sägen gefahren wird; hängt von Aufbau/Gegebenheiten ab!
    HomePos False;
    
    IF (TeileNr>=1) and (TeileNr<=CTeilMax) THEN
      ! hier die ggfls. zu verwendenden Verschiebung berücksichtigen
      PDispSet [[DX,DY,0],[1,0,0,0]];
      %CSaege_Procs{TeileNr}%;
      PDispOff;
    ENDIF
  ENDPROC


  PROC haupt()
    Reset doVisionActive;
    ! Nach Home; Greifer auf
    HomePos True;


    ! warten bis Vision initialisiert ist; nur einmal beim Programmstart
    WaitUntil (nVisionStatus <> CVS_NOTINIT);
    ! Vision triggern
    Set doVisionActive;


    WHILE True DO
      ! Warten bis Vision fertig
      WaitDO doVisionActive, 0;
      !
      IF nVisionStatus = CVS_RESULTOK THEN
        ! wenn Ergebnis des Vision Systems gültig ist, dann Teil anfahren, greifen und sägen
        GreifposAnfahren XWERT, YWERT, ZROT+30;
        TeilAnheben XWERT, YWERT;
        ! Vision wird erneut beim Verlassen des Sichtfeldes getriggert (siehe Proc "Saegen")
        Saegen;
        HomePos True;
      ELSE
        ! wenn Vision Result nicht OK, dann erneut triggern
        Set doVisionActive;
      ENDIF
    ENDWHILE
  ENDPROC