Sterowanie robotem Astorino za pomocą mikrokontrolera Arduino z wykorzystaniem komunikacji UART

Do stworzenia artykułu wykorzystano robota Astorino (wersja B) i Arduino Due.

Konieczne jest wykorzystanie mikroprocesora o napięciu systemowym 3,3 V, ponieważ mikroprocesor wykorzystany w Astorino operuje na tym zakresie. Wykorzystanie płytki o napięciu systemowym wyższym niż 3,3 V (np. Arduino UNO z napięciem 5 V) może spowodować uszkodzenie kontrolera Astorino.

Płytkę programowano w środowisku Arduino IDE 2.3.6.

Biblioteka C++ do obsługi Astorino

Do sprawnej komunikacji stworzona została biblioteka w języku C++, udostępniająca komendy pozwalające na ustanowienie komunikacji między urządzeniami oraz wydawanie podstawowych poleceń robotowi.

Oto najważniejsze z nich:

astorino::astorino(HardwareSerial& port)

Konstruktor obiektu klasy astorino. Obiekt ten jest wymagany do wysyłania komend.

Argumenty:

• HardwareSerial port – port UART używany do komunikacji

Przykład użycia:

astorino r(Serial1);

byte astorino::Connect()

Otwiera połączenie między robotem a mikrokontrolerem

Argumenty: brak

Przykład użycia:

if(r.Connect() == 0){
	//Otwórz połączenie. Jeżeli poprawnie, wykonaj kod.
	//{...}
}

byte astorino::Disconnect()

Zamyka połączenie między robotem a mikrokontrolerem

Argumenty: brak

Przykład użycia:

r.Disconnect();

byte astorino::emergencyStop()

Wystawia do robota sygnał “Error”. Aby zresetować błąd należy zatwierdzić go na TP.

Argumenty: brak

Przykład użycia:

r.emergencyStop();

byte astorino::setMotorOn()

Włącza silniki robota

Argumenty: brak

Przykład użycia:

r.setMotorOn();

byte astorino::setMotorOff()

Robot wykonuje ruch do pozycji wyłączenia silników, następnie wyłącza silniki

Argumenty: brak

Przykład użycia:

r.setMotorOff();

byte astorino::reset()

Resetuje błąd robota

Argumenty: brak

Przykład użycia:

r.reset();

astorino::RetVal (wiele deklaracji)

Konstruktor zmiennej przechowującej odpowiedź robota

Argumenty: brak

Przykład użycia:

astorino::RetVal ret;

byte astorino::setHomeHere()

Ustawia położenie domowe robota w pozycji w której znajduje się koniec robota

Argumenty: brak

Przykład użycia:

r.setHomeHere();

byte astorino::setHome(double jt1, double jt2, double jt3, double jt4, double jt5, double jt6)

oraz przeciążenie funkcji:

byte astorino::setHome(double jt1, double jt2, double jt3, double jt4, double jt5, double jt6, double jt7)

istnieje również przeciążenie funkcji operujące na wektorze:

byte astorino::setHome(double* jt, int length)

Ustawia położenie domowe robota w podanej jako argumenty pozycji złączowej

Argumenty:

• double jt1-7 – kąty kolejnych osi robota.

Jeżeli robot nie obsługuje 7 osi, można użyć przeciążenia sześcioargumentowego, lub wpisać 0 jako argument siódmy.

Przeciążenie wektorowe:

• double * jt – wektor kątów kolejnych osi,
• int length – długość wektora

Przykład użycia:

r.setHome((double)0,(double)0,(double)-90,(double)0,(double)-90,(double)0);
r.setHome((double)0,(double)0,(double)-90,(double)0,(double)-90,(double)0,(double)0);

double h[] = {0,0,-90,0,-90,0};
int length = 6;
r.setHome(h,length);

byte astorino::Zero()

Wykonuje zerowanie robota

Argumenty: brak

Przykład użycia:

r.Zero();

byte astorino::HOME(byte spd, byte acc, byte dec)

Robot wykonuje ruch do pozycji domowej z określonymi parametrami.

Argumenty:

• byte spd – % prędkości maksymalnej robota,
• byte acc – % maksymalnego przyspieszenia robota,
• byte dec – % maksymalnego spowalniania robota

Przykład użycia:

r.HOME(30,90,80);

byte astorino::CMOVE(byte pointType, byte spd, byte acc, byte dec, double* middle, double* target, int length)

istnieje również przeciążenie funkcji, pozwalające na wykorzystanie istniejących punktów w programie robota:

byte astorino::CMOVE(byte pointType1, byte pointIndex1, byte pointIndex2, byte spd, byte acc, byte dec)

Robot wykonuje ruch po okręgu określonym punktem pierwszym do punktu drugiego z określonymi parametrami.

• byte pointType – typ podawanego punktu: 1 – punkt kartezjański, 2 – punkt złączowy.
  byte spd – % prędkości maksymalnej robota,
• byte acc – % maksymalnego przyspieszenia robota,
• byte dec – % maksymalnego spowalniania robota,
• double* middle – wektor zawierający współrzędne punktu pierwszego (pomocniczego),
• double* target – wektor zawierający współrzędne punktu końcowego,
• int length – ilość punktów podanych w wektorze

• byte pointType1 – typ podawanego punktu – 1 – punkt kartezjański, 2 – punkt złączowy.
• byte pointIndex1 – numer punktu w pamięci robota który ma być użyty jako punkt pomocniczy okręgu,
• byte pointIndex2 – numer punktu w pamięci robota który ma być użyty jako punkt końcowy ruchu po okręgu,

Przykład użycia:

double p[] = {0,0,-80,0,-90,0};
double k[] = {10,0,-90,0,-90,0};
int length = 6;
r.CMOVE(2, 90, 80, 80, p, k, length);

r.CMOVE(1, 5, 6, 80, 80, 90);

byte astorino::executeASCommand(String command)

Robot wykonuje podaną jako argument komendę.

Argument:

• String command – napisana w języku AS komenda przyjmowana przez robota.

Przykład użycia:

r.executeASCommand("SIGNAL 57"); //Ustawia sygnał 57 na wysoki

byte astorino::JMOVE(byte pointType, byte pointIndex, byte spd, byte acc, byte dec)

oraz przeciążenie pozwalające użycie własnego punktu:

byte astorino::JMOVE(byte pointType, byte spd, byte acc, byte dec, double* target, int length)

Robot wykonuje ruch złączowy do podanego punktu z określonymi parametrami.

Argumenty:

• byte pointType – typ podawanego punktu: 1 – punkt kartezjański, 2 – punkt złączowy.
• byte pointIndex – numer punktu w pamięci robota który ma być użyty jako punkt końcowy ruchu
• byte spd – % prędkości maksymalnej robota,
• byte acc – % maksymalnego przyspieszenia robota,
• byte dec – % maksymalnego spowalniania robota,
• double* target – wektor zawierający współrzędne punktu,
• int length – długość wektora punktów.

Przykład użycia:

r.JMOVE(1,2,90,80,80);

double punkt[6]={0,0,-90,0,-90,0};
r.JMOVE(2,40,90,90,punkt,6);

byte astorino::LMOVE(byte pointType, byte pointIndex, byte spd, byte acc, byte dec)

oraz przeciążenie pozwalające użycie własnego punktu:

byte astorino::LMOVE(byte pointType, byte spd, byte acc, byte dec, double* target, int length)

Robot wykonuje ruch liniowy do podanego punktu z określonymi parametrami.

Argumenty:

• byte pointType – typ podawanego punktu: 1 – punkt kartezjański, 2 – punkt złączowy
• byte pointIndex – numer punktu w pamięci robota który ma być użyty jako punkt końcowy ruchu
• byte spd – % prędkości maksymalnej robota,
• byte acc – % maksymalnego przyspieszenia robota,
• byte dec – % maksymalnego spowalniania robota,
• double* target – wektor zawierający współrzędne punktu,
• int length – długość wektora punktów.

Przykład użycia:

r.LMOVE(1,2,90,80,80);

double punkt[6]={0,0,-90,0,-90,0};
r.LMOVE(2,40,90,90,punkt,6);

astorino::RetVal astorino::Pose()

Zwraca położenie kartezjańskie robota.

Argumenty: brak

Przykład użycia:

ret = r.Pose();
Serial.print(ret.values[0]); //x
Serial.print(ret.values[1]); //y
Serial.println(ret.values[2]); //z

astorino::RetVal astorino::JT()

Zwraca położenie złączowe robota

Argumenty: brak

Przykład użycia:

ret=r.JT();
Serial.print(ret.values[0]);
Serial.print(ret.values[1]);
Serial.print(ret.values[2]);
Serial.print(ret.values[3]);
Serial.print(ret.values[4]);
Serial.println(ret.values[5]);  

Dodanie biblioteki do Arduino IDE

Aby dodać bibliotekę, należy z górnej wstążki oprogramowania rozwinąć Sketch, a następnie Include Library i wybrać Add .ZIP Library….

W eksploratorze plików należy wybrać odpowiednią bibliotekę.

Podłączenie przewodu do Serial1

Aby poprawnie ustanowić połączenie, należy:

• biały przewód podłączyć pod GND,
• czarny przewód podłączyć pod TX1 (pin 18),
• niebieski przewód podłączyć pod RX1 (pin 19).

Prosta aplikacja dla Arduino

Poniżej prezentujemy prosty program, który wykonuje następujące operacje:

• włączenie silników robota,
• zerowanie robota,
• ustawienie pozycji domowej i ruch do pozycji domowej,
• wysłanie do komputera przez łącze szeregowe pozycji, w jakiej znajduje się robot,
• wysłanie do robota instrukcji w języku AS: DRAW,
• ruch liniowy robota do zdefiniowanego na Arduino punktu,
• wyłączenie silników robota.

Objaśnienia znajdują się  w komentarzach programu.

Przed włączeniem programu należy upewnić się że robot każdy ruch zawarty w kodzie jest bezpieczny. Ponadto należy ustawić robota w tryb REPEAT.

#include "astorino.h"

astorino r(Serial1); // wskazanie, że komunikacja będzie odbywała się po Serial1
astorino::RetVal ret; //zmienna do której robot będzie zwracał informacje
 
//współrzędne używane do ustawienia pozycji domowej
int home1 = 0;
int home2 = 0;
int home3 = -90;
int home4 = 0;
int home5 = -90;
int home6 = 0;
int home7 = 0;

// punkt w formie wektora wykorzystany do polecenia LMOVE
double punkt[]={0,0,-85,15,-90,45};

void setup() {

	pinMode(LED_BUILTIN, OUTPUT); //ustawienie diody wbudowanej jako wyjście
	Serial.begin(115200);         // rozpoczęcie komunikacji Serial z komputerem (przez port programowalny) z prędkościa 115200 baudów/s
	delay(1000);

	if(r.Connect() == 0) //otworzenie połączenia. jeżeli otwarte, wykonaj kod
	{
		digitalWrite(LED_BUILTIN, HIGH); //ustawienie stanu diody wbudowanej na wysoki

		r.setUartTimeout(1000); //czas Timeout'u na połączenie robot-płytka

		r.setMotorOn(); //włączenie silników
		Serial.println("Włączono silniki robota");
		delay(1000); // opóźnienia w programie dla stabilności

		r.Zero(); // zerowanie robota
		delay(1000);

		Serial.println("Wyzerowano robota");
		delay(1000);

		r.setHome((double)home1,(double)home2,(double)home3,(double)home4,(double)home5,(double)home6); //ustawienie pozycji domowej robota po konwersji punktu z int na double

		r.HOME(30,90,80); // ruch robota do pozycji domowej
		delay(1000);

		r.executeASCommand("DRAW 5,5,20"); //przesłanie do robota komendy DRAW - przesunięcie liniowe względem BASE o x,y,z

		//Zapytanie robota o pozycję karezjańską i wyświetlenie w Serial Monitorze
		ret = r.Pose(); // read current position
		if (ret.returnCode == 0)
		{
			Serial.println("Pozycja kartezjańska");
			Serial.print(ret.values[0]); //x
			Serial.print(" ");
			Serial.print(ret.values[1]); //y
			Serial.print(" ");
			Serial.println(ret.values[2]); //z
			delay(200);
		}

		//Zapytanie robota o pozycję złączową i wyświetlenie w Serial Monitorze
		ret=r.JT();
		
		if (ret.returnCode == 0)
		{
			Serial.println("Pozycja złączowa");
			Serial.print(ret.values[0]);
			Serial.print(" ");
			Serial.print(ret.values[1]);
			Serial.print(" ");
			Serial.print(ret.values[2]);
			Serial.print(" ");
			Serial.print(ret.values[3]);
			Serial.print(" ");
			Serial.print(ret.values[4]);
			Serial.print(" ");
			Serial.println(ret.values[5]);
		}

		r.LMOVE(2,40,90,90,punkt,6); // ruch liniowy do punktu "punkt"
		delay(1000);

		r.setMotorOff(); //ruch robota do pozycji wyłączenia silników i wyłączenie silników.
		delay(1000);

		r.Disconnect(); //rozłączenie
		delay(1000);

		digitalWrite(LED_BUILTIN, LOW); //ustawienie stanu diody wbudowanej na niski
	}
}

void loop() {
	// put your main code here, to run repeatedly:
	delay(100);
}

Ściągnij plik z kodem źródłowym programu.

Komunikaty odebrane przez port szeregowy komputera:

Jeżeli program utknie po wykonaniu zerowania (dotyczy starszych wersji firmware robota), należy zresetować płytkę przyciskiem Reset.

Zaawansowana aplikacja – sterowanie robotem

Program zaawansowany – użycie drążka analogowego i przycisków do sterowania robotem w przestrzeni liniowej i złączowej, zamykania i otwierania chwytaka oraz przesyłania aktualnej pozycji robota przez UART.

Sposób podłączenia peryferiów (kliknij, aby powiększyć):

Gotowy program:

#include "astorino.h"

astorino r(Serial1); 
astorino::RetVal ret;

int home1 = 0;
int home2 = 0;
int home3 = -90;
int home4 = 0;
int home5 = -90;
int home6 = 0;
int home7 = 0;

double point[6];

int x = 0;
int y = 0;
int z = 0;

bool gripper=0;

int length = 6;

int switch1=0;

bool in_joint_mode=0;

void setup()
{
	// ustawienie odpowiednich trybów pracy I/O
	pinMode(12, INPUT_PULLUP);
	pinMode(11, INPUT_PULLUP);
	pinMode(10, INPUT_PULLUP);
	pinMode(9, INPUT_PULLUP);
	pinMode(8,INPUT_PULLUP);
	pinMode(2, OUTPUT);
	pinMode(3, OUTPUT);
	pinMode(4, OUTPUT);

	Serial.begin(115200);
	delay(1000);
	
	//Polecenia wykonywane przed oddaniem kontroli do użytkownika
  
	if(r.Connect() == 0)
	{
		r.setUartTimeout(1000);
		digitalWrite(2,HIGH);
		r.setMotorOn();
		delay(1000);
		Serial.println("Włączono silniki");
		delay(1000);
		r.Zero();
		delay(1000);
		Serial.println("Wyzerowano robota");
		delay(1000);
		//r.setHome((double)home1,(double)home2,(double)home3,(double)home4,(double)home5,(double)home6);
		r.HOME(30,90,80);
		Serial.println("Robot w pozycji domowej");
		delay(1000);
		Serial.println("Sterowanie użytkownika");
		delay(1000);
		Serial.println("Tryb liniowy. X,Y");
	}
}

void loop()
{
	//zmiana trybu ruchu - kartezjański/złączowy

	bool mode_switch = digitalRead(10);
	
	if(mode_switch==LOW && in_joint_mode==0)
	{
		Serial.println("Zmieniono tryb na złączowy. 1,2");
		in_joint_mode=1;
		switch1=0;
		delay(200);
	}
	else if(mode_switch==LOW)
	{
		Serial.println("Zmieniono tryb na liniowy. X,Y");
		in_joint_mode=0;
		switch1=0;
		delay(200);
	}

	//zapalenie diód kartezjański/złączowy

	if (in_joint_mode)
	{
		digitalWrite(3,HIGH);
		digitalWrite(4,LOW);
	}
	else if (in_joint_mode==0)
	{
		digitalWrite(4,HIGH);
		digitalWrite(3,LOW);
	}

	//zmiana płaszczyzn ruchu kartezjańskiego

	int analog_button = digitalRead(11);
	if(analog_button==LOW && switch1==0 && in_joint_mode==0)
	{
		switch1++;
		Serial.println("Tryb liniowy. X,Z");
		delay(200);
	}
	else if (analog_button==LOW && switch1==1 && in_joint_mode==0)
	{
		switch1++;
		Serial.println("Tryb liniowy. Y,Z");
		delay(200);
	}
	else if(analog_button==LOW && switch1==2 && in_joint_mode==0)
	{
		switch1=0;
		Serial.println("Tryb liniowy. X,Y");
		delay(200);
	}

	// zmiana złączy ruchu złączowego

	if(analog_button==LOW && switch1==0 && in_joint_mode==1)
	{
		switch1++;
		Serial.println("Tryb złączowy. 3,4");
		delay(200);
	}
	else if (analog_button==LOW && switch1==1 && in_joint_mode==1)
	{
		switch1++;
		Serial.println("Tryb złączowy. 5,6");
		delay(200);
	}
	else if(analog_button==LOW && switch1==2 && in_joint_mode==1)
	{
		switch1=0;
		Serial.println("Tryb złączowy. 1,2");
		delay(200);
	}

	// odczyt wartości z wejść analogowych i sterowanie robotem w zależności od nich w układzie kartezjańskim

	int yValue = analogRead(A0);
	int xValue = analogRead(A1);
	if(in_joint_mode==0)
	{
		switch(switch1)
		{
			case 0:
			//Poruszanie się x,y
			if(xValue<600)
			{
				x=-1;
			}
			if(xValue>900)
			{
				x=1;
			}
			if(yValue<600)
			{
				y=-1;
			}
			if(yValue>900)
			{
				y=1;
			}
			if (xValue>600 && xValue<900)
			{
				x=0;
			}
			if (yValue>600 && yValue<900)
			{
				y=0;
			}
			break;

			case 1:
			//Poruszanie się x,z
			if(xValue<600)
			{
				x=-1;
			}
			if(xValue>900)
			{
				x=1;
			}
			if(yValue<600)
			{
				z=-1;
			}
			if(yValue>900)
			{
				z=1;
			}
			if (xValue>600 && xValue<900)
			{
				x=0;
			}
			if (yValue>600 && yValue<900)
			{
				z=0;
			}
			break;
		
			case 2:
			//Poruszanie się y,x
			if(xValue<600)
			{
				y=-1;
			}
			if(xValue>900)
			{
				y=1;
			}
			if(yValue<600)
			{
				z=-1;
			}
			if(yValue>900)
			{
				z=1;
			}
			if (yValue>600 && yValue<900)
			{
				z=0;
			}
			if (xValue>600 && xValue<900)
			{
				y=0;
			}
			break;
		};
		
		//informacja o aktualnej pozycji złączowej robota, wykorzystana do ruchu
	}
	else if (in_joint_mode==1)
	{
		ret=r.JT();
		
		for (int i=0;i<6;i++)
		{
			point[i]=ret.values[i];
		}
		
		//sterowanie robotem w zależności od wejść analogowych w przestrzeni złączowej
		switch(switch1)
		{
			case 0:
			//Poruszanie się 1,2
			if(xValue<600)
			{
				point[0]++;
			}
			if(xValue>900)
			{
				point[0]--;
			}
			if(yValue<600)
			{
				point[1]--;
			}
			if(yValue>900)
			{
				point[1]++;
			}
			break;

			case 1:
			//Poruszanie się 3,4
			if(yValue<600)
			{
				point[2]++;
			}
			if(yValue>900)
			{
				point[2]--;
			}
			if(xValue<600)
			{
				point[3]--;
			}
			if(xValue>900)
			{
				point[3]++;
			}
			break;
		
			case 2:
			//Poruszanie się 5,6
			if(yValue<600)
			{
				point[4]--;
			}
			if(yValue>900)
			{
				point[4]++;
			}
			if(xValue<600)
			{
				point[5]--;
			}
			if(xValue>900)
			{
				point[5]++;
			}
		};
	}

	//operacja zmiany typu zmiennych
	String x_s=String(x);
	String y_s=String(y);
	String z_s=String(z);
	String bar="DRAW "+x_s+","+y_s+","+z_s;
	
	//wysyłanie komend ruchowych do robota
	
	if(in_joint_mode==0 && (x!=0 || y!=0 || z!=0))
	{
		r.executeASCommand(bar);
	}
	else if (in_joint_mode==1)
	{
		r.LMOVE(2,30,100,100,point,6);
	}	
	
// zwracanie pozycji robota na żądanie

	bool pozycja = digitalRead(9);
	
	if (pozycja==LOW)
	{
		ret = r.Pose(); // read current position
		if (ret.returnCode == 0)
		{
			Serial.println("Pozycja kartezjańska");
			Serial.print(ret.values[0]); //x
			Serial.print(" ");
			Serial.print(ret.values[1]); //y
			Serial.print(" ");
			Serial.println(ret.values[2]); //z
			delay(200);
		}
	
		ret=r.JT();
		Serial.println("Pozycja złączowa");
		Serial.print(ret.values[0]);
		Serial.print(" ");
		Serial.print(ret.values[1]);
		Serial.print(" ");
		Serial.print(ret.values[2]);
		Serial.print(" ");
		Serial.print(ret.values[3]);
		Serial.print(" ");
		Serial.print(ret.values[4]);
		Serial.print(" ");
		Serial.println(ret.values[5]);	
	}

	//zaciśnięcie chwytaka na żądanie
	
	bool zgripper=digitalRead(8);
	
	if (zgripper==LOW)
	{
		if (gripper == 0)
		{
			r.executeASCommand("SIGNAL 57");
			Serial.println("Zamknięto gripper");
			gripper = 1;
			delay(500);
		}
		else if (gripper == 1)
		{
			r.executeASCommand("SIGNAL -57");
			Serial.println("Otwarto gripper");
			gripper = 0;
			delay (500);
		}
	}

	//wyłączenie robota na żądanie

	bool wylacznik = digitalRead(12);
	
	if (wylacznik == LOW)
	{ 
		Serial.println("Wyłączanie silników");
		r.HOME(50,90,80);
		delay(1000);
		r.setMotorOff();
		delay(1000);
		r.Disconnect();
		digitalWrite(2,LOW);
		r.HOME(10,90,80);
		Serial.println("Wyłączono silniki");
	}
}

Ściągnij plik z kodem źródłowym programu.

Opis działania programu:

1. Nawiązanie połączenia.

2. Włączenie silników.

3. Zerowanie robota.

4. Ustawienie robota w pozycji HOME.

5. Zezwolenie na sterowanie użytkownika.

Sterowanie użytkownika i peryferia:

• Dioda podłączona pod DO 2 informuje o tym, czy zawarto połączenie z robotem.
• Diody podłączone pod DO 3 i 4 informują o tym, czy sterowanie ruchem odbędzie się odpowiednio w trybie liniowym, czy złączowym.
• Przycisk podłączony pod DI 8 – wciśnięcie powoduje, że robot wystawia stan wysoki na wyjście 57. Kolejne wciśnięcie spowoduje że robot na tym wyjściu wystawi stan niski. W naszym przypadku to wyjście steruje chwytakiem pneumatycznym.
• Przycisk podłączony pod DI 9 – wciśnięcie powoduje zwrot do Serial Monitor pozycji kartezjańskiej i złączowej, w jakiej znajduje się robot.
• Przycisk podłączony pod DI 10 – wciśnięcie powoduje zmianę trybu ruchu gałką analogową między trybem kartezjańskim a złączowym.
• Przycisk podłączony pod DI 12 – wciśnięcie powoduje powrót robota do pozycji domowej, ruch robota do pozycji wyłączenia silników, wyłączenie silników i zakończenie komunikacji z robotem.
• Wciśnięcie gałki analogowej powoduje zmianę płaszczyzny ruchu w trybie ruchu liniowego i zmianę złączy poruszanych w trybie ruchu złączowego. Wysunięcie gałki analogowej spowoduje ruch robota w określonym wcześniej trybie w wybranej płaszczyźnie / z użyciem wybranych złączy.

Zaleca się korzystanie z przycisków, gdy gałka analogowa nie jest wysunięta.

Wszystkie akcje sterujące za wyjątkiem wysunięcia gałki analogowej przesyłają komunikat o wykonanej przez nie akcji przez port szeregowy do komputera.

Informacje przekazywane do komputera przez port szeregowy: