ZAKŁAD MECHANIKI PŁYNÓW I AERODYNAMIKI 

LABORATORIUM ROBOTYZACJI 

 
 

 
 
 
 
 
 
 
 

Programowanie kontrolera RH robota 

S-420S 

Opracował: Karol Szostek 

 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Rzeszów 2008 

1. Cel ćwiczenia

 

 

Celem  ćwiczenia jest zapoznanie się z budową robota S-420S obsługą sterownika oraz 

nauka podstaw programowania sterownika RH. 
 

2. Konfiguracja robota S-420S  
 

Robot S-420S pokazany na rysunku 2.1 (Mechanical unit) sterowany jest z 

wykorzystaniem sterownika RH (Control unit). Sterownik wyposażony jest w panel 
sterowania CRT/KB zawierając monitor i klawiaturę. Do sterownika podłączony jest za 
pomocą przewodu ręczny programator TeachPendent. 

 

Rysunek 2.1. Konfiguracja robota S-420S. 

 

Robot S-420S posiada 6 serwonapędów sterujących jego pracą. Trzy z napędów są 

wykorzystywane do wykonywania ruchów wokół osi 1,2,3 w wyniku których możliwe jest 
przemieszczanie końcówki ramienia robota w układzie współrzędnych X,Y,Z ja na rysunku 
2.2. Trzy pozostałe napędy wykorzystane są do obrotów wokół osi 4,5,6 dzięki którym 
możliwa jest zmiana ustawienia narzędzia mocowanego na końcówce ramienia.     

 

Rysunek 2.2. Osie obrotów robota S-420S. 

 

Sterownik RH zawiera komputer z systemem operacyjnym. Płyta główna komputera, 

moduły  śledzenia  ścieżki ruchu, moduł pamięci oraz moduł kontroli serwonapędów 
umocowane są na panelu Backpłane pokazanym na rysunku 2.3. W sterowniku RH 
umieszczone są karty rozszerzeń wejscia/wyjscia I/O wykorzystywane miedz innymi do 
komunikacji w sieci przekazywania stanu linii produkcyjnej itp. Robot posiada zamontowane 
cztery karty I/O mające w sumie 32 wejścia i 32 wyjścia cyfrowych. W sterowniku 
wbudowane są również wzmacniacze serwonapędów Servo amp. 1- Servo amp. 6. co jest 
pokazane na rysunku 2.3.  

 

 

Rysunek 2.3. Sterownik RH.

 

 
 Dokładny opis budowy robota oraz sterownika znajduje się w dokumentacji [1]. 
 

3. Programowanie kontrolera 
 

 

Programy kontrolera RH pisane są w specyficznym języku i zapisywane w plikach z 

rozszerzeniem *.KL. Przed wykonaniem programu konieczna jest jego translacja do kodu 

wykonywalnego. Po kompilacji pliki z kodem wykonywalnym zapisywane są w plikach z 
rozszerzeniem *.PC. Podczas translacji oprócz pliku wykonywalnego tworzony jest plik z 
parametrami programu o rozszerzeniu *.VR. 
 
 

Błędy systemowe występujące podczas pracy sterownika są wyświetlane w lewym 

górnym rogu monitora panelu CRT/KB. 
 
 Dokładny opis składni języka, obsługi panelu CRT/KB, komend wiersza poleceń 
KCL, zmiennych systemowych, kodów błędów można znaleźć w dokumentacji Fanuc 
Robotics [2]. 
 
 Istnieje 

możliwość przesyłania plików pomiędzy sterownikiem a komputerem PC 

poprzez łącze szeregowe RS 232. W cel skopiowania programów należy mieć odpowiednio 
skonfigurowane łącze RS 232, na komputera PC należy uruchomić emulator stacji dyskietek 
KFloppy.exe w programie należy ustawić katalog roboczy i włączyć emulację dyskietki. Pliki 
kopiuje się wydając komendę cop w wierszu poleceń KCL sterownika RH albo 
wykorzystując program kopiujący File/Utility menu głównego systemu KAREL np.: 
Polecenie KCL>cop/o bm:DOCKING.KL fd: skopiuje plik DOCKING.KL z pamięci 
sterownika na dyskietkę. 
 

 
Przykładowy program DOCKING.KL przesuwający ramię robota z pozycji aktualnej 

do pozycji zadanej pokazany jest w Tabela 3.1. Pozycja zadana zapamiętana jest w zmiennej 
baza. W celu poprawnego działania programu wartość zmiennej baza należy wprowadzić do 
programu za pomocą programatora TeachPendent. Zmienna baza jest zapamiętywana w pliku 
DOCKIND.VR. 
 

PROGRAM docking 
--przesuwanie ramienia do pozycji baza 
VAR 
  baza: POSITION 
BEGIN 
  $SPEED=200 
  $MOTYPE=LINEAR 
  MOVE TO baza 
END docking

 

Tabela 3.1 Program przesuwający ramię robota do pozycji baza po prostej. 

 
 Dokładne informacje na temat wykorzystywania programatora TeachPendent oraz 
podstawy programowania można znaleźć w dokumentacji [1].    
 

 

 

W tabeli 3.2 zamieszczony jest program rysujący zygzak oraz kółko na płaszczyźnie 

XY. W programie tym wszystkie pozycje wyliczane są na podstawie pozycji początkowej 
ramienia robota i do jego poprawnego działania nie jest konieczne wprowadzanie żadnych 
danych z wykorzystaniem programatora TeachPendent. 
 

PROGRAM zygzak 
--programy rysujący na płaszczyźnie XY  
VAR 
   i: INTEGER 
   punkt: POSITION 
   pocz: POSITION 
   x: real 
   y: real 
   z: real 
   w: real 

   p: real 
   r: real 
   A: POSITION 
   B: POSITION 
   C: POSITION 
   D: POSITION 
   config: string[100] 
BEGIN 
 
 WRITE(cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr) 
 WRITE('program zygzak',cr) 
 WRITE TPMENU('program zygzak                                    ',cr) 
 
 $MOTYPE=LINEAR  
 $SPEED=200     
  
 pocz=CURPOS 
 unpos(CURPOS,x,y,z,w,p,r,config) 
 WRITE('x=',x,cr) 
 WRITE('y=',y,cr) 
 WRITE('z=',z,cr) 
 WRITE('w=',w,cr) 
 WRITE('p=',p,cr) 
 WRITE('r=',r,cr) 
 
 z=z+20 
 punkt=POS(x,y,z,w,p,r,config) 
 MOVE TO punkt 
 x=x+30 
 y=y-30 
 punkt=POS(x,y,z,w,p,r,config) 
 MOVE TO punkt 
 
 z=z-25 
 punkt=POS(x,y,z,w,p,r,config) 
 MOVE TO punkt 
 FOR i=1 TO 3 DO 
  x=x+20 
  y=y-20 
  punkt=POS(x,y,z,w,p,r,config) 
  MOVE TO punkt 
  x=x-20 
  y=y-20 
  punkt=POS(x,y,z,w,p,r,config) 
  MOVE TO punkt 
 ENDFOR 
 
 A=CURPOS 
 B=POS(x+40,y-40,z,w,p,r,config) 
 C=POS(x,y-80,z,w,p,r,config) 
 D=POS(x-40,y-40,z,w,p,r,config) 
  
 WITH $MOTYPE=CIRCULAR MOVE TO C VIA B 
 WITH $MOTYPE=CIRCULAR MOVE TO A VIA D 
 
 $MOTYPE=LINEAR  
 z=z+5 
 punkt=POS(x,y,z,w,p,r,config) 
 MOVE TO punkt 
 MOVE TO pocz 
END zygzak 

Tabela 3.2 Program rysujący na płaszczyźnie XY. 

 
 

W tabeli 3.3 zamieszczony jest program który może zostać wykorzystany do 

przestawiania np. butelki pomiędzy dwoma położeniami, do poprawnego działania programu 
konieczne jest wprowadzenie danych pozycyjnych w ścieżkach: scieA, scieB, scieC z 
wykorzystaniem programatora TeachPendet. 
 

PROGRAM butelka 
--programy przestawianie przedmiotu 
VAR 
   scieA: PATH 
   scieB: PATH 
   scieC: PATH 
BEGIN 
 
 WRITE(cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr,cr) 
 WRITE('program przestawianie przedmiotu',cr) 
 WRITE TPMENU('program przestawianie przemiotu                         ',cr) 
 
 $SPEED=500 
    
OPEN HAND 1 
MOVE ALONG scieA 
CLOSE HAND 1 
DELAY 500 
MOVE ALONG scieB 
OPEN HAND 1 
DELAY 500 
MOVE ALONG scieC 
CLOSE HAND 1 
 
END butelka     

Tabela 3.2 Program do przestawiania przedmiotów. 

 

4. Zadania do wykonania 

 

Istnieje do wyboru zrealizowanie jednego z kilku zadań. 

Zadanie I. 

Wykonanie programu piszącego napis na płaszczyźnie XY. Zadanie to może zostać 

rozwiązane na kilka sposobów. W najprostszym przypadku wypisywany napis może być 
nierozłącznie związany z programem, w przypadku bardziej zaawansowanym sterownik po 
uruchomieniu programu powinien wypisać na ekranie panelu CRT albo na wyświetlaczu 
programatora TeachPendent prośbę o wprowadzenia z klawiatury ciągu znaków ewentualnie 
o podanie innych danych takich jak wielkość liter rodzaj czcionki itp. oraz o  potwierdzenie 
wprowadzenia danych. Sam program może  być napisany w jednym module ale w wersji 
zaawansowanej powinien składać się z kilku podprogramów programu głównego 
komunikującego się z operatorem, programu piszącego napis, a nawet podprogramów 
piszących poszczególne litery. 
Zadanie II 

Nalewanie z butelki do kubka. Zadanie nalewania z butelki do kubków można 

rozwiązać w różny dwojaki w zależności od tego czy mamy wystarczają ilość kubków do 
rozlania całej butelki czy też nie. Jeśli mamy wystarczającą liczbę kubków można od razu 
rozlać całą butelkę. W przypadku gdy liczba kubków jest mała robot po napełnieniu kubków 
powinien na monitorze wypisać prośbę o potwierdzenie wykonania napełnienia kolejnych i 
dopiero po potwierdzeniu napełnić ponownie kubki. 
Zadanie III 

 

 Wykonanie programu piszącego na dowolnej płaszczyźnie. 

Zadanie IV 
 

Wykonanie programu piszącego na zdefiniowanej powierzchni np. na sferze. 

 
5. Stanowisko laboratoryjne 

Stanowisko laboratoryjne wyposażone jest w robot S-420S sterowany sterownikiem 

RH. 

6. Przebieg ćwiczenia 
 

1.  Zapoznanie się z budową robota S-420S oraz sterownika RH. 
2.  Uruchomienie robota. Nauka obsługi panelu sterowania CRT/KB oraz manipulatora 

TeachPendent. Zapoznanie się z menu głównym systemu operacyjnego, wierszem 
poleceń KCL, zmiennymi systemowymi, elementami języka KAREL, edytorem tekstu. 

3.  Uruchomienie umieszczonych w sterowniku programów. 

 

7. Opracowanie wyników 

W opracowaniu sprawozdaniu należy umieści wykonany program oraz omówić 

wyniki z przeprowadzonych testów pracy programu. 

8. Literatura  

 

1. Dokumentacja Fanuc Robotics  Maintenance manual [MARMKS42H1174EF][B-67205E-

G01].pdf 

2. Dokumentacja Fanuc Robotics MAROKENHA0885EF - Enhanced KAREL Operations 

Manual v. 2.22 R.pdf 

3. Dokumentacja Fanuc Robotics MARSKAMSH0885EF - KAREL Reference Manual v.2.22 

REV.A..pdf