POLITECHNIKA WARSZAWSKA

WYDZIAŁ 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.

Komputer Kontroler-USB

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ądź 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ądź 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 ź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ądź 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 ź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 ź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 wskaźnik do

funkcji CallBackFun. Typ CallBackFun jest wskaź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 wskaź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ądź 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ądź 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 manipulatoraHome('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ądź 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ładEnterManual(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 analogicznaDefineVector('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.