Gelöst - KUKA KRC2 FTC-Sensor von Schunk via DeviceNet ohne TP FTCtrl ansprechen

[matze77]

Hallo werte Gemeinde!

Ich bin dabei den Kraft-Momentensensor FTC 50-80-V-Device 322305 der Firma Schunk ohne das Technologiepaket (TP) Force-Torque-Control (FTCtlr) in Gang zu bringen. Der Sensor ist via DeviceNet an der MFC angeschlossen und funktionierte auch schon mit FTCtrl v1.3. Nun benutze ich KSS 5.4 und die FTCtrl v1.3 funktioniert nicht mehr. Deshalb möchte ich trotzdem nicht auf die Sensordaten verzichten und plane folgendes:

1. DeviceNet-Daten auf die IOs der Steuerung umleiten, also IOSYS.INI bearbeiten

2. in RSI mit Hilfe von ST_DIGIN die Sensordaten an den IOs der Steuerung abgreifen

3. und dann mit Ethernet RSI XML an einen externen PC senden.

Schon bei 1. tue ich mir sehr schwer. In der DevNet.ini steht als MACID die "5", keine Ahnung, ob das stimmt. Ich werd wohl den Sensor aufschrauben und nach den DIP-Schaltern schauen müssen. Der Sensor soll folgendermaßen arbeiten:

Daten Kommandos des F-T-Sensors Firma Schunk:

Kräfte & Momente als Integer senden

dazu Befehl an Sensor:
1x8 Bit Wert (INT8) "76"

Antwort vom Sensor:
1x8 Bit Wert (INT8) "77"
6x16 Bit Werte (INT16) Fx,Fy,Fz,Mx,My,Mz
1x16 Bit Statuswort

Bemerkung
Umrechnung INT ! [N] : /32
Umrechnung INT ! [Nm] : /1024

Dazu habe ich folgendes in die IOSYS.INI geschrieben:

IOSYS.INI
=========
[DRIVERS]
RSI=50,rsiLibInit,rsiLib.o
MFC=0,mfcEntry,mfcdrv.o
DEVNET=2,dnInit,dn2drv.o
ERX=58,rsiCOROBInit,ethRSIXML.o
...

[DEVNET]
INW1=5,0,x6          ;5 soll hier die MACID sein, x6 - 6x16bit
OUTW1=5,0,x1       ;x1 - 1x16bit

**************************
edit: Also richtig muss es heißen ...

INW0=5,1,x7
OUTB0=5,0,x1

x7, weil das Statuswort auch berücksichtigt werden soll.
Warum richtig? Siehe unten nächster Beitrag ...
**************************

KUKA sagt dazu gar nichts! Was meint ihr dazu? Ist das so richtig?

In meinem KRL-Programm soll dann folgendes stehen:

;digitaler Output um dem Sensor den Befehl "76" zu schicken, so das der dann seine Daten zurückschickt
SIGNAL FTCommand $OUT[33]  TO  $OUT[48]
edit: falsch! stattdessen
SIGNAL FTCommand $OUT[1]  TO  $OUT[8]
Warum? Siehe unten nächster Beitrag ...

;digitale Inputs
SIGNAL FTCValFx $IN[33]  TO  $IN[48]
SIGNAL FTCValFy $IN[49]  TO  $IN[64]
SIGNAL FTCValFz $IN[65]  TO  $IN[80]
SIGNAL FTCValMx $IN[81]  TO  $IN[96]
SIGNAL FTCValMy $IN[97]  TO  $IN[112]
SIGNAL FTCValMz $IN[113]  TO  $IN[128]
edit: diese 6 Deklarationen sind absolut unnötig. Warum? Siehe unten 3. Beitrag ...

FTCommand = 122      ;Sensor nullen oder auch kalibrieren

WAIT 5 SEC                 ;5 Sekunden warten, damt genullt werden kann

FTCommand = 76        ;Sensor auffordern Kräfte & Momente (INT) auszuspucken

err=ST_DIGIN(...)        ;digitale Inputs in RSI auslesen, siehe unten 3. Beitrag ...

Naja, ihr seht schon ich hab nur n groben Plan, aber mir fehlt einfach das Detailwissen. KUKA schrieb folgendes:
Sehr geehrter Herr ...,

Ich würde laut Beschreibung:

1. An Sensor Befehl                76 senden über $OUT[...]
2. Auf Antwort, Antwort                77 warten über $IN[...]

3. Werte an $IN[...] als INT32 abgreifen über ST_DIGIN()

Die Dokumentation der einzelnen Objekte und deren Bedeutung ist im File rsiCommands.chm zu finden.

DOKU:

Description:
Creates a RSI object to access the digital inputs $IN[].

Declaration:
GLOBAL DEFFCT RSIERR ST_DIGIN(OBJID:OUT,CONTID:IN,INIDX:IN,LEN:IN,UNIT:IN)

Parameters:
OUT OBJID (INT): ID of the created RSI object
IN CONTID (INT): ID of the container for the new object
IN INIDX (INT): Offset of digitals input to read
-> in Bit for LEN 0 (Bit) >=1
-> in Byte for LEN 1,2,3 (Byte, Word) >=1
IN LEN (INT): Range of bits to read
-> 0: Bit
-> 1: Byte
-> 2: Word unsigned
-> 3: Word signed
IN UNIT (INT): Unit of input signal (unused for Bits)

Object Parameters:
1 INIDX (INT): s. parameter INIDX

Object Inputs:
No

Object Outputs:
1 (INT): Digital input value at $IN[INIDX]

-------
BSP: $IN[1-32]
...=ST_DIGIN(irgendwas,0,1,3,rsiunit_no)
-------

Jetzt die Frage, wie Sie aus zwei 16 bit Werten einen 32Bit wert machen:

LSB & MSB*2^16 = 32Bit Wert

Bitte die INT32 noch laut Doku mit INT32*1/32 b.z.w. !/1024 umrechen.

Naja, das ist hilfreich, aber nicht viel. Ich weiß nicht, wie ich mit ST_DIGIN die Sensordaten an den Inputs abgreifen soll. Das Beispiel von KUKA ist für mich nicht wirklich brauchbar. Viel zu knapp!

Wäre schön, wenn ihr mir Schritt für Schritt helfen würdet! Also erst mal klären was in der IOSYS.INI zu stehen hat, damit die Daten von DeviceNet richtig auf die IOs der Steuerung umgeleitet werden.

Gruß, Matze

[matze77]

Hatte heute nen Wissensdurchbruch und konnte herausfinden was die Anweisungen in der IOSYS.INI bedeuten. Speziell in der Rubrik [DEVNET] und SyntaxForm 2.

Was INB, INW, INWD bedeutet ist schnell klar und auch aus der Kuka Doku erkennbar.

INB   -  ein Bereich von 8 Bits (1 B=Byte) wird mit den Inputs der Steuerung verknüpft
INW  -  ein Bereich von 16 Bits (1 W=Word = 2 Bytes) ...
INDW - ein Bereich von 32 Bits (2 Word = 4 Bytes, D=Double) ...

Aber was bedeuet zum Beispiel: INW4 = 5,2,x4  ??? Auf jeden Fall wird hier  SyntaxForm 2 verwendet.

Es kann ja durchaus vorkommen, dass man nicht nur ein Gerät am Feldbus angschlossen hat, sondern auch 2 oder 3. Dann muss man dafür Sorge tragen, dass die Daten, welche diese Geräte an die Steuerung senden auch alle Platz an den Inputs selbiger finden. Ein Beispiel:

Gerät 1 (MACID=2) sendet 1 x 8 Bit, 3 x 16 Bit und Gerät 2 (MACID=5) sendet 1 x 16 Bit und 2 x 32 Bit. Für Gerät 1 kann dann folgendes in der IOSYS.INI stehen:

INW0 = 2,1,x3

Es werden die Inputs 1 - 48 (",x3" -> 3x16Bit) mit dem Signal von Gerät 1 verknüpft. Aber was ist mit den ersten 8 Bit? Tja da kommt die ",1 " mit ins Spiel. Die sagt nämlich, dass das erste Byte, also die ersten 8 Bit des Signals verworfen werden und nur der Rest (3 x 16 Bit) gelesen werden sollen. Wenn dort eine 2 stehen würde, dann würden vom Signal die ersten 16 Bit ignoriert werden. Wozu braucht man das? Damit lassen sich uninteressante Informationen der Sensoren rausfiltern und es bleiben mehr Inputs für andere Geräte frei.

Nun zu Gerät 2. Auch hier benötigen wir nicht alle Daten und schreiben:

INDW6 = 5,2,x2

Jetzt werden die Inputs 49 - 112 mit den 2 x 32 Bit Infos des Geräts 2 verknüpft. Die ersten 16 Bit des Signals werden mit ",2" ignoriert. Und nun kommts: Die "6" bedeutet hier, dass die Infos des Geräts 2 nicht bei Input 1 beginnend verknüpft werden sollen, sondern erst ab einem Abstand von 48 Bit. Also 6 x 1 Byte bzw. 6 x 8 Bit entfernt von Input 1. Würde man hier statt der "6" wieder eine "0" schreiben, dann käme es zu einem Konflikt bei der Inputverknüpfung, da sich dann die 2 Signale überschneiden würden.

Was die 2 und die 5 gleich nach dem "=" bedeuten ist jetzt wohl auch klar. Sie entsprechen den MACIDs (Adressen) der beiden Geräte 1 & 2 und müssen korrekt angegeben werden, denn sonst kann die Steuerung keine Daten von den Geräten empfangen, weil sie an falscher Stelle auf Daten warten würde. In der DEVNET.INI
werden diese Nummern eingetragen. Zum Beispiel so:

[KRC]
debug=0
baudrate=500

[1]
macid=2 ;Gerät 1

[2]
macid=5 ;Gerät 2

[3]
macid=3

usw. ...

Im KRL - Programm kann man dann sehr bequem auf diese Inputs zugreifen. Zum Beispiel auf die 2 x 32 Bits des Geräts 2:

SIGNAL Wert1 $IN[49]  TO  $IN[80]
SIGNAL Wert2 $IN[81]  TO  $IN[112]

In den Integervariablen Wert1 und Wert2 stehen dann empfangenen die Infos von Gerät 2 in Binärform, also "0" und "1".
edit: Ich denke das stimmt so nicht. Habs nicht ausprobiert, aber es sollten ganzzahlige dezimale Integerwerte in den beiden Variablen stehen, also zB. 25, 415 oder 9 ...

Was für die Inputs gilt, das lässt sich auch ganz analog auf die Outputs anwenden.

Die IOSYS.INI und die DEVNET.INI befinden sich in im Verzeichnis (Ordner): C:\KRC\ROBOTER\INIT\

So, ich hoffe dass meine Beschreibungen gut verständlich sind und andere Anwender damit schneller ans Ziel kommen als ich.

Meine nächsten Ziele sind nun das Auslesen der Inputs mit ST_DIGIN unter RSI und das Versenden der Werte via Ethernet RSI XML an den externen PC. Natürlich würde ich mich über jede Hilfe sehr freuen.

Gruß, Matze

[matze77]

So, es ist vollbracht! Über das RSI Objekt ST_DIGIN werden nun die Sensordaten an den Inputs der Steuerung abgegriffen und per Ethernet RSI XML an das externe System gesendet.

KRL Code der SRC-Datei:

Code:

DECL AXIS HOME

SIGNAL FTCommand $OUT[1]  TO  $OUT[8]

INI

HOME       = {AXIS:  A1 0,A2  -90,A3  90,A4   0,A5   0,A6   0}
PTP HOME

FTCommand = 122     ;FTSensor nullen
WAIT SEC 5
FTCommand = 76      ;Sensor sendet Fxyz, Mxyz als 6x16Bit Integer


;FOLD Create RSI Object ST_Ethernet
; Create RSI Object ST_Ethernet, read object configuration .../INIT/ERXConfig.xml
err = ST_ETHERNET(hEthernet,0,"ERXconfig.xml")
IF (err <> #RSIOK) THEN
  HALT
ENDIF
; err = ST_SETPARAM(hEthernet,eERXmaxLatePackages,1) ; after "value" to late packages the robot stopps
; err = ST_SETPARAM(hEthernet,eERXmaxLateInPercent,10) ; RSIWARNING if the limit reached
; err = ST_SETPARAM(hEthernet,eERXmaxFieldOfView,1000) ;reset every 'value' statistics.
; err = ST_SETPARAM(hEthernet, eERXFastCycle, 1) ; FALSE: Time to answer 11ms / TRUE: Fast cycle: answer <2ms necessary!
;ENDFOLD
 
;FOLD RSI-Objects to link in ST_Ethernet

; read $IN[1-16]
err = ST_DIGIN(hDin1,0,1,3,rsiunit_no)
IF (err <> #RSIOK) THEN
  HALT
ENDIF

; read $IN[17-32]
err = ST_DIGIN(hDin2,0,3,3,rsiunit_no)
IF (err <> #RSIOK) THEN
  HALT
ENDIF

; read $IN[33-48]
err = ST_DIGIN(hDin3,0,5,3,rsiunit_no)
IF (err <> #RSIOK) THEN
  HALT
ENDIF

; read $IN[49-64]
err = ST_DIGIN(hDin4,0,7,3,rsiunit_no)
IF (err <> #RSIOK) THEN
  HALT
ENDIF

; read $IN[65-80]
err = ST_DIGIN(hDin5,0,9,3,rsiunit_no)
IF (err <> #RSIOK) THEN
  HALT
ENDIF

; read $IN[81-96]
err = ST_DIGIN(hDin6,0,11,3,rsiunit_no)
IF (err <> #RSIOK) THEN
  HALT
ENDIF

; read $IN[97-112]
err = ST_DIGIN(hDin7,0,13,3,rsiunit_no)
IF (err <> #RSIOK) THEN
  HALT
ENDIF

err = ST_NEWLINK_IN(hDin1,1,hEthernet,1,"FTSensor.Fx")
IF (err <> #RSIOK) THEN
 HALT
ENDIF

err = ST_NEWLINK_IN(hDin2,1,hEthernet,2,"FTSensor.Fy")
IF (err <> #RSIOK) THEN
 HALT
ENDIF

err = ST_NEWLINK_IN(hDin3,1,hEthernet,3,"FTSensor.Fz")
IF (err <> #RSIOK) THEN
 HALT
ENDIF

err = ST_NEWLINK_IN(hDin4,1,hEthernet,4,"FTSensor.Mx")
IF (err <> #RSIOK) THEN
 HALT
ENDIF

err = ST_NEWLINK_IN(hDin5,1,hEthernet,5,"FTSensor.My")
IF (err <> #RSIOK) THEN
 HALT
ENDIF

err = ST_NEWLINK_IN(hDin6,1,hEthernet,6,"FTSensor.Mz")
IF (err <> #RSIOK) THEN
 HALT
ENDIF

err = ST_NEWLINK_IN(hDin7,1,hEthernet,7,"FTSensor.Status")
IF (err <> #RSIOK) THEN
 HALT
ENDIF
;ENDFOLD

;FOLD RSI-Objects to link out of ST_Ethernet
; link RKorr to correction on path
err = ST_PATHCORR(hPath,0)
IF (err <> #RSIOK) THEN
  HALT
ENDIF

err = ST_SETPARAM(hPath,1,-100)   ; max. neg. Korr. in X [mm]
err = ST_SETPARAM(hPath,7,+100)   ; max. pos. Korr. in X [mm]
err = ST_SETPARAM(hPath,4, -50)   ; max. neg. Korr. A [grad]
err = ST_SETPARAM(hPath,10,+50)   ; max. pos. Korr. A [grad]
err = ST_SETPARAM(hPath,6, -50)   ; max. neg. Korr. C [grad]
err = ST_SETPARAM(hPath,12,+50)   ; max. pos. Korr. C [grad]

err = ST_NEWLINK_OUT(hEthernet,1,hPath,1,"RKorr.X")
IF (err <> #RSIOK) THEN
  HALT
ENDIF
err = ST_NEWLINK_OUT(hEthernet,2,hPath,2,"RKorr.Y")
IF (err <> #RSIOK) THEN
  HALT
ENDIF
err = ST_NEWLINK_OUT(hEthernet,3,hPath,3,"RKorr.Z")
IF (err <> #RSIOK) THEN
  HALT
ENDIF
err = ST_NEWLINK_OUT(hEthernet,4,hPath,4,"RKorr.A")
IF (err <> #RSIOK) THEN
  HALT
ENDIF
err = ST_NEWLINK_OUT(hEthernet,5,hPath,5,"RKorr.B")
IF (err <> #RSIOK) THEN
  HALT
ENDIF
err = ST_NEWLINK_OUT(hEthernet,6,hPath,6,"RKorr.C")
IF (err <> #RSIOK) THEN
  HALT
ENDIF
;ENDFOLD

;err = ST_ON1(#BASE,1)
err = ST_ON1(#TCP,1)
;err = ST_ON1(#WORLD,1)
IF (err <> #RSIOK) THEN
  HALT
ENDIF


; *=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=**=*=*=*=*=*=*=*=*=*=*
ST_SKIPSENS() ;Hold on - until RSI-Break reason occur
; *=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=**=*=*=*=*=*=*=*=*=*=*

PTP HOME
END

KRL Code der DAT-Datei (Auszug):

Code:

DECL RSIERR err
INT hEthernet
INT hDin1,hDin2,hDin3,hDin4,hDin5,hDin6,hDin7
INT UNIT_RSI

ERXConfig.xml:

Code:

<ROOT>
   <CONFIG>
      <IP_NUMBER>192.168.10.20</IP_NUMBER>    <!-- IP Number of the socket !-->
      <PORT>6008</PORT>                   <!-- Port Number of the socket !-->
      <PROTOCOL>TCP</PROTOCOL>            <!-- TCP or UDP, Protocol of the socket !-->
      <SENTYPE>ImFree</SENTYPE>           <!-- The name of your system send in <Sen Type="" > !-->
      <PROTCOLLENGTH>Off</PROTCOLLENGTH>  <!-- On or Off, Send the length of data in bytes before XML data begins!-->
      <ONLYSEND>FALSE</ONLYSEND>          <!-- TRUE means the client don't expect a answer. Do not send anything to robot!-->
   </CONFIG>
   <!-- RSI Data: TYPE=  "BOOL", "STRING", "LONG", "FLOAT", "DOUBLE" !-->
   <!-- INDX= "INTERNAL" switch on internal read values. Needed by DEF_...!-->
   <!-- INDX= "nmb" Input/Output index of RSI-Object / Maximum of RSI Channels: 64 !-->
   <!-- UNIT: The same like RSI Units, insert a decimal value. !-->
   <!-- HOLDON="1", set this output index of RSI Object to the last value  !-->   
   <!-- DEF_Delay count the late packages and send it back to server  !-->
   <!-- DEF_Tech: .C = advance .T = main run / .C1 advance set function generator 1 !-->
   
   <SEND>
      <ELEMENTS>
         <ELEMENT TAG="DEF_RIst" TYPE="DOUBLE" INDX="INTERNAL" UNIT="0" />
         <ELEMENT TAG="DEF_AIPos" TYPE="DOUBLE" INDX="INTERNAL" UNIT="0" />
         <ELEMENT TAG="FTSensor.Fx" TYPE="LONG" INDX="1" UNIT="0" />
         <ELEMENT TAG="FTSensor.Fy" TYPE="LONG" INDX="2" UNIT="0" />
         <ELEMENT TAG="FTSensor.Fz" TYPE="LONG" INDX="3" UNIT="0" />
         <ELEMENT TAG="FTSensor.Mx" TYPE="LONG" INDX="4" UNIT="0" />
         <ELEMENT TAG="FTSensor.My" TYPE="LONG" INDX="5" UNIT="0" />
         <ELEMENT TAG="FTSensor.Mz" TYPE="LONG" INDX="6" UNIT="0" />
         <ELEMENT TAG="FTSensor.Status" TYPE="LONG" INDX="7" UNIT="0" />
         <ELEMENT TAG="DEF_Delay" TYPE="LONG" INDX="INTERNAL" UNIT="0" />
      </ELEMENTS>
   </SEND>
   <RECEIVE>
      <ELEMENTS>
         <ELEMENT TAG="DEF_EStr" TYPE="STRING" INDX="INTERNAL" UNIT="0" />
         <ELEMENT TAG="RKorr.X" TYPE="DOUBLE" INDX="1" UNIT="1" HOLDON="1" />
         <ELEMENT TAG="RKorr.Y" TYPE="DOUBLE" INDX="2" UNIT="1" HOLDON="1" />
         <ELEMENT TAG="RKorr.Z" TYPE="DOUBLE" INDX="3" UNIT="1" HOLDON="1" />
         <ELEMENT TAG="RKorr.A" TYPE="DOUBLE" INDX="4" UNIT="0" HOLDON="1" />
         <ELEMENT TAG="RKorr.B" TYPE="DOUBLE" INDX="5" UNIT="0" HOLDON="1" />
         <ELEMENT TAG="RKorr.C" TYPE="DOUBLE" INDX="6" UNIT="0" HOLDON="1" />
         <ELEMENT TAG="AKorr.A1" TYPE="DOUBLE" INDX="7" UNIT="0" HOLDON="0" />
         <ELEMENT TAG="AKorr.A2" TYPE="DOUBLE" INDX="8" UNIT="0" HOLDON="0" />
         <ELEMENT TAG="AKorr.A3" TYPE="DOUBLE" INDX="9" UNIT="0" HOLDON="0" />
         <ELEMENT TAG="AKorr.A4" TYPE="DOUBLE" INDX="10" UNIT="0" HOLDON="0" />
         <ELEMENT TAG="AKorr.A5" TYPE="DOUBLE" INDX="11" UNIT="0" HOLDON="0" />
         <ELEMENT TAG="AKorr.A6" TYPE="DOUBLE" INDX="12" UNIT="0" HOLDON="0" />
      </ELEMENTS>
   </RECEIVE>
</ROOT>

DEVNET.INI:

Code:

[krc]
debug=0
baudrate=500

[1]
macid=5

IOSYS.INI (Auszug):

Code:

[DEVNET]
INW0=5,1,x7
OUTB0=5,0,x1

Der Sensor sendet 7 x 16Bit Integerwerte. ST_DIGIN kann maximal 16 Inputs auf einmal auslesen, also reichts grad so mit einem ST_DIGIN Objekt auch genau einen Wert des Sensors von den Inputs der Steuerung abzugreifen. Deswegen sind im KRL Code auch 7 ST_DIGIN Objekte erstellt worden. Diese werden dann mit ST_Ethernet verknüpft (nicht die Einstellungen in der ERXConfig.xml vergessen) und somit die Sensorwerte an das externe System gesendet.

Noch ein kleiner Hinweis zu ST_DIGIN:

; read $IN[1-16]
err = ST_DIGIN(hDin1,0,1,3,rsiunit_no)
-----------------------*
Der 3. Parameter ",1" gibt den Byteabstand an, mit dem ST_DIGIN an den Inputs was abgreifen soll. Eine "0" wäre hier bestimmt sinvoller gewesen, aber KUKA machts anders, warum auch immer. Es werden also mit Parameter 3 die ersten 16 Inputs der Steuerung ausgelesen.

; read $IN[17-32]
err = ST_DIGIN(hDin2,0,3,3,rsiunit_no)

Um die nächsten 16 Inputs auszulesen muss nun der Byteabstand vergrößert werden. Die ersten 16 Inputs sind gelesen und müssen sozusagen übergangen werden. Statt der ",1" wird nun ",3" als Parameter 3 verwendet. 3 - 1 = 2 und damit 2 x 8 Bit = 16 Bit Abstand zum Input [1]. Es werden nun die nächsten 16 Inputs angefangen von Input 17 bis 32 ausgelesen.

Was die anderen Parameter von ST_DIGIN bedeuten ist leicht aus der KUKA-DOKU zu erkennen, siehe 1. Beitrag "KUKA schrieb:"

Ok, vielen Danke für Eure Hilfe!

Matze

[Robotnik]

Hi Matze, ich darfs nun endlich auch tun - RsiXml - FT.

  für den Beitrag

Bin mal gespannt ob bei mir auch  funzt