POLITECHNIKA WARSZAWSKA
WYDZIAA
ELEKTRYCZNY
PRACA PRZEJŚCIOWA
Temat: Sterowanie manipulatora Scorbot-er 4u
Autorzy: Opiekun:
Michał Macias dr inż. Dominik Sierociuk
Mariusz Rylski
Spis treści
Cel pracy.....................................................................................................................................2
1.1.1Przedstawienie manipulatora Scorbot-er 4u.......................................................................2
1.2.1Podzespoły układu sterowania............................................................................................2
Komputer .................................................................................................................................3
c.Układy odniesienia manipulatora.............................................................................................4
d.Układy napędowe manipulatora Scorbot-er 4u........................................................................5
3Programowanie manipulatora Scorbot-er 4u............................................................................5
a.Programowanie przy wykorzystaniu środowiska Robocell.....................................................6
3.2.Programowanie manipulatora z poziomu języka C++..........................................................9
3.2.1.Etapy nawiązywania połączenia między komputerem a manipulatorem........................10
3.2.2.Funkcje ruchu manipulatora w języku C++.....................................................................13
4Zauważone błędy....................................................................................................................16
Cel pracy
Celem pracy jest poznanie zasad sterowania manipulatora dydaktycznego Scorbot-er 4u
w oparciu o dostarczone przez producenta narzędzia:
" Środowisko Robocell
" pliki biblioteczne umożliwiające programowanie przy wykorzystaniu języka wysokiego
poziomu C++
1.1.1 Przedstawienie manipulatora Scorbot-er 4u
W dalszej części sprawozdania opisane zostały podstawowe wiadomość związane
z manipulatorem dydaktycznym Scorbot-er 4u. Przedstawiono komponenty sprzętowe
dostarczone wraz z manipulatorem, standardowe środowisko programowania Robocell oraz
opis czynności, które należy wykonać przed programowaniem określonych przez
użytkownika zachowań manipulatora wraz z krótkim wyjaśnieniem często używanych
funkcji.
1.2.1 Podzespoły układu sterowania
Podstawowy system Scorbot obejmuje 3 komponenty sprzętowe:
" komputer
" kontroler USB
" manipulator Scorbot-er 4u
Komputer połączony jest z Kontrolerem USB poprzez kabel USB. Manipulator natomiast
łączy się z kontrolerem-USB poprzez specjalny opatentowany przewód dostarczony przez
producenta.
Kontroler-USB stanowiący mostek pomiędzy manipulatorem a komputerem pozwala na
dołączenie dwóch dodatkowych urządzeń peryferyjnych, które manipulator Scorbot-er 4u
rozpoznaje jako oś 7 i 8  przykładem takiego urządzenia może być taśma transmisyjna
symulująca fragment linii produkcyjnej. Dodatkowo Kontroler-USB umożliwia sterowanie
silnikami manipulatora, odczytywanie stanu wejść oraz wyjść cyfrowych, odczytywanie stanu
wejść oraz wyjść analogowych, a także posiada umiejętność przetwarzania sygnałów
z enkodera i mikroswitcha. Port RS-232 może zostać wykorzystany do transmisji danych.
W razie jakiegokolwiek zagrożenia naciśnięcie przycisku awaryjnego powoduje
natychmiastowe przerwanie wykonywanej pracy.
Kontroler-USB
Komputer
Scorbot-er 4u
" Stop awaryjny
" Osie 7 i 8
" Wejścia cyfrowe
" Wyjścia cyfrowe
" Wejścia analogowe
" Wyjścia analogowe
" RS 232
rys. 1. Podzespoły układu sterowania manipulatora Scorbot-er 4u
c. Układy odniesienia manipulatora
Po wcześniejszym podłączeniu komponentów istnieje możliwość sterowania danym
manipulatorem w jego zmiennych wewnętrznych, a także w zmiennych zewnętrznych
względem układu bazowego skojarzonego z nieruchomą podstawą manipulatora.
Manipulator Scorbot-er 4u posiada 5 stopni swobody, przez stopnie swobody należy
rozumieć ilość niezależnych ruchów jakie manipulator może wykonać. Ruchy te nie mogą
być superpozycją innych przemieszczeń.
W układzie zewnętrznym pozycję punktu aktywnego chwytaka TCP (Tool Center Point)
określają wartości współrzędnych X,Y,Z a orientację chwytaka względem układu bazowego
określają wartości kątowe Pitch oraz Roll.
Pozycję oraz orientację narzędzia w układzie zmiennych wewnętrznych określają wartości
kątowe reprezentujące poszczególne przemieszczenia kątowe: osi 1 , osi 2, osi 3, osi 4, osi 5
oraz osi 6.
w zmiennych węwnętrznych
w zmiennych zewnętrznych
rys. 2.Układy odniesienia manipulatora (z lewej układ wewnętrzny, z prawej układ
zewnętrzny)
d. Układy napędowe manipulatora Scorbot-er 4u
Ruch każdego ogniwa manipulatora możliwy jest dzięki serwonapędom prądu stałego,
które umożliwiają obrót ogniwa względem określonej osi. Dodatkowo każdy silnik jest
połączony z enkoderem, którego zadaniem jest określenie położenia i ruchu każdej osi.
Przenoszenie ruchu pomiędzy ogniwami możliwe jest dzięki wykorzystaniu kół zębatych oraz
kół pasowych.
rys. 3.Umiejscowienie silników oraz budowa osi manipulatora
3 Programowanie manipulatora Scorbot-er 4u
Programowanie manipulatora Scorbot-er 4u może odbywać się poprzez dostarczony przez
producenta środowisko Robocell bądz
poprzez wykorzystanie dostępnych plików
bibliotecznych z poziomu języka wysokiego poziomu C++ .
a. Programowanie przy wykorzystaniu środowiska
Robocell
Dostarczone przez producenta wraz z manipulatorem środowisko Robocell umożliwia:
sterowanie manipulatorem w zmiennych zewnętrznych oraz wewnętrznych
" programowanie manipulatora Scorbot-er 4u
" ukazanie symulacji zaprogramowanego ruchu
" uzyskanie wartości zmiennych zewnętrznych oraz wewnętrznych manipulatora
Okno
sterowania manipulatorem w środowisku Robocell
Po uruchomieniu środowiska Robocell następuje automatyczna inicjalizacji manipulatora
ze środowiskiem programowania. Inicjalizacja jest niezbędna do nawiązania pomiędzy
manipulatorem a komputerem.
Po dokonaniu inicjalizacji należy wybrać tryb pracy manipulatora spośród 3 dostępnych:
tryp online, offline oraz symulacji. Tryb online pozwala na wykonywanie czynności przez
rzeczywisty manipulator Scorobot-er 4u. Tryb offline pozwala programowanie manipulatora
bez odzwierciedlenia ruchów na modelu rzeczywistym. Ciekawym rozwiązaniem jest
możliwość obserwacji zaprogramowanych wcześniej ruchów w oknie symulacji, należy
wówczas włączyć tryb symulacji manipulatora.
Następnym bardzo ważnym krokiem jest wykonanie bazowania manipulatora. Bez owej
czynności manipulator nie będzie znał swojego bieżącego położenia, a wszystkie wykonane
czynności manipulacyjne mogą okazać się bezużyteczne.
Opis podstawowych funkcji okien występujących w programie Robocell:
Panel sterowania (Manual Movement) umożliwia sterowanie
manipulatora w zmiennych wewnętrznych (opcja Joints) oraz w
zmiennych zewnętrznych (opcja XYZ), ale również za jego pomocą
można określić prędkość wykonywania ruchów w zakresie od 1-9.
Ustawiona za pomocą panelu Manual Movement pozycja
manipulatora może zostać następnie zapamiętana przy użyciu okna
Teach Positions. Panel ten pozwala również na określenie rodzaju
trajektorii dojścia do zapamiętywanego ruchu. Manipulator
Scorbot-er 4u pozwala na dojście do określonego punktu trzema
trajektoriami: liniową, quasiliniową oraz kołową.
Panel Robot Movement umożliwia ręczne wykonywanie ruchów
manipulatora, za jego pomocą podobnie jak panelu Manual
Movement możemy wykonywać ruchy manipulatora w odniesieniu
do układu bazowego bądz
w układzie zmiennych wewnętrznych. Ułatwieniem jest
zobrazowanie na rysunku wszystkich osi manipulatora.
Lista instrukcji zaimplementowanych w środowisku Robocell może zostać uruchomiona
na trzy sposoby:
" krokowe wykonywanie programu
" pojedyncze wykonanie całego programu
" cykliczne wykonywanie programu
3.2. Programowanie manipulatora z poziomu języka C++
Do tworzenia kodów z
ródłowych sterujących manipulatorem z poziomu języka C++
niezbędne są pliki konfiguracyjne oraz biblioteki udostępnione przez producenta.
Niezbędne do programowania manipulatora pliki to:
" biblioteka usbc.dll  powinna zostać umieszczona w folderze, w którym znajduje się kod
programu bądz
w folderze Windows
" usbc.ini  powinien znajdować się razem z plikiem usbc.dll . Plik usbc.ini jest plikiem
konfiguracyjnym przekazującym m.in. informacje o miejscu gdzie znajduje się plik
konfiguracyjny er4conf.ini zawierający parametry manipulatora Scorbot-er 4u.
" folder PAR, w którym umieszczone są pliki konfiguracyjne poszczególnych osi
manipulatora.
Do prawidłowej pracy kodu z
ródłowego z manipulatorem niezbędne są również pliki
nagłówkowe, które należy dołączyć na etapie tworzenia programu w kodzie z
ródłowym, są to
pliki:
" usbc.h, w którym znajdują się definicje dostępnych podczas programowania manipulatora
funkcji
" usbcdef.h, który przechowuje definicje stałych oraz struktur wykorzystywanych przez
bibliotekę usbc.dll
" extern.h, w którym znajduje się definicja klasy ErrorInfo
" error.h, który zawiera definicję błędów
Kolejnym niezbędnym plikiem podczas tworzenia programów w C++ jest plik usbc.lib,
który powinien znajdować się w katalogu projektu.
3.2.1. Etapy nawiązywania połączenia między komputerem
a manipulatorem.
Przed rozpoczęciem programowania właściwych ruchów konieczne jest nawiązanie
połączenia z manipulatorem. Połączenie realizowane jest poprzez funkcję Initialization
z odpowiednimi parametrami aktualnymi.
Opis parametrów formalnych funkcji Initialization:
bool Initialization(short sMode, short sSystemType, CallBackFun InitEnd, CallBackFun ErrMessage)
Parametr sMode odpowiada za ustawienie trybu pracy manipulatora. W miejsce
parametru można wstawić następujące wartości:
INIT_MODE_DEFAULT=0  parametr odwołuje się do pliku konfiguracyjnego i powoduje
włączenie ostatnio używanego trybu pracy z manipulatorem
INIT_MODE_ONLINE=1  parametr ten nawiązuje połączenie online z manipulatorem.
Parametr sSystemType odpowiada za nawiązanie połączenia z określonym typem
manipulatora. W miejsce parametru można wstawić następujące wartości:
DEFAULT_SYSTEM_TYPE=0  parametr ten odpowiada za autodetekcję, pozwala
automatycznie wykryć podłączony rodzaj manipulatora.
ER4USB_SYSTEM_TYPE=41  parametr ten odpowiada za nawiązanie połączenia
z manipulatorem Scorbot-er 4u
Ostatnie dwa parametry są funkcjami o parametrach określonych przez wskaz
nik do
funkcji CallBackFun. Typ CallBackFun jest wskaz
nikiem pokazującym na funkcję, której
deklaracja wygląda następująco: void nazwa_funkcji (void*).
Funkcja InitEnd jest wywoływana jeżeli inicjalizacja z urządzeniem przebiegła
pomyślnie. Przeciwieństwem funkcji InitEnd jest funkcja będąca jednocześnie ostatnim
parametrem inicjalizacji ErrMessage. Funkcja ta jest realizowana jeżeli przy połączeniu
z manipulatorem wystąpi błąd.
Przykład
//W przykładzie wykorzystano rzutowanie wskaz
nika typu ConfigData oraz ErrorInfo na
typ //void*. Niezbędne jest wcześniejsze dołączenie do kodu bibliotek wcześniej opisanych.
void InitEnd(ConfigData *pTheConfigData)
{
printf("Inicjalizacja...");
}
void ErrorMessage(ErrorInfo *pTheErrorInfo)
{
printf("Blad");
printf("\nError: %d ",pTheErrorInfo->lNumber);
printf("%s",pTheErrorInfo->cOptional);
error=1;
}
int main(void)
{
Initialization(INIT_MODE_ONLINE,ER4USB_SYSTEM_TYPE,(CallBackFun)&InitEnd,
(CallBackFun)&ErrorMessage);
}
Kolejnym zalecanym krokiem jest włączenie kontroli ruchu dla poszczególnych osi
manipulatora. Bez wykonania tej czynności funkcje ruchu nie będą wykonywane. Kontrola
włączana bądz
wyłączana jest za pomocą funkcji Control z odpowiednimi parametrami
aktualnymi. Parametr BisOn ustawiony jako TRUE włącza kontrolę poszczególnych osi,
natomiast ustawiony jako FALSE wyłącza sterowanie określonymi wcześniej osiami.
Opis parametrów formalnych funkcji Control:
bool Control(unsigned char ucGroupAxis, bool BisOn)
Parametr ucGroupAxis odpowiada za wybranie określonych osi, które następnie zostaną
włączone bądz
wyłączone. Parametr ucGroupAxis może przyjmować następujące wartości:
 A  włącza sterowanie wszystkimi osiami manipulatora (bez osi urządzeń peryferyjnych 
7 i 8).
 &  włącza sterowanie wszystkimi osiami manipulatora, włącznie z urządzeniami
peryferyjnymi.
 B  włącza sterowanie osiami odpowiadającymi za urządzenia peryferyjne (oś 7 oraz 8).
Przykład
//Włączenie sterowania wszystkich osi.
Control('&',TRUE)
Pomimo nawiązania połączenia z manipulatorem oraz zezwolenia na określanie ruchów
tylko niektóre funkcji ruchowe będą wykonywane. Do prawidłowego poruszania
manipulatorem konieczne jest również bazowanie osi. Manipulator po bazowaniu jest
w stanie określić swoją aktualną pozycję zarówno w zmiennych zewnętrznych jak
i wewnętrznych.
Za ustawienie manipulatora w pozycji bazowej odpowiada funkcja Home wraz
z odpowiednimi parametrami aktualnymi.
Opis parametrów formalnych funkcji Home:
bool Home(unsigned char ucGroupAxis,CallBackFun fnHomingNotif)
Pierwszy parametr (ucGroupAxis) odpowiada za wybór osi, które mają zostać ustawione
w pozycji bazowej. Parametr ten może przyjmować następujące wartości:
 A  bazowaniu zostaną poddane wszystkie osie manipulatora
 B  bazowaniu zostaną poddane osie przypisane urządzeniom peryferyjnym (7 oraz 8)
 0 - 7  bazowaniu zostaną poddane pojedyncze osie.
Należy zauważyć, iż w języku C++ pojedyncze osie numerowane są począwszy od 0.
Funkcja fnHomigNotif wywoływana jest przy rozpoczyniu bazowania i konczenia każdej
osi. Parametr ten nie jest niezbędny do prawidłowego funkcjonowania procesu bazowania,
może więc zostać ustawiony jako NULL.
Poniżej przedstawione są przykłady właściwego bazowania manipulatora Scorbot-er 4u.
W przypadku bazowania manipulatora w oparciu o pojedyncze osie, należy określić
odpowiednią kolejność, ponieważ niektóre osie są ze sobą sprzęgnięte. Prawidłowa kolejność
bazowania osi jest następująca: 1,2,4,3,0,5. Nieuwzględnie prawidłowej kolejności przy
bazowaniu pojedynczymi osiami powoduje niewłaściwe ustawienia manipulatora.
Przykład
//Bazowanie osi manipulatora
Home('A',NULL)
//Bazowanie manipulatora przy wykorzystaniu poszczególnych osi  0 - 5
int bazowanie[]={1,2,4,3,0,5};
for (int i=0; i<=5; i++)
{
Home(bazowanie[i],NULL);
}
3.2.2. Funkcje ruchu manipulatora w języku C++
Po nawiązaniu połączenia z manipulatorem i zbazowaniu jego osi można przystąpić do
programowania określonych czynności. Dzięki dostarczonym przez producenta plikom
można uzyskać dostęp do kilku rodzajów funkcji odpowiedzialnych za ruch manipulatora.
Poniżej przedstawiono funkcje ruchu wraz z opisem argumentów formalnych oraz
przykładami.
Wykorzystując funkcją MoveManual istnieje możliwość poruszania manipulatora
w zmiennych zewnętrznych oraz wewnętrznych. Przed jej wywołaniem konieczne jest jednak
określenie typu układu, w którym ruch będzie wykonywany. Odpowiada za to funkcja
EnterManual.
Opis parametrów formalnych funkcji EnterManual:
bool EnterManual(short sManualType)
Parametr sManualType może przyjmować następujące wartości:
MANUAL_TYPE_ENC=0, odpowiada za sterowanie manipulatora w zmiennych
wewnętrznych.
MANUAL_TYPE_XYZ=1, odpowiada za sterowanie manipulatora w zmiennych
zewnętrznych.
Opis parametrów formalnych funkcji MoveManual:
bool MoveManual(unsigned char ucAxis, long Velocity)
W parametrach formalnych określona jest oś, którą zostanie wykonany ruch oraz prędkość
wykonywania ruchu. Parametr ucAxis przyjmuje tutaj wartości od 0 do 7 przy sterowaniu
w zmiennych wewnętrznych oraz od 0 do 5 przy sterowaniu w zmiennych zewnętrznych.
Wartości te odpowiadają za wykonanie ruchu określoną osią bądz
w określonym kierunku.
W przypadku gdy nie zostanie określony czas trwania ruchu wówczas ruch zostanie
wykonany do końca zakresu danej osi.
Do wyznaczenia czasu trwania wykorzystywana jest funkcja Time(usigned char ucGroup,
long Time). Pierwszy parametr formalny odpowiada za wybór osi dla której następnie
zostanie określony czas wykonywania ruchu. Jednostką czasu są milisekundy.
Przykład
EnterManual(MANUAL_TYPE_ENC);
Time('A',50);
MoveManual(0,20);
W celu wykonania ruchu przy wykorzystaniu funkcji: MoveLinear, MoveJoint oraz
MoveCircularVect należy zdefiniować wektor przy użyciu funkcji DefineVector, w który
umieszczone będą pozycje manipulatora. Pozycje dodawane są do wektora utworzonego przy
pomocy funkcji DefineVector przy użyciu funkcji Teach z odpowiednimi parametrami.
Opis parametrów formalnych funkcji DefineVector:
bool DefineVector(unsigned char ucGroup, char *VectorName, short Dimension)
Parametr ucGroup może przyjmować następujące wartości:
 A  jeżeli zapamiętywane pozycje będą dotyczyć jedynie manipulatora
 B  jeżeli zapamiętywane pozycje będą dotyczyć urządzeń peryferyjnych
 &  jeżeli zapamiętywane pozycje będą dotyczyć wszystkich osi (manipulatora i urządzeń
peryferyjnych)
Parametr VectorName jest nazwa wektora, który będzie przechowywał określone pozycje
manipulatora, natomiast parametr Dimension odpowiada za ilość przechowywanych pozycji.
Opis parametrów formalnych funkcji Teach:
bool Teach (char *VectorName, short PointNumber, long *plCoorArray, short splCoorArray,
long lPointType )
Parametr VectorName jest nazwą wcześniej zdefiniowanego wektora, PointNumber
odpowiada za numer pozycji w danych wektorze, której zostaną przydzielone współrzędne
punku określone przez tablicę plCoorArray, splCoorArray jest rozmiarem tablicy
plCoorArray, a lPointType może przyjmować wartości:
ABS_XYZ_A  zapamiętanie pozycji zmiennych zewnętrznych względem układu bazowego
ABS_JOINT  zapmiętanie pozycji zmiennych wewnętrznych względem układu bazowego
Podobnie jak w przypadku programowania manipulatora w środowisku Robocell
w języku C++ istnieje możliwość wykonywania ruchów z interpolacją: quasiliniową
(MoveJoint), liniową (MoveLinear) oraz kołową (MoveCircuitVect).
Opis parametrów formalnych funkcji MoveJoint:
bool MoveJoint(char *VectorName, short PointNumber)
Parametr VectorName jest nazwą wektora przechowującego numer pozycji(PointNumber)
do której manipulator się przemieści.
Opis parametrów formalnych funkcji MoveLinear:
bool MoveLinear(char *VectorName, short PointNumber)
Parametr VectorName jest nazwą wektora przechowującego numer pozycji(PointNumber)
do której manipulator się przemieści z interpolacją linii prostej.
Opis parametrów formalnych funkcji MoveLinear:
bool MoveCircularVect(char *VectorName, short ThroughtPointNumber, short
toPointNumber)
Parametr VectorName jest nazwą wektora przechowującego numer pozycji
(ThroughtPointNumber) przez którą manipulator ruchem z interpolacją kołową przemieści się
do pozycje określonej przez parametr toPointNumber. Pozycje powinny znajdować się
w wektorze o tej samej nazwie
Przykład
//przykład dotyczy pozycjonowania quasiliniowego, reszta jest analogiczna
DefineVector('A',"pozycje",2);
long *plCoorArray=new long[5];
plCoorArray[0]=169000;
plCoorArray[1]=0;
plCoorArray[2]=500000;
plCoorArray[3]=-63000;
plCoorArray[4]=0;
Teach("pozycje",2,plCoorArray,5,ABS_XYZ_A);
MoveJoint("pozycje",1);
Za otwieranie oraz zamykanie chwytaka odpowiedzialne są bezparametrowe funkcje
OpenGripper() oraz CloseGripper(). Istnieje także możliwość określania odstępu pomiędzy
szczękami chwytaka.
4 Zauważone błędy
Częstym pojawiającym się błędem podczas bazowania było przekroczenie dostępnego
czasu przeznaczonego na bazowanie czwartej osi. Problem ten został rozwiązany poprzez
zwiększenie domyślnego czasu na wykonanie bazowania tejże osi w pliku konfiguracyjnym.
Kolejnym błędem podczas bazowania jest przekroczenie przez manipulator przestrzeni
ruchowej i przerwanie prawidłowo wykonywanego programu. Manipulator mimo
nieprawidłowego bazowania przechodzi do dalszego wykonywania instrukcji, dlatego
konieczne jest programowanie ograniczenie ruchowe.
Podczas bazowania manipulator nie ma ograniczeń ruchowych i jest w stanie uderzyć
napędem piątej osi we własny korpus.