bozza 3 - Ingegneria elettrica ed elettronica

Transcript

Dipartimento di Ingegneria Elettronica ed Elettrica di Cagliari !
http://www.diee.unica.it/
Robot Arm
Lynxmotion lynx 6
Realizzato da Alessandro Floris 34395 & Andrea Marras 33641
Robot Arm Lynxmotion
Indice
Introduzione
4
Elettronica
5
Servo motori
6
Sintassi di comunicazione
7
Interfaccia grafica in Matlab
8
Il Tool GUIDE
8
Configurazione porta seriale e setup del braccio.
10
Metodi di inserimento
11
Cinematica Diretta
12
Cinematica inversa
13
Descrizione del robot Lynxmotion
13
Descrizione delle funzioni per la cinematica diretta
17
Descrizione delle funzioni per la cinematica inversa
17
Limitazioni del braccio robotico
20
Conclusioni
22
Codice sorgente!
23
File di inizializzazione
23
File di creazione della singola matrice di trasformazione omogenea
24
File di creazione della funzione cinematica diretta
25
File di creazione della funzione cinematica inversa
25
File di verifica delle soluzioni cinematiche inverse
29
File per la creazione dell’interfaccia grafica e comunicazione seriale
29
File di avvio procedura
63
Introduzione
Lo scopo di questo progetto è stato quello di creare un programma che permettesse di
pilotare un braccio robotico con 6 gradi di libertà utilizzando un’interfaccia grafica
intuitiva. Il programma creato avrà il compito di sollevare l’utente dall’oneroso compito di
determinare le matrici dirette ed inverse, nell’esposizione si cercherà di spiegare al
meglio i problemi sorti durante i test e le conseguenti soluzioni prese.
E’ stato scelto Matlab per la sua versatilità e ricca dotazione di strumenti che facilitano il
lavoro di programmazione. Il braccio utilizzato per le prove in laboratorio è stato il
Lynxmotion lynx 6, un braccio robotico dotato di servi analogici interfacciati con un
elettronica di controllo SSC-32.
Esamineremo inizialmente la parte elettronica e meccanica del robot, per poi passare
all’analisi del programma e della cinematica diretta e inversa che governa il braccio.
Elettronica
L’elettronica SSC-32 ha il compito di interfacciare il computer con i servomotori. I
comandi in formato stringa di testo, ricevuti tramite la porta seriale, vengono convertiti in
segnali elettronici ed inviati fisicamente ai servomotori.
Elenco connessioni elettronica SSC’32 (fig. 1)
L’elettronica SSC-32 presenta tre fonti di alimentazione separate:
VL: VS1: VS2: alimentazione elettronica di controllo, parte logica; (4)
alimentazione di potenza servi 1-16; (6)
alimentazione di potenza servi 16-32; (2)
Per questo esperimento si è scelto di separare l’alimentazione di potenza da quella
logica per evitare interferenze dovute ai motori. Si è infatti notato che i motori
introducono dei disturbi che vanno ad interferire con la comunicazione seriale fra PC e
scheda elettronica. La separazione delle due alimentazioni viene fatta rimuovendo il
jumper VL=VS (figura sopra n.5). Si è inoltre scelto di collegare le due alimentazioni di
potenza (figura sopra n.3), fattore non influente in quanto il secondo banco di servi
16-32 non è stato utilizzato.
La velocità di connessione viene impostata attraverso i jumpers presenti al centro della
scheda secondo la configurazione riportata di seguito.
Configurazione velocità di connessione (fig. 2)
Si è deciso di utilizzare la massima velocità possibile per la SSC-32 (115,2 Kbps), per
migliorare le prestazioni complessive della simulazione e rendere più fluidi i movimenti.
Servo motori
Gli attuatori utilizzati sono dei servomotori analogici HS-475HB con feedback di
posizione alimentati con una tensione continua positiva di 5V. Il servomotore viene
pilotato attraverso una sequenza di impulsi positivi la cui durata è direttamente
proporzionale alla posizione angolare del servo, e nello specifico varia da 0.9mS a 2.2 mS
distanziati da un intervallo di 20mS. La durata dell’impulso determina quindi la posizione
angolare assoluta del servo, ed è importante notare che un impulso di durata t[i] porterà
il servo sempre nella posizione angolare iesima, qualunque sia la sua posizione di
partenza. Di seguito viene riportata una tabella con alcuni esempi numerici che rendono
più facile la comprensione del meccanismo:
Un impulso della durata di 0.9mS porterà
sempre il servo nella posizione angolare
pari a -45° qualunque sia la sua posizione
di partenza.
sempre il servo nella posizione centrale
qualunque sia la sua posizione di
partenza.
sempre il servo nella posizione angolare
pari a +45° qualunque sia la sua posizione
di partenza.
Esempi numerici di posizioni angolari dei servomotori (fig. 3)
Le connessioni dei servi con l’elettronica di controllo sono realizzate attraverso dei
connettori DIN a tre terminali:
Rosso
Nero Bianco
alimentazione positiva 5V;
alimentazione massa;
segnale di comando;
Sintassi di comunicazione
La comunicazione fra il calcolatore e la scheda elettronica di controllo avviene tramite
connessione seriale. Nel caso non si disponesse di una connessione seriale, è sufficiente
installare un adattatore USB-Seriale. I dati scambiati sono di tipo ASCII con carattere
terminale di fine stringa <cr>. La stringa di comando inviata all’elettronica è suddivisa in
tre sezioni separate da uno spazio e deve essere così composta:
#<num>
Sostituendo a <num> un numero compreso fra 1 e 32 si seleziona il servo a cui inviare i
dati:
P<num>
Sostituendo a <num> un valore compreso fra 1 e 3000 si imposta la posizione angolare
del servo.
S<num>
Sostituendo a <num> un valore compresso fra 1 e 600 si imposta la velocità con cui il
servo deve raggiungere la posizione assegnata (in mS).
Una riga di esempio potrebbe essere la seguente:
#3 P600 S400 <cr>
La stringa in questione porta il servo numero tre nella posizione angolare 600 in 400 mS.
E’ anche possibile inviare contemporaneamente comandi a gruppi di servi, con l’unica
limitazione di usare un unico valore di velocità valido per tutto il gruppo. Un esempio
può essere:
#3 P500 #4 P350 #7 P2000 S400 <cr>
La stringa porta il servo 3 nella posizione 500, il servo 4 nella posizione 350 ed il servo 7
nella posizione 2000 con una velocità di gruppo pari a 4000mS.
Interfaccia grafica in Matlab
Per il controllo del braccio robotico si è deciso di utilizzare un interfaccia grafica semplice
ed intuitiva adattabile a varie configurazioni. Particolare attenzione si è posta nella
configurazione della porta seriale, rendendola completamente configurabile ed
adattabile, attraverso la scelta del nome della porta e la velocità di connessione di
quest'ultima. Per lo sviluppo della GUI (graphical user interface) è stato utilizzato il tool
integrato di Matlab “guide”, tool che semplifica la realizzazione del codice attraverso
oggetti pre-compilati. Lo sviluppo del codice è stato graduale e si è svolto in più passi.
Questo ha portato ad un’evoluzione progressiva dello stesso, che ha determinato alcune
discrepanze nella nomenclatura delle variabili. Il codice risulta perfettamente
funzionante, ma in alcune casi i pedici delle variabili assumono valori non sono coerenti.
Un esempio può essere la variabile “slider1” associata alla variabile “teta0”.
Il Tool GUIDE
Il tool GUIDE è uno strumento incorporato in Matlab che permette la facile realizzazione
di interfacce grafiche (GUI) attraverso l’utilizzo di primitive. Il tool viene avviato dal
workspace principale attraverso il comando :
>> guide
Scelta la tipologia del progetto, nel nostro caso “finestra vuota”, si apre una finestra in
cui è presente nella sinistra la barra degli strumenti e al centro l’ambiente di lavoro.
Schermata di avvio del tool guide (fig. 4)
Per l’inserimento delle primitive è sufficiente trascinare gli oggetti presenti nella barra a
sinistra all’interno della finestra di lavoro ed il tool provvederà in modo automatico alla
creazione del codice guida. Una volta trascinato l’oggetto è buona norma assegnargli un
nome appropriato che ci aiuterà ad individuarlo in maniera univoca e celere all’interno del
codice sorgente.
Fatta questa piccola premessa su come sviluppare un’applicazione grafica in
Matlab, passiamo alla descrizione dettagliata dell’applicazione RobotArmcontrol.m.
Il progetto è costituto da 7 file:
- init.m
- mtrasf.m
- mtot.m
-inversa.m
-controllo.m
-RobotArmControl.m
-go.m
Il file init.m provvede all’inizializzazione di tutte le variabili e costanti globali. Una
modifica hardware del progetto, per esempio una modifica alla lunghezza del braccio del
robot, o la sostituzione di un servo con un altro con diverse caratteristiche hardware
richiede un aggiornamento delle variabili corrispondenti.
Il file mtrasf.m ha il compito di creare la matrice di trasformazione omogenea,
prendendo in ingresso i valori di traslazione lungo asse x e z, le rotazioni di z attorno
all'asse x e di x attorno all'asse z del giunto i-esimo.
Il file mtot.m crea la funzione cinematica diretta richiamando le matrici di trasformazione
omogenea e moltiplicandole fra loro.
Il file inversa.m, tramite la funzione cinematica diretta ricevuta in ingresso, calcola due
soluzioni con il metodo della cinematica inversa.
Il file controllo.m deve verificare che le soluzioni proposte dalla funzione inversa.m siano
accettabili.
Il file RobotArmControl.m è il file che interfaccia i dati provenienti dai file di calcolo
( inversa.m, mtot.m , mtrasf.m ) con l’interfaccia grafica a video e si occupa della
creazione delle stringhe di testo da inviare ai servomotori attraverso la connessione
seriale.
Il file go.m è un semplice script che provvede al lancio in successione dei file init.m e
RobotArmControl.m.
Nel caso in cui il lettore volesse modificare l’interfaccia grafica della finestra di controllo,
dovrà semplicemente aprire il file RobotArmControl.m attraverso il tool guide ed
aggiungere o modificare le primitive interessate.
Descritto il funzionamento delle varie parti del codice e dei vari file che lo costituiscono,
passiamo al lancio vero e proprio dell’applicazione attraverso il comando:
>> go
Fnestra di controllo (fig. 4.1)
Si apre la finestra di controllo RobotArmControl (fig. 4.1) strutturata in quattro sezioni
suddivise in base al compito che devono assolvere:
1. configurazione porta seriale e setup del braccio;
2. inserimento delle posizioni angolari in gradi dei bracci;
3. Matrice della cinematica diretta;
4. Soluzioni della cinematica inversa.
Vediamo in dettaglio il funzionamento e le caratteristiche di ogni singola sezione.
Configurazione porta seriale e setup del braccio.
Configurazione porta seriale (fig. 5)
Il primo passo da effettuare è la creazione e configurazione della porta seriale.
In mancanza di questo primo passaggio il sistema restituisce un errore per qualsiasi
operazione si cerchi di effettuare. Attraverso il menu a tendina Port l’utente è in grado di
assegnare la relativa porta di comunicazione al software. La porta di comunicazione
cambia da computer a computer ed è assegnata in automatico dal sistema operativo con
l’installazione dei driver proprietari.
Il menu Baud Rate permette l’impostazione della velocità di connessione, sono stati
inseriti tutti i valori supportati dall’elettronica SSC32.
Una volta impostati i valori rispettivi al nome della porta e alla velocità di connessione, il
pulsante set provvederà alla creazione e all’apertura di un oggetto porta seriale,
distinguibile nel codice sorgente attraverso la variabile s.
Il pulsante set inoltre inizializza i cursori degli sliders (fig. 6) ai valori di start. La
taratura dei valori di star è stata eseguita empiricamente attraverso misurazioni e prove
successive. L’operazione di taratura è stata necessaria per allineare i servi alla posizione
iniziale. Durante le prove si è infatti riscontrato che non tutti i servi sono stati fissati sui
supporti con la stessa posizione angolare rispetto al rispettivo braccio di supporto, e
questo implica che due servi pilotati con la medesima stringa fanno assumere ai
rispettivi bracci angoli differenti. Per ovviare a questo inconveniente sono state introdotte
delle costanti numeriche di offset all’interno del codice che si vanno a sommare al valore
impostato manualmente dall’utente.
Cursori (sliders) per il controllo degli assi (fig. 6)
Metodi di inserimento
Sono state previste due modalità di inserimento:
-live: se selezionato, ogni modifica sul cursore degli sliders (fig. 6) o inserimento
attraverso le caselle di testo (fig. 8) viene immediatamente inviata al robot, si noti che in
questa modalità viene aggiornato lo stato di un singolo servo per volta, non è quindi
possibile realizzare movimenti composti;
-go: alla pressione del tasto go vengono aggiornati contemporaneamente tutti i servi
inviando i valori presenti in quell’istante nella finestra di controllo. In questo modo è
possibile realizzare movimenti composti.
Modalità live e go (fig. 7)
Sul lato destro sono presenti delle caselle di testo (fig. 8) attraverso cui è possibile
impostare direttamente la posizione in gradi del servo o la velocità a cui il servo dovrà
muoversi. Di default tutti i servi sono impostati attraverso il file init.m sulla posizione
zero gradi e con velocità 400 ms.
Caselle di testo inserimento angoli e velocità (fig. 8)
Cinematica Diretta
Ogni modifica effettuata tramite cursore slider o casella di testo aggiorna
automaticamente la matrice della cinematica diretta. Il lettore presti attenzione al fatto
che i servi non hanno alcun tipo di feedback, per cui se il servo (per impedimenti
meccanici, quali posizione angolare fuori range ammissibile) non può raggiungere la
posizione assegnata, il software di controllo non può in alcun modo saperlo, e in quel
caso le coordinate presenti a video non corrispondono alla reale posizione fisica del
braccio robotico.
Questo problema verrà trattato nel corso della tesina.
La soluzione a questo problema sarebbe l’implementazione di un sistema di feedback di
posizione va oltre le richieste di questo corso e richiede servi con encoder incrementale.
Cinematica diretta (fig. 9)
Cinematica inversa
I dati della cinematica diretta appena calcolata vengono utilizzati per ricavare la soluzione
della cinematica inversa. Attraverso la pressione del tasto calculate il software provvede
ad inviare i dati presenti nella funzione cinematica diretta, presente nella finestra di
controllo, alla funzione inversa.m che restituisce un vettore con le due soluzioni
possibili: gomito alto, gomito basso. I tasti go in prossimità delle soluzioni inviano ai
servi la soluzione corrispondente realizzando rispettivamente la configurazione a gomito
basso e a gomito alto. Il calcolo della cinematica inversa è stato volutamente
implementato in modo manuale e non automatico attraverso il tasto calculate (come per
la cinematica diretta) per permettere all’utente il confronto con la posizione precedente.
Cinematica inversa (fig. 10)
Descrizione del robot Lynxmotion
La struttura del robot è costituita da un insieme di bracci connessi in cascata
tramite giunti rotoidali. Grazie alla conoscenza degli angoli di ogni braccio è
possibile, tramite lo studio della cinematica diretta, conoscere il punto nel quale
la pinza del robot andrà a posizionarsi. Lo studio della cinematica verrà eseguito
tramite l’approccio di Denavit-Hartenberg.
Schematizzazione geometrica del progetto (fig. 11.1)
Individuiamo cinque giunti rotoidali:
- il Giunto 1 rappresenta la base e il suo asse di rotazione è z0. Il giunto può
effettuare una rotazione pari a θ1 lungo l’asse z0, nel piano x0y0;
- il Giunto 2 è invece la spalla, e il suo asse è perpendicolare all’asse del primo
giunto. L’angolo θ2 associato alla rotazione del secondo giunto si ha sul piano
x1y1;
- il Giunto 3 rappresenta il gomito, e il suo asse z è parallelo all’asse del giunto
1. Il giunto può effettuare una rotazione pari a θ3 nel piano x2y2;
- il Giunto 4 rappresenta il polso, come nel caso del giunto 3, il suo asse z risulta
essere parallelo all’asse z del primo giunto. L’angolo associato è θ4 nel piano
x3y3;
- il Giunto 5 rappresenta la pinza, il cui asse z4 è perpendicolare all’asse z3.
L’angolo associato alla pinza è θ5 e il piano di rotazione è x4y4.
Schematizzazione geometrica del progetto (fig. 11.2)
Fatta questa breve introduzione sugli assi e gli angoli che questi formano con i
bracci del robot, verranno ora introdotte le variabili che compongono la tabella
dei parametri usati per la convenzione di D-H :
-
di: ai: θi: αi: distanza lungo l’asse z;
distanza tra due giunti lungo l’asse x (tutti pari a 12 cm);
angolo lungo l’asse z, e il cui piano di rotazione è xy;
angolo tra due angoli adiacenti.
Riassumendo tutti i valori in una tabella:
Giunto
θ
d
a
α
1
θ1
0
0
90°
2
θ2
0
a2
0
3
θ3
0
a3
0
4
θ4
0
a4
-90°
5
θ5
0
0
0
6
θ6
0
0
0
Tabella dei parametri per il Lynx-6 (fig. 12)
Gli angoli θ5 e θ6 non sono stati presi in considerazione nel calcolo della
funzione cinematica diretta, essendo influenti sulla posizione finale dell’organo
terminale. Infatti θ5 è l’angolo legato al polso del robot, mentre θ6 è legato alla
presa della pinza. Definiamo quindi la funzione cinematica diretta:
ottenuta moltiplicando le matrici di trasformazione omogenea A(θ), che
rappresentano la trasformazione di coordinate in termini di matrici 4x4:
Nello specifico:
La matrice di trasformazione T06 sarà quindi una matrice 4x4, nella quale n, s, a
sono i versori di una terna solidale all’organo terminale del manipolatore, mentre
p è il vettore posizione dell’origine della terna solidale alla pinza rispetto
all’origine della terna base. n, s, a, p sono funzioni del vettore q delle variabili di
giunto.
Il versore a (a sta per approccio) è stato scelto nella direzione di avvicinamento
all’oggetto, mentre il versore s (s sta per scivolamento) è normale ad a. Infine il
versore n (normale) è stato scelto normale agli altri due versori in modo da creare
una terna levogira.
Il termine di maggiore rilevanza sarà p, dato dai primi tre termini dell’ultima
colonna partendo da sinistra, che indica la posizione del centro della pinza
rispetto alla base.
Descrizione delle funzioni per la cinematica diretta
Per la creazione delle matrici A(θ) è stata creata la funzione mtrasf.m , la quale
prende in ingresso i valori della tabella vista sopra, e rende in uscita la matrice di
trasformazione omogenea A.
function [A] = mtrasf(a,z,d,x)
La successiva funzione, la mtot.m, avrà il compito di moltiplicare le varie matrici
di trasformazione omogenea per ottenere la funzione di trasformazione diretta.
function [mtotale]= mtot(t1,t2,t3,t4)
Per semplicità è stato considerato l’ultimo angolo, quello legato alla rotazione
della pinza, pari a zero, poiché non influisce sulla posizione finale del braccio.
Descrizione delle funzioni per la cinematica inversa
Se tramite la cinematica diretta è possibile, partendo dagli angoli di giunto,
ottenere una posizione finale della pinza, tramite la cinematica inversa è invece
possibile trovare gli angoli che hanno permesso alla pinza di posizionarsi in quel
determinato punto dello spazio.
Per la determinazione degli angoli si parte dalla matrice T06, che verrà riscritta
qui sotto per completezza:
La determinazione degli angoli è un procedimento molto lungo, per cui verrà
descritta solo una sintesi della risoluzione.
L’angolo θ1 può essere trovato con semplicità:
A causa delle limitazioni fisiche del braccio meccanico, si è scelto di non
considerare la soluzione:
in quanto fisicamente irrealizzabile dal robot. Il braccio infatti non può ruotare
totalmente su se stesso poiché limitato da vincoli meccanici costruttivi.
L’angolo successivo è θ3, che può essere ottenuto grazie alla conoscenza di θ1 e
di pwx, pwy e pwz, calcolati grazie al vettore pw = p - a4*[n] . Quest’ultimo
vettore indica la posizione nello spazio del Giunto 4 del robot.
Si ricava il coseno tramite:
Per ogni coseno positivo esistono due seni, uno positivo e l’altro negativo:
Si trovano così due angoli, θ31 e θ32.
All’interno della funzione inversa.m sono stati valutati alcuni problemi che si
sono verificati durante le prove pratiche in laboratorio con il robot.
Uno di questi problemi è l’aver ottenuto in alcuni casi un c3>1, che ovviamente
non esiste. Il problema si è verificato varie volte a causa di alcune conversioni e
approssimazioni eseguite durante i calcoli. A causa dell’altissima precisione di
matlab, un coseno pari a 1.00000000001 viene considerato maggiore di 1. Il
problema è stato risolto forzando i vari coseni maggiori di 1 come uguali a 1.
Il calcolo dell’angolo θ2 risulta più complesso, e viene ottenuto grazie al calcolo
del seno e del coseno in funzione di θ1, pw_i e le distanze a_i tra i giunti lungo
l’asse x.
Nel calcolo del θ2 si sono volutamente trascurati i denominatori, infatti questi
vengono semplificati nell’argomento dell’atan :
θ2=atan2(s2,c2)
θ3 presenta due valori, quindi
definiti come θ21 e θ22.
si avranno conseguentemente due distinti θ2
dove
Il calcolo dell’ultimo angolo θ4 è ottenuto sfruttando le formule trigonometriche
di addizione:
che nello specifico diventano:
grazie ai quali si ottengono due coseni e quattro seni:
dai quali posso essere calcolati quattro θ4:
Ottenute le quattro soluzioni possibili, dovrà essere effettuata una verifica infatti
solo due di queste sono accettabili e fisicamente realizzabili dal robot. Per la
verifica sono state create quattro funzioni di cinematica diretta, che confrontano i
termini p_i (quindi l’ultima colonna di ogni matrice) con i termini p_i della
funzione cinematica diretta originaria tramite la funzione matlab controllo.m.
Anche in questo caso sono stati introdotti dei blocchi di controllo nei quali
vengono risolti i problemi legati alla precisione dei valori dovuti a matlab.
Al termine dei controlli si avranno solo due soluzioni accettabili, che saranno
quelle che permettono al braccio di raggiungere fisicamente il punto nello spazio
tramite determinate configurazioni di angoli.
In linea teorica le due soluzioni dovrebbero portare la pinza nello stesso punto
dello spazio, ma si è riscontrato che utilizzando la matrice di trasformazione
inversa, a seconda della configurazione scelta, si può avere una posizione
leggermente diversa della pinza per le due soluzioni. L’errore è causato dalla non
perfetta linearità dei servi e alle approssimazioni successive durante i calcoli.
Per limitare il problema delle approssimazioni sono stati introdotti dei blocchi di
controllo all’interno delle funzioni di Matlab, per permettere a due angoli che
differiscono di 0.2° di essere considerati come identici. In alcune configurazioni le
approssimazioni influiscono in maniera significativa sui calcoli, e le due posizioni,
gomito alto e gomito basso, posso differire fra loro, portando la pinza in due
posizioni leggermente differenti. L’errore riscontrato è dell’ordine dei millimetri,
un errore accettabile se si considera il fatto che sono stati utilizzati strumenti
amatoriali e non professionali.
Limitazioni del braccio robotico
Durante le prove in laboratorio si sono verificati alcuni problemi, e più volte si è
creduto di aver sbagliato il modello matematico realizzato. In alcuni casi il robot
raggiungeva il punto stabilito nel modo corretto, mentre in altri la posizione era
totalmente errata. Dopo aver cercato invano l’errore, all’interno della procedura di
calcolo, ci si è resi conto di un particolare: quando la posizione del robot risultava
errata, uno o più servi lavoravano sotto sforzo, infatti l’amperometro
dell’alimentatore registrava un consumo anomalo di corrente. Si è così capito che
la posizione calcolata dal programma era corretta, ma non poteva essere
fisicamente raggiunta dal braccio a causa delle limitazioni meccaniche nei suoi
movimenti.
Sono stati così determinati empiricamente dei vincoli che devono essere rispettati
affinché il modello matematico funzioni in modo corretto:
θ1
θ2
θ3
θ4
=
=
=
=
-80° ÷ 80°
0° ÷ 120°
-100° ÷ 47°
-107° ÷ 105°
Non sono stati volutamente introdotti dei vincoli all’interno del programma, in
modo da mantenere l’universalità del codice (nel caso in cui venisse usato con
altri modelli di robot).
Un esempio in cui compare il problema è la seguente configurazione:
θ1
θ2
θ3
θ4
=
=
=
=
0°
-37°
-69°
0°
θ2 non rispetta i vincoli appena descritti. Le configurazioni che si otterranno
dalle due soluzioni saranno quindi :
A sinistra la posizione corretta, a destra quella errata (fig. 13)
spetterà quindi all’utilizzatore finale verificare che gli angoli possano essere
fisicamente raggiunti dal robot.
Conclusioni
Questo elaborato si è posto come fine quello di descrivere in maniera chiara ed accurata
il problema della cinematica diretta ed inversa di un braccio robotico a sei gradi di libertà.
Unendo le nostre conoscenze elettroniche e matematiche, abbiamo cercato di superare le
varie difficoltà che si sono presentate durante lo studio del modello, e si è cercato di
semplificare e di ridurre al minimo le problematiche legate alle limitazioni fisiche del
robot e alla sua non perfetta linearità. Lo studio in laboratorio ci ha permesso di
apprezzare quanto un modello matematico differisca da un modello reale e come, in fase
di test, il primo debba essere modificato per raggiungere il risultato desiderato.
Sperando che questo studio possa essere utile per sviluppi futuri, auguriamo un buon
lavoro a coloro che vorranno approfondire il progetto da noi intrapreso.
Andrea Marras Alessandro Floris [email protected]
[email protected]
Codice sorgente
File di inizializzazione
%-------------------------------------------------------------------------
% File di inizializzazione di tutte le variabili globali
%-------------------------------------------------------------------------
% cancello lo schermo e la memoria del workspace, per evitare conflitti.
clc
clear
% Cancello il vettore con le impostazioni della porta seriale, per
% eliminare ogni configurazione precedente.
delete(instrfind);
% dichiaro le variabili globali
global sliderValue1 sliderValue2 sliderValue3 sliderValue4 sliderValue5 sliderValue6;
global speed1 speed2 speed3 speed4 speed5 speed6;
global a2 a3 a4 teta0 teta1 teta2 teta3 teta4 teta5 ;
global const live;
% const e' una costante necessaria per la conversione del valore proveniente
% dagli slider in radianti
const = 542.4;
%inizializzo il flag live a zero
live = 0;
% imposto gli angoli
teta0 =0;
teta1 =0;
teta2 =0;
teta3 =0;
teta4 =0;
teta5 =0;
% Valore centrale di default
defaultMiddle = 1500;
% Velocitaʼ di default
defaultSpeed = '400';
% Lunghezza bracci
a2 = 12;
a3 = 12;
a4 = 12;
% Setto i valori di default degli slider e delle velocita'
% I valori numerici sono il risultato di successive tarature empiriche
% (per ulteriori chiarimenti vedi testo)
sliderValue1 = defaultMiddle;
sliderValue2 = 720;
sliderValue3 = 960;
speed1 = defaultSpeed;
File di creazione della singola matrice di trasformazione omogenea
%------------------------------------------------------------------------% Il file mtrasf.m ha il compito di creare la matrice di
% trasformazione omogenea, prendendo in ingresso i valori di
% traslazione lungo asse x e z, le rotazioni di z attorno
% all'asse x e di x attorno all'asse z del giunto i-esimo.
%------------------------------------------------------------------------%
%
%
%
a traslazione lungo asse x
d traslazione lungo asse z
z rotazione di z attorno all'asse x
x rotazione di x attorno all'asse z
function [b] = mtrasf(a,z,d,x)
% Per avere un'approssimazione a due valori dopo la virgola
% viene usato il comando format bank
format bank
cz=cos(z);
sz=sin(z);
cx=cos(x);
sx=sin(x);
% Matrice di trasformazione omogenea
b=[cx -cz*sx sx*sz a*cx
sx cx*cz -cx*sz a*sx
0
sz
cz
d
0
0
0
1];
% Fine approssimazione a due cifre dopo la virgola
format
end
File di creazione della funzione cinematica diretta
%------------------------------------------------------------------------% Il file mtot.m crea la funzione cinematica diretta richiamando
% le matrici di trasformazione omogenea e moltiplicandole fra loro.
%------------------------------------------------------------------------function [mtotale]= mtot(t1,t2,t3,t4)
global a2 a3 a4;
% Matrici di trasformazione omogenea:
% m_i = mtransf(a_i,alpha_i,d_i,teta_i)
m1=mtrasf(0,pi/2,0,t1);
m2=mtrasf(a2,0,0,t2);
m3=mtrasf(a3,0,0,t3);
m4=mtrasf(a4,-pi/2,0,t4);
m5=mtrasf(0,-pi/2,0,0);
% Funzione cinematica diretta
mtotale=m1*m2*m3*m4*m5;
end
File di creazione della funzione cinematica inversa
%------------------------------------------------------------------------% Il file inversa.m, tramite la funzione cinematica diretta ricevuta in
% ingresso, calcola due soluzioni con il metodo della cinematica inversa.
%------------------------------------------------------------------------% La funzione prende in ingresso i parametri della
% della matrice di trasformazione (n_i, o_i, a_i e p_i)
% e rende gli angoli che hanno generato la stessa.
function [Inv]= inversa(nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz)
global a2 a3 a4;
%////// ANGOLO t1
% Se px e' minore di zero, si impone al braccio di lavorare nel semipiano
% destro, spalla destra.
if px>0
t1 = atan2(py,px);
else
t1 = atan2(py,-px);
end
s1 = sin(t1);
c1 = cos(t1);
%////// ANGOLO t3
% Per poter calcolare t3, deve essere calcolato il vettore pw = p - a4*[n]
pwx= px - (a4*nx);
pwy= py - (a4*ny);
pwz= pz - (a4*nz);
% Calcolo di c3
c3 = ((pwx^2)+(pwy^2)+(pwz^2)-(a2^2)-(a3^2))/(2*a2*a3);
%
%
%
%
A causa di imprecisioni nei passaggi degli angoli, puo’ accadere
che c3=1.0000000001 . Questo porta ad un s3 immaginario (mentre dovrebbe
essere uguale a zero).
Il problema puo’ essere risolto con il seguente comando:
if c3>1
c3=1;
end
% Per c3 si hanno due possibili seni, entrambi validi:
s3_1 = +sqrt(1-c3^2);
s3_2 = -sqrt(1-c3^2);
% Di conseguenza si hanno due angoli possibili t3
t3_1 = atan2(s3_1,c3);
t3_2 = atan2(s3_2,c3);
%////// ANGOLO t2
% Verranno calcolati due s2 e due c2, ottenendo cosÏ due possibili t2.
% Nel calcolo del t2 si sono volutamente trascurati i denominatori,
% infatti questi vengono semplificati nell’argomento dell’atan.
s2_1
s2_2
c2_1
c2_2
=
=
=
=
(pwz*((a3*c3)+a2))-(a3*s3_1*((c1*pwx)+(s1*pwy)));
(pwz*((a3*c3)+a2))-(a3*s3_2*((c1*pwx)+(s1*pwy)));
((pwz)*(a3*s3_1))+(((a3*c3)+a2)*((c1*pwx)+(pwy*s1)));
((pwz)*(a3*s3_2))+(((a3*c3)+a2)*((c1*pwx)+(pwy*s1)));
t2_1 = atan2(s2_1,c2_1);
t2_2 = atan2(s2_2,c2_2);
%////// ANGOLO t4
% Il calcolo dell’ultimo angolo t4 e’ ottenuto sfruttando le formule
% trigonometriche di addizione.
c23_1
c23_2
s23_1
s23_2
=
=
=
=
(cos(t2_1)*cos(t3_1))-(sin(t2_1)*sin(t3_1));
(cos(t2_2)*cos(t3_2))-(sin(t2_2)*sin(t3_2));
(sin(t2_1)*cos(t3_1))+(sin(t3_1)*cos(t2_1));
(sin(t2_2)*cos(t3_2))+(sin(t3_2)*cos(t2_2));
% Grazie alle quali puo’ essere calcolato cos(t4)
c4_1 = (nz*s23_1)-(oz*c23_1);
c4_2 = (nz*s23_2)-(oz*c23_2);
% Anche in questo caso puo’ accadere che c4=1.0000000001 .
% Il problema viene risolto come prima:
if c4_1>1
c4_1 = 1;
end
if c4_2>1
c4_2 = 1;
end
% Calcolo dei sin(t4)
s4_11
s4_12
s4_21
s4_22
=
=
=
=
+sqrt(1-c4_1^2);
-sqrt(1-c4_1^2);
+sqrt(1-c4_2^2);
-sqrt(1-c4_2^2);
% Calcolo dei quattro t4
t4_11
t4_12
t4_21
t4_22
=
=
=
=
atan2(s4_11,c4_1);
atan2(s4_12,c4_1);
atan2(s4_21,c4_2);
atan2(s4_22,c4_2);
% Ottenute le quattro soluzioni possibili, viene effettuata una
% verifica perche’ solo due di queste sono accettabili.
% Per la verifica vengono create quattro funzioni di cinematica diretta
% che confrontano i termini p_i con i termini p_i della funzione
% cinematica diretta originaria tramite controllo.m .
ver_11
ver_12
ver_21
ver_22
=
=
=
=
mtot(t1,t2_1,t3_1,t4_11);
mtot(t1,t2_2,t3_2,t4_22);
mtot(t1,t2_1,t3_1,t4_12);
mtot(t1,t2_2,t3_2,t4_21);
% Viene richiamata la funzione controllo per la matrice ver_11
controllo1=controllo(ver_11(1,4),ver_11(2,4),ver_11(3,4), px,py,pz);
% Se la funzione rende zero, gli angoli di ver_11 non sono accettabili
% e quella configurazione di angoli viene scartata.
% Se la funzione rende 1, la serie di angoli viene mandata in uscita.
if controllo1 == 1
inv_11 = [t1,t2_1,t3_1,t4_11];
end
% Se la funzione rende zero, la configurazione di ver_12 non e’
% accettabile e viene scartata.
% Se invece la funzione rende 1, la configurazione viene mandata in uscita.
if controllo2 == 1
inv_12 = [t1,t2_2,t3_2,t4_22];
end
% Manca il controllo delle ultime due funzioni (ver_21 e ver_22).
%
%
%
%
%
In uscita si dovranno ottenere due configurazioni possibili e, come
detto prima, solo 2 delle 4 configurazioni sono valide.
Associamo ver_21 a ver_11 e ver_22 a ver_12. In questo modo, se con
ver_11 non si ha una configurazione valida, entrera’ in gioco ver_21.
Lo stesso per ver_22 e ver_12.
% controllo3 viene impostato a zero:
controllo3=0;
% Se la prima funzione cinematica diretta non e’ valida (controllo1 == 0)
% viene richiamata la funzione controllo3. Se quest'ultima risulta == 1,
% allora verra’ mandata in uscita la configurazione ver_21.
if controllo1 == 0
controllo3 = controllo(ver_21(1,4),ver_21(2,4),ver_21(3,4), px,py,pz);
end
if controllo3 == 1
inv_11 = [t1,t2_1,t3_1,t4_12];
end
% Stesso discorso va fatto con ver_22
controllo4=0;
if controllo2==0
end
if controllo4 == 1
inv_12 = [t1,t2_2,t3_2,t4_21];
end
% Le 2 soluzioni rimanenti (non valide) vengono quindi impostate a zero.
inv_21 = [0,0,0,0];
inv_22 = [0,0,0,0];
% Uscita
Inv = [inv_11
inv_12
inv_21
inv_22
];
end
File di verifica delle soluzioni cinematiche inverse
%------------------------------------------------------------------------% La funzione prende in ingresso le componenti px, py e pz della
% funzione cinematica diretta di verifica, qui associate ad a, b, c.
% Queste vengono messe a confronto con px, py e pz della funzione
% cinematica diretta originale (qui associate a px, py, pz).
% La funzione rendera’ in uscita la variabile "control".
% Se (a = px), (b = py) e (c = pz), "control" verra’ impostato a 1,
% altrimenti rimarra’ 0.
%------------------------------------------------------------------------function [control]= controllo(a,b,c,px,py,pz)
%
%
%
%
%
A causa delle approssimazioni nei calcoli, puo’ accadere che a = 1.5e-15
e che px = 4.6e-16.
Matlab dira’ che a e’ diverso da px. Per ovviare a questo
problema di precisione, forziamo due valori che differiscono fra loro
meno di due decimali ad essere considerati identici.
% Si imposta la variabile "verificato" a 0, e se le condizioni del prossimo
% if saranno verificate, "verificato" sara’ impostato a 1.
verificato=0;
% Se (a = px), (b = py) e (c = pz) allora verificato = 1
if abs(a - px) < 0.2
if abs(b - py) < 0.2
if abs(c - pz) < 0.2
verificato = 1;
end
end
end
% La variabile in uscita assumera’ il valore ottenuto da "verificato"
control= verificato;
end
File per la creazione dellʼinterfaccia grafica e comunicazione seriale
%-------------------------------------------------------------------------
%!
!
Guida per una facile lettura del codice
%-------------------------------------------------------------------------
% Variabili utilizzate:
%
% sliderValueX: contiene il valore dello slider (cursore) X
% speedX: contiene il valore della velocita' del servo X
% s: rappresenta l'oggetto porta seriale
% live: e' un flag per il controllo live del robot, se live e' pari ad uno
%!
ogni modifica di posizione viene immediatamente inviata al robot, se
%!
live e' pari a zero le modifiche vengono solamente aggiornate nello
%!
schermo.
% tetaX: rappresenta l'angolo associato al grado di liberta' X
% const: costante necessaria per la conversione in radiati del valore letto
% attraverso lo slider
%
%
% Per non appesantire la lettura del codice si e' deciso di commentare solo
% la prima funzione di ogni sezione, cio' non dovrebbe creare problemi al
% lettore visto che le funzioni sono ripetitive per ogni grado di liberta'.
% (cambiano esclusivamente i pedici).
%
% la seguente parte di codice e' generata in automatico dal Tool Guide.
function varargout = RobotArmControl(varargin)
% ROBOTARMCONTROL M-file for RobotArmControl.fig
%
ROBOTARMCONTROL, by itself, creates a new ROBOTARMCONTROL or raises
the existing
%
singleton*.
%
%
H = ROBOTARMCONTROL returns the handle to a new ROBOTARMCONTROL or
the handle to
%
the existing singleton*.
%
%
ROBOTARMCONTROL('CALLBACK',hObject,eventData,handles,...) calls the local
%
function named CALLBACK in ROBOTARMCONTROL.M with the given input
arguments.
%
%
ROBOTARMCONTROL('Property','Value',...) creates a new ROBOTARMCONTROL
or raises the
%
existing singleton*. Starting from the left, property value pairs are
%
applied to the GUI before RobotArmControl_OpeningFcn gets called. An
%
unrecognized property name or invalid value makes property application
%
stop. All inputs are passed to RobotArmControl_OpeningFcn via varargin.
%
%
*See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
%
instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help RobotArmControl
% Last Modified by GUIDE v2.5 25-Aug-2009 12:32:05
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',
mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @RobotArmControl_OpeningFcn, ...
'gui_OutputFcn', @RobotArmControl_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT
% --- Executes just before RobotArmControl is made visible.
function RobotArmControl_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% varargin command line arguments to RobotArmControl (see VARARGIN)
% Choose default command line output for RobotArmControl
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
% UIWAIT makes RobotArmControl wait for user response (see UIRESUME)
% uiwait(handles.figure1);
% --- Outputs from this function are returned to the command line.
function varargout = RobotArmControl_OutputFcn(hObject, eventdata, handles)
% varargout cell array for returning output args (see VARARGOUT);
% hObject handle to figure
% Get default command line output from handles structure
varargout{1} = handles.output;
%-------------------------------------------------------------------------
% !
%!
!
!
Eseguito al movimento dello slider1
!
( posizione servo 1)
%------------------------------------------------------------------------Realizzato da Alessandro Floris 34395 & Andrea Marras 33641
function slider1_Callback(hObject, eventdata, handles)
global sliderValue1 live s speed1;
global teta0 const;
% se il flag live e' attivo
if(live)
% leggo il valore dello slider e lo memorizzo in sliderValue1
% (con un approssimazione di due cifre decimali).
sliderValue1 = num2str(get(handles.slider1,'Value'),'%6.2f');
% Il valore sottratto allo sliderValue1, in questo caso 1500, rappresenta l'offset
% necessario per centrare il servo motore del robot (vedi spiegazione nel testo).
% la constante const e' necessaria per la conversione dal valore fornito dallo
% slider ai radianti.
% 57.29577951 viene invece utilizzato per la conversione da radianti a gradi
% e viceversa.
% Converto la posizione dello slider in gradi e lo inserisco all'interno del
% textbox corrispondente, in questo caso edit1.
set(handles.edit1,'String',((sliderValue1-1500)/const)*57.29577951);
% imposto teta0 in radianti
teta0 = (sliderValue1-1500)/const;
% Aggiorno l'interfaccia grafica
% Genero la stringa da spedire al servo
word = ['#0P',num2str(sliderValue1,'%6.2f'),'S',speed1];
% Invio la stringa al servo
fprintf(s,word)
% Aggiorno la matrice della cinematica diretta chiamando la funzione
% upggradematrix (vedi piu' avanti)
upgradematrix(hObject, eventdata, handles)
% Se il flag live e' disattivato
else
% leggo il valore dello slider e lo memorizzo in sliderValue1
sliderValue1 = get(handles.slider1,'Value');
% come descritto sopra converto in gradi ed in seguito inserisco il valore
% letto nel texbot corrispondente.
set(handles.edit1,'String', num2str((((sliderValue1-1500)/const)*57.29577951),'%6.2f'));
% imposto teta0 in radianti
% Aggiorno la matrice della cinematica diretta chiamando la funzione
% upggradematrix
end
% il seguente codice e' generato in automatico dal tool GUIDE
% --- Executes during object creation, after setting all properties.
function slider1_CreateFcn(hObject, eventdata, handles)
% hObject handle to slider1 (see GCBO)
% handles empty - handles not created until after all CreateFcns called
% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor',[.9 .9 .9]);
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
%-------------------------------------------------------------------------
% Per i commenti sul codice vedere slider1
global sliderValue2 live s speed2 teta1 const;
if(live)
fprintf(s,word)
else
end
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
%-------------------------------------------------------------------------
if(live)
word = ['#2P',num2str(1920-sliderValue3,'%6.0f'),'S',speed3];
fprintf(s,word)
else
end
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
%-------------------------------------------------------------------------
if(live)
fprintf(s,word)
else
end
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
%-------------------------------------------------------------------------
if(live)
fprintf(s,word)
else
end
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
%-------------------------------------------------------------------------
if(live)
fprintf(s,word)
else
end
end
% --- If Enable == 'on', executes on mouse press in 5 pixel border.
% --- Otherwise, executes on mouse press in 5 pixel border or over slider1.
function slider1_ButtonDownFcn(hObject, eventdata, handles)
% --- Executes on key press with focus on slider1 and none of its controls.
function slider1_KeyPressFcn(hObject, eventdata, handles)
% eventdata structure with the following fields (see UICONTROL)
%!
Key: name of the key that was pressed, in lower case
%!
Character: character interpretation of the key(s) that was pressed
%!
Modifier: name(s) of the modifier key(s) (i.e., control, shift) pressed
%sliderValu5---------------------------------------------------VERIFICA
%-------------------------------------------------------------------------
% !
%!
!
!
Eseguito con la modifica del textbox 1
!
%-------------------------------------------------------------------------
function edit1_Callback(hObject, eventdata, handles)
global sliderValue1 const teta0;
% Come visto in precedenza, leggo lo slider e lo memorizzo in gradi nella variabile
% corrispondente.
sliderValue1 = ((str2num(get(handles.edit1,'String')))*(const/57.29577951))+1500;
% Inserisco il valore appena calcolato all'interno del textbox corrispondente
set(handles.slider1,'Value',sliderValue1);
% imposto teta in radianti
function edit1_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit1 (see GCBO)
% Hint: edit controls usually have a white background on Windows.
%
See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get
(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
%-------------------------------------------------------------------------
% Per i commenti sul codice vedere textbox 1
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
%-------------------------------------------------------------------------
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
%-------------------------------------------------------------------------
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
%-------------------------------------------------------------------------
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
%-------------------------------------------------------------------------
%
end
% !
%!
!
!
!
( velocita'servo 1)
%-------------------------------------------------------------------------
global speed1;
% imposto la variabile speedX con il valore letto nel textbox
speed1 = (get(hObject,'String'));
% Codice generato in automatico dal tool GUIDE
% Hints: get(hObject,'String') returns contents of edit7 as text
%
str2double(get(hObject,'String')) returns contents of edit7 as a double
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
( velocita'servo 2)
%-------------------------------------------------------------------------
global speed2;
speed2 = get(hObject,'String');
%
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
( velocita'servo 3)
%-------------------------------------------------------------------------
global speed3;
%
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
( velocita'servo 4)
global speed4;
%
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
( velocita'servo 5)
%-------------------------------------------------------------------------
global speed5;
%
% handles
empty - handles not created until after all CreateFcns called
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
!
( velocita'servo 6)
%-------------------------------------------------------------------------
global speed6;
%
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
Eseguito con la modifica del popup menu 1
!
(menu con nome porta seriale)
%-------------------------------------------------------------------------
% viene utilizzata la variabile globale 'com' per settare la porta seriale
function popupmenu1_Callback(hObject, eventdata, handles)
global com
% Leggo il valore presente nel popup menu. Nota bene: la funzione restituisce un
% vettore, dobbiamo estrapolare il solo valore in formato stringa.
% valuecom e' un vettore
valueCom = get(hObject,'String');
% preleviamo il primo valore del vettore (valore selezionato dell'utente )
% e lo memorizziamo in com
com = valueCom(get(hObject,'Value'),1);
function popupmenu1_CreateFcn(hObject, eventdata, handles)
% hObject handle to popupmenu1 (see GCBO)
% Hint: popupmenu controls usually have a white background on Windows.
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
Eseguito con la modifica del popup menu 2
!
(menu con velocita' porta seriale)
%-------------------------------------------------------------------------
function popupmenu2_Callback(hObject, eventdata, handles)
global baudratecustom;
% Leggo il valore presente nel popup menu. Nota bene: la funzione restituisce un
% vettore, dobbiamo estrapolare il solo valore in formato stringa.
% valueBauRate e' un vettore
valueBaudRate = get(hObject,'String');
% Preleviamo il primo valore del vettore (valore selezionato dall'utente)
baudrate = cell2mat(valueBaudRate(get(hObject,'Value'),1));
% Converto il valore letto da stringa a numero
baudratecustom = str2num(baudrate);
function popupmenu2_CreateFcn(hObject, eventdata, handles)
% hObject handle to popupmenu2 (see GCBO)
% Hint: popupmenu controls usually have a white background on Windows.
%
end
%-------------------------------------------------------------------------
% !
%!
!
!
Eseguito con la pressione del button 1 (pulstante 'go')
(invio le stringhe a tutti i servi contemporaneamente)
%-------------------------------------------------------------------------
function pushbutton1_Callback(hObject, eventdata, handles)
global live s;
global speed1 speed2 speed3 speed4 speed5 speed6 word0;
% Se il flag live e' disattivato, genero le stringhe da inviare ai servi e le invio
% immediatamente
if(live == 0)
word0 = ['#0P',num2str(sliderValue1,'%6.0f'),'S',speed1];
% ---> * * * ATTENZIONE * * * <--% Il terzo servo ruota in senso opposto rispetto a tutti gli altri, e' stato
% necessario quindi un offset di posizione ed un cambio di segno del valore associato allo
% slider (nota il - davanti alla variabile). Il valore 1920 e' stato trovato empiricamente
% attraverso tarature successive.
word2 = ['#2P',num2str(1920-sliderValue3,'%6.0f'),'S',speed3];
% invio le stringhe ai servi
fprintf(s,word0)
fprintf(s,word1)
fprintf(s,word2)
fprintf(s,word3)
fprintf(s,word4)
fprintf(s,word5)
%aggiorno la cinematica diretta (vedi piu' avanti)
end
% --- Executes on button press in checkbox1.
function checkbox1_Callback(hObject, eventdata, handles)
global live;
live = get(handles.checkbox1,'Value');
% hObject handle to checkbox1 (see GCBO)
% Hint: get(hObject,'Value') returns toggle state of checkbox1
%-------------------------------------------------------------------------
% !
%!
!
!
Eseguito con la pressione del button 2 (pulsante 'set')
(Imposto tutti i parametri e configuro la porta seriale)
%-------------------------------------------------------------------------
function pushbutton2_Callback(hObject, eventdata, handles)
global com baudratecustom s;
% Imposto tutti gli slider ai valori iniziali. Nota bene, non tutti gli slider sono
% centrati, questo e' dovuto alla taratura empirica dei servo motori attraverso
% regolazioni successive, i valori di offset sono nel file init.m
% Creo ed configuro la porta seriale
s = serial(com,'BaudRate',baudratecustom,'Terminator','CR/LF');
% chiamo la funzione per l'apertura della porta seriale
openport(s)
%-------------------------------------------------------------------------
% !
!
Funzione per apertura della posta seriale
%-------------------------------------------------------------------------
function openport (port)
% e' stato necessario la creazione di una funzione dedicata,
% perche' all'interno delle funzione GUI non e' possibile chiamare funzioni
% che restituiscono un vettore come la funzione fopen()
fopen(port)
%-------------------------------------------------------------------------
% !
%!
%!
%!
%!
%
%!
%!
%!
%!
%
%!
%!
%!
%!
%
In seguito troviamo le funzioni che implementano i textbox
della matrice associata alla cinematica diretta. Il codice e'
generato in automatico dal tool GUIDE.
Per semplicitaʼ non verranno commentate, il lettore sappia che
la matrice e' cosi' strutturata:
!
!
!
!
nx!
!
ny ! !
nz!
!
edit25!!
sx!
!
sy !
!
sz!
!
edit 26!
ax!
!
ay!
!
az!
!
edit 27!
pz
py
pz
edit 28
NOTA BENE:
I textbox editX sono necessari per rendere la matrice quadrata,
i rispettivi valori sono [0 0 0 1]
%-------------------------------------------------------------------------
function nx_Callback(hObject, eventdata, handles)
% hObject handle to nx (see GCBO)
% Hints: get(hObject,'String') returns contents of nx as text
%
str2double(get(hObject,'String')) returns contents of nx as a double
function nx_CreateFcn(hObject, eventdata, handles)
% hObject handle to nx (see GCBO)
%
end
function sx_Callback(hObject, eventdata, handles)
% hObject handle to sx (see GCBO)
% Hints: get(hObject,'String') returns contents of sx as text
%
str2double(get(hObject,'String')) returns contents of sx as a double
function sx_CreateFcn(hObject, eventdata, handles)
% hObject handle to sx (see GCBO)
%
end
function ax_Callback(hObject, eventdata, handles)
% hObject handle to ax (see GCBO)
% Hints: get(hObject,'String') returns contents of ax as text
%
str2double(get(hObject,'String')) returns contents of ax as a double
function ax_CreateFcn(hObject, eventdata, handles)
% hObject handle to ax (see GCBO)
%
end
function px_Callback(hObject, eventdata, handles)
% hObject handle to px (see GCBO)
% Hints: get(hObject,'String') returns contents of px as text
%
str2double(get(hObject,'String')) returns contents of px as a double
function px_CreateFcn(hObject, eventdata, handles)
% hObject handle to px (see GCBO)
%
end
function ny_Callback(hObject, eventdata, handles)
% hObject handle to ny (see GCBO)
% Hints: get(hObject,'String') returns contents of ny as text
%
str2double(get(hObject,'String')) returns contents of ny as a double
function ny_CreateFcn(hObject, eventdata, handles)
% hObject handle to ny (see GCBO)
%
end
function ay_Callback(hObject, eventdata, handles)
% hObject handle to ay (see GCBO)
% Hints: get(hObject,'String') returns contents of ay as text
%
str2double(get(hObject,'String')) returns contents of ay as a double
function ay_CreateFcn(hObject, eventdata, handles)
% hObject handle to ay (see GCBO)
%
end
function py_Callback(hObject, eventdata, handles)
% hObject handle to py (see GCBO)
% Hints: get(hObject,'String') returns contents of py as text
%
str2double(get(hObject,'String')) returns contents of py as a double
function py_CreateFcn(hObject, eventdata, handles)
% hObject handle to py (see GCBO)
%
end
function nz_Callback(hObject, eventdata, handles)
% hObject handle to nz (see GCBO)
% Hints: get(hObject,'String') returns contents of nz as text
%
str2double(get(hObject,'String')) returns contents of nz as a double
function nz_CreateFcn(hObject, eventdata, handles)
% hObject handle to nz (see GCBO)
%
end
function sz_Callback(hObject, eventdata, handles)
% hObject handle to sz (see GCBO)
% Hints: get(hObject,'String') returns contents of sz as text
%
str2double(get(hObject,'String')) returns contents of sz as a double
function sz_CreateFcn(hObject, eventdata, handles)
% hObject handle to sz (see GCBO)
%
end
function az_Callback(hObject, eventdata, handles)
% hObject handle to az (see GCBO)
% Hints: get(hObject,'String') returns contents of az as text
%
str2double(get(hObject,'String')) returns contents of az as a double
function az_CreateFcn(hObject, eventdata, handles)
% hObject handle to az (see GCBO)
%
end
function pz_Callback(hObject, eventdata, handles)
% hObject handle to pz (see GCBO)
% Hints: get(hObject,'String') returns contents of pz as text
%
str2double(get(hObject,'String')) returns contents of pz as a double
function pz_CreateFcn(hObject, eventdata, handles)
% hObject handle to pz (see GCBO)
%
end
%
%
end
% handles
structure with handles and user data (see GUIDATA)
%
%
end
%
%
end
%
%
end
function sy_Callback(hObject, eventdata, handles)
% hObject handle to sy (see GCBO)
% Hints: get(hObject,'String') returns contents of sy as text
%
str2double(get(hObject,'String')) returns contents of sy as a double
function sy_CreateFcn(hObject, eventdata, handles)
% hObject handle to sy (see GCBO)
%
end
%------------------------------------------------------------------------%
%!
!
Funzione utilizzata per aggiornare la matrice inversa a video
%
%-------------------------------------------------------------------------
function upgradematrix(hObject, eventdata, handles)
global teta0 teta1 teta2 teta3 teta4 teta5 ;
global NX NY NZ SX SY SZ AX AY AZ PX PY PZ;
global matrix const ;
% Leggo i valori degli slider ed imposto gli angoli da inviare alla matrice
% per il calcolo dell'inversa
% La funzione mtot restituisce la matrice inversa e la memorizza in matrix
matrix = mtot(teta0,teta1,teta2,teta3);
% Le seguenti variabili possono esser utilizzate per un test sui valori
% restituiti dalla matrice inversa. E' possibile una loro visualizzazione
% rimuovendo il punto e virgola alla fine di ogni riga.
NX= matrix(1,1);
NY= matrix(2,1);
NZ= matrix(3,1);
SX= matrix(1,2);
SY= matrix(2,2);
SZ= matrix(3,2);
AX= matrix(1,3);
AY= matrix(2,3);
AZ= matrix(3,3);
PX= matrix(1,4);
PY= matrix(2,4);
PZ= matrix(3,4);
% Imposto i valori calcolati dalla matrice inversa nei rispettivi textbox
% per la visualizzazione a schermo
set(handles.nx,'String', num2str(matrix(1,1),'%6.2f'));
set(handles.ny,'String', num2str(matrix(2,1),'%6.2f'));
set(handles.nz,'String', num2str(matrix(3,1),'%6.2f'));
set(handles.sx,'String', num2str(matrix(1,2),'%6.2f'));
set(handles.sy,'String', num2str(matrix(2,2),'%6.2f'));
set(handles.sz,'String', num2str(matrix(3,2),'%6.2f'));
set(handles.ax,'String', num2str(matrix(1,3),'%6.2f'));
set(handles.ay,'String', num2str(matrix(2,3),'%6.2f'));
set(handles.az,'String', num2str(matrix(3,3),'%6.2f'));
set(handles.px,'String', num2str(matrix(1,4),'%6.2f'));
set(handles.py,'String', num2str(matrix(2,4),'%6.2f'));
set(handles.pz,'String', num2str(matrix(3,4),'%6.2f'));
%-------------------------------------------------------------------------
% !
In seguito troviamo le funzioni che implementano i textbox
%!
%!
%!
%!
%
%!
%!
%!
%
%!
%!
%!
%!
della matrice associata alla cinematica inversa. Il codice e'
generato in automatico dal tool GUIDE.
Per semplicità non verranno commentate, il lettore sappia che
la matrice e' così strutturata:
!
!
!
teta11!teta21!teta31!teta41
teta12!teta22!teta32!teta42
NOTA BENE
Il primo apice indica l'angolo, il secondo apice il numero
della soluzione.
%-------------------------------------------------------------------------
function teta11_Callback(hObject, eventdata, handles)
% hObject handle to teta11 (see GCBO)
% Hints: get(hObject,'String') returns contents of teta11 as text
%
str2double(get(hObject,'String')) returns contents of teta11 as a double
function teta11_CreateFcn(hObject, eventdata, handles)
%
end
%
%
end
%
%
end
%
%
end
%
%
end
%
%
end
%
%
end
%
%
end
%-------------------------------------------------------------------------
%!
%!
!
!
Eseguito alla pressione del tasto go 1
(invia la prima soluzione calcolata dalla cinematica inversa ai servi)
%-------------------------------------------------------------------------
function go1_Callback(hObject, eventdata, handles)
global speed1 speed2 speed3 speed4;
global s const Word0;
% Genera la stringa posizione dalla lettura della matrice inversa
word0 = num2str((str2num(get(handles.teta11,'String'))*(const/57.29577951))
+1500,'%6.0f');
+720,'%6.0f');
word2 = num2str((str2num(get(handles.teta31,'String'))*(-const/57.29577951))
+960,'%6.0f');
+1500,'%6.0f');
% Genero le stringhe da inviare ai servo motori
Word0 = ['#0P',word0,'S',speed1];
% Invio le stringhe precedentemente create ai servo motori
fprintf(s,Word0)
fprintf(s,Word1)
fprintf(s,Word2)
fprintf(s,Word3)
%-------------------------------------------------------------------------
%!
%!
!
!
Eseguito alla pressione del tasto go 2
(invia la seconda soluzione calcolata dalla cin. inversa ai servi)
%-------------------------------------------------------------------------
function go2_Callback(hObject, eventdata, handles)
global speed1 speed2 speed3 speed4;
global s const;
% Genera la stringa posizione dalla lettura della matrice inversa.
% Ricordiamo che i valori sommati, sono frutto di regolazioni empiriche del braccio.
+1500,'%6.0f');
+720,'%6.0f');
word2 = num2str((str2num(get(handles.teta32,'String'))*(-const/57.29577951))
+960,'%6.0f');
+1500,'%6.0f');
% Genero le stringhe da inviare ai servo motori
% Invio le stringhe precedentemente create ai servo motori
fprintf(s,Word0)
fprintf(s,Word1)
fprintf(s,Word2)
fprintf(s,Word3)
%-------------------------------------------------------------------------
%!
%!
!
!
Eseguito alla pressione del tasto 'calculate'
!
(calcola la cinematica inversa )
%-------------------------------------------------------------------------
function calculate_Callback(hObject, eventdata, handles)
global NX NY NZ SX SY SZ AX AY AZ PX PY PZ;
% inversa restituisce la matrice inversa memorizzo in inv.
inv = inversa(NX,NY,NZ,SX,SY,SZ,AX,AY,AZ,PX,PY,PZ);
% Imposto i textbox con i valori appena calcolati dalla matrice inversa
set(handles.teta11,'String', num2str(inv(1,1)*57.29577951,'%6.2f'));
File di avvio procedura
%------------------------------------------------------------------------%
script per il lancio in sequenza dei file del progetto
%------------------------------------------------------------------------% configurazione delle costanti e delle variabili relative all'hardware
init;
% Lancio il file che cura l'Interfaccia grafica e la comunicazione seriale
RobotArmControl;

bozza 3 - Ingegneria elettrica ed elettronica

Transcript

Documenti analoghi

Un Self Powered Neutron Detector (SPND) come monitor di

arco sport impugnature 2

prime only definitivo

vita parrocchiale / parish life

Outdoor Recreation Aviano AB Italy HOW TO GET A FISHING

FIAT QUBO 08

Praiano riconoscimento per Casa Angelina dal Daily Telegraph 21

- Provincia di Asti

SEE Electrical

Forging Shoah Memories