Dipartimento di Ingegneria Elettronica ed Elettrica di ...pisano/FlorisMarras_Tesina.pdf ·...

Robot Arm Lynxmotion lynx 6

Dipartimento di Ingegneria Elettronica ed Elettrica di Cagliari ! http://www.diee.unica.it/

Realizzato da Alessandro Floris 34395 & Andrea Marras 33641

http://diee.unica.it


IndiceIntroduzione 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



Robot Arm Lynxmotion



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: alimentazione elettronica di controllo, parte logica; (4)VS1: alimentazione di potenza servi 1-16; (6)VS2: 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:

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 alimentazione positiva 5V;Nero alimentazione massa;Bianco segnale di comando;



Un impulso della durata di 0.9mS porterà sempre il servo nella posizione angolare pari a -45° qualunque sia la sua posizione di partenza.

Un impulso della durata di 1.5mS porterà sempre il servo nella posizione centrale qualunque sia la sua posizione di partenza.

Un impulso della durata di 2.1mS porterà sempre il servo nella posizione angolare pari a +45° qualunque sia la sua posizione di partenza.



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:

Sostituendo a <num> un numero compreso fra 1 e 32 si seleziona il servo a cui inviare i dati:

Sostituendo a <num> un valore compreso fra 1 e 3000 si imposta la posizione angolare del servo.

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:

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:

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.

#<num>

P<num>

S<num>

#3 P600 S400 <cr>

#3 P500 #4 P350 #7 P2000 S400 <cr>





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 :

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)

>> guide





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:

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.

>> go





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: distanza lungo l’asse z;- ai: distanza tra due giunti lungo l’asse x (tutti pari a 12 cm);- θi: angolo lungo l’asse z, e il cui piano di rotazione è xy;- αi: 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.

La successiva funzione, la mtot.m, avrà il compito di moltiplicare le varie matrici di trasformazione omogenea per ottenere la funzione di trasformazione diretta.

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:

function [A] = mtrasf(a,z,d,x)

function [mtotale]= mtot(t1,t2,t3,t4)





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 :

θ3 presenta due valori, quindi si avranno conseguentemente due distinti θ2 definiti come θ21 e θ22.

θ2=atan2(s2,c2)





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:

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).

θ1 = -80° ÷ 80° θ2 = 0° ÷ 120° θ3 = -100° ÷ 47° θ4 = -107° ÷ 105°





Un esempio in cui compare il problema è la seguente configurazione:

θ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.

θ1 = 0° θ2 = -37° θ3 = -69° θ4 = 0°





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 [email protected] Floris [email protected]



mailto:[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.clcclear

% 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 radianticonst = 542.4;

%inizializzo il flag live a zerolive = 0;

% imposto gli angoliteta0 =0;teta1 =0;teta2 =0;teta3 =0;teta4 =0;teta5 =0;

% Valore centrale di default defaultMiddle = 1500;

% Velocitaʼ di default defaultSpeed = '400';

% Lunghezza braccia2 = 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;sliderValue4 = defaultMiddle;sliderValue5 = defaultMiddle;sliderValue6 = defaultMiddle;speed1 = defaultSpeed;speed2 = defaultSpeed;speed3 = defaultSpeed;speed4 = defaultSpeed;speed5 = defaultSpeed;speed6 = 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 omogeneab=[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 virgolaformatend





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 direttamtotale=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 = (pwz*((a3*c3)+a2))-(a3*s3_1*((c1*pwx)+(s1*pwy)));s2_2 = (pwz*((a3*c3)+a2))-(a3*s3_2*((c1*pwx)+(s1*pwy)));c2_1 = ((pwz)*(a3*s3_1))+(((a3*c3)+a2)*((c1*pwx)+(pwy*s1)));c2_2 = ((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 = (cos(t2_1)*cos(t3_1))-(sin(t2_1)*sin(t3_1));c23_2 = (cos(t2_2)*cos(t3_2))-(sin(t2_2)*sin(t3_2));s23_1 = (sin(t2_1)*cos(t3_1))+(sin(t3_1)*cos(t2_1));s23_2 = (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 = +sqrt(1-c4_1^2);s4_12 = -sqrt(1-c4_1^2);s4_21 = +sqrt(1-c4_2^2);s4_22 = -sqrt(1-c4_2^2);

% Calcolo dei quattro t4

t4_11 = atan2(s4_11,c4_1);t4_12 = atan2(s4_12,c4_1);t4_21 = atan2(s4_21,c4_2);t4_22 = 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 = mtot(t1,t2_1,t3_1,t4_11);ver_12 = mtot(t1,t2_2,t3_2,t4_22);ver_21 = mtot(t1,t2_1,t3_1,t4_12);ver_22 = mtot(t1,t2_2,t3_2,t4_21);

% Viene richiamata la funzione controllo per la matrice ver_11controllo1=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.

controllo2=controllo(ver_12(1,4),ver_12(2,4),ver_12(3,4), px,py,pz);


% 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 == 0controllo3 = controllo(ver_21(1,4),ver_21(2,4),ver_21(3,4), px,py,pz);end


% Stesso discorso va fatto con ver_22

controllo4=0;

if controllo2==0controllo4=controllo(ver_22(1,4),ver_22(2,4),ver_22(3,4), px,py,pz);end


% Le 2 soluzioni rimanenti (non valide) vengono quindi impostate a zero.

inv_21 = [0,0,0,0];inv_22 = [0,0,0,0];

% UscitaInv = [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 endend

% 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 RobotArmControlhandles.output = hObject;

% Update handles structureguidata(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% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Get default command line output from handles structurevarargout{1} = handles.output;

%-------------------------------------------------------------------------% ! ! Eseguito al movimento dello slider1%! ! ! ( posizione servo 1) %-------------------------------------------------------------------------





function slider1_Callback(hObject, eventdata, handles)global sliderValue1 live s speed1;global teta0 const;

% se il flag live e' attivoif(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 graficaguidata(hObject, handles);

% Genero la stringa da spedire al servoword = ['#0P',num2str(sliderValue1,'%6.2f'),'S',speed1];

% Invio la stringa al servofprintf(s,word)

% Aggiorno la matrice della cinematica diretta chiamando la funzione% upggradematrix (vedi piu' avanti)upgradematrix(hObject, eventdata, handles)

% Se il flag live e' disattivatoelse

% leggo il valore dello slider e lo memorizzo in sliderValue1sliderValue1 = 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 teta0 = (sliderValue1-1500)/const;





% Aggiorno la matrice della cinematica diretta chiamando la funzione% upggradematrixupgradematrix(hObject, eventdata, handles)

% Aggiorno l'interfaccia graficaguidata(hObject, handles);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)% eventdata reserved - to be defined in a future version of MATLAB% 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

%-------------------------------------------------------------------------% ! ! Eseguito al movimento dello slider2%! ! ! ( posizione servo 2) %-------------------------------------------------------------------------% Per i commenti sul codice vedere slider1

function slider2_Callback(hObject, eventdata, handles)global sliderValue2 live s speed2 teta1 const;

if(live)sliderValue2 = get(handles.slider2,'Value');set(handles.edit2,'String', num2str((((sliderValue2-720)/const)*57.29577951),'%6.2f'));teta1 = (sliderValue2-720)/const;guidata(hObject, handles);word = ['#1P',num2str(sliderValue2,'%6.0f'),'S',speed2];fprintf(s,word)upgradematrix(hObject, eventdata, handles)

elsesliderValue2 = get(handles.slider2,'Value');set(handles.edit2,'String', num2str((((sliderValue2-720)/const)*57.29577951),'%6.2f'));teta1 = (sliderValue2-720)/const;upgradematrix(hObject, eventdata, handles)guidata(hObject, handles);end


% --- Executes during object creation, after setting all properties.function slider2_CreateFcn(hObject, eventdata, handles)





% hObject handle to slider2 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called




if(live)sliderValue3 = get(handles.slider3,'Value');set(handles.edit3,'String', num2str((((sliderValue3-960)/const)*57.29577951),'%6.2f'));teta2 = (sliderValue3-960)/const;guidata(hObject, handles);word = ['#2P',num2str(1920-sliderValue3,'%6.0f'),'S',speed3];fprintf(s,word)upgradematrix(hObject, eventdata, handles)


















if(live)sliderValue5 = get(handles.slider5,'Value');set(handles.edit5,'String', num2str((((sliderValue5-1500)/const)*57.29577951),'%6.2f'));





teta4 = (sliderValue5-1500)/const;guidata(hObject, handles);word = ['#4P',num2str(sliderValue5,'%6.0f'),'S',speed5];fprintf(s,word)upgradematrix(hObject, eventdata, handles)








elsesliderValue6 = get(handles.slider6,'Value');set(handles.edit6,'String', num2str((((sliderValue6-1500)/const)*57.29577951),'%6.2f'));teta5 = (sliderValue6-1500)/const;upgradematrix(hObject, eventdata, handles)guidata(hObject, handles);





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)% hObject handle to slider1 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% --- Executes on key press with focus on slider1 and none of its controls.function slider1_KeyPressFcn(hObject, eventdata, handles)% hObject handle to slider1 (see GCBO)% 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% handles structure with handles and user data (see GUIDATA)%sliderValu5---------------------------------------------------VERIFICA

%-------------------------------------------------------------------------% ! ! Eseguito con la modifica del textbox 1%! ! ! ( posizione servo 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





teta0 = (sliderValue1-1500)/const;

% Aggiorno l'interfaccia graficaguidata(hObject, handles);


% --- Executes during object creation, after setting all properties.function edit1_CreateFcn(hObject, eventdata, handles)% hObject handle to edit1 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called

% 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

%-------------------------------------------------------------------------% ! ! Eseguito con la modifica del textbox 2%! ! ! ( posizione servo 2) %-------------------------------------------------------------------------% Per i commenti sul codice vedere textbox 1


sliderValue2 = ((str2num(get(handles.edit2,'String')))*(const/57.29577951))+720;set(handles.slider2,'Value',sliderValue2);teta1 = (sliderValue2-720)/const;guidata(hObject, handles);

















%-------------------------------------------------------------------------% ! ! Eseguito con la modifica del textbox 5%! ! ! ( posizione servo 5)





%-------------------------------------------------------------------------% Per i commenti sul codice vedere textbox 1










%-------------------------------------------------------------------------





% ! ! Eseguito con la modifica del textbox 7%! ! ! ( velocita'servo 1) %-------------------------------------------------------------------------

function edit7_Callback(hObject, eventdata, handles)global speed1;

% imposto la variabile speedX con il valore letto nel textboxspeed1 = (get(hObject,'String'));

% Codice generato in automatico dal tool GUIDE

% hObject handle to edit7 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit7 as text% str2double(get(hObject,'String')) returns contents of edit7 as a double

% --- Executes during object creation, after setting all properties.function edit7_CreateFcn(hObject, eventdata, handles)

% hObject handle to edit7 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called

% 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

%-------------------------------------------------------------------------% ! ! Eseguito con la modifica del textbox 8%! ! ! ( velocita'servo 2) %-------------------------------------------------------------------------% Per i commenti sul codice vedere textbox 7


speed2 = get(hObject,'String');


% Hints: get(hObject,'String') returns contents of edit8 as text





% str2double(get(hObject,'String')) returns contents of edit8 as a double










%-------------------------------------------------------------------------% ! ! Eseguito con la modifica del textbox 10%! ! ! ( velocita'servo 4) %-------------------------------------------------------------------------





% Per i commenti sul codice vedere textbox 7












% --- Executes during object creation, after setting all properties.function edit11_CreateFcn(hObject, eventdata, handles)

% hObject handle to edit11 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB





% handles empty - handles not created until after all CreateFcns called









%-------------------------------------------------------------------------% ! ! 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 vettorevalueCom = get(hObject,'String');

% preleviamo il primo valore del vettore (valore selezionato dell'utente )% e lo memorizziamo in com com = valueCom(get(hObject,'Value'),1);


% --- Executes during object creation, after setting all properties.function popupmenu1_CreateFcn(hObject, eventdata, handles)% hObject handle to popupmenu1 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called

% Hint: popupmenu 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

%-------------------------------------------------------------------------% ! ! 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 vettorevalueBaudRate = 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 numerobaudratecustom = str2num(baudrate);


% --- Executes during object creation, after setting all properties.function popupmenu2_CreateFcn(hObject, eventdata, handles)% hObject handle to popupmenu2 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called





% Hint: popupmenu 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

%-------------------------------------------------------------------------% ! ! 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 sliderValue1 sliderValue2 sliderValue3 sliderValue4 sliderValue5 sliderValue6;global speed1 speed2 speed3 speed4 speed5 speed6 word0;

% Se il flag live e' disattivato, genero le stringhe da inviare ai servi e le invio % immediatamenteif(live == 0)word0 = ['#0P',num2str(sliderValue1,'%6.0f'),'S',speed1];word1 = ['#1P',num2str(sliderValue2,'%6.0f'),'S',speed2];

% ---> * * * 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];

word3 = ['#3P',num2str(sliderValue4,'%6.0f'),'S',speed4];word4 = ['#4P',num2str(sliderValue5,'%6.0f'),'S',speed5];word5 = ['#5P',num2str(sliderValue6,'%6.0f'),'S',speed6];

% invio le stringhe ai servifprintf(s,word0)fprintf(s,word1)fprintf(s,word2)fprintf(s,word3)fprintf(s,word4)fprintf(s,word5)

%aggiorno la cinematica diretta (vedi piu' avanti)upgradematrix(hObject, eventdata, handles)

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)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% 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;global sliderValue1 sliderValue2 sliderValue3 sliderValue4 sliderValue5 sliderValue6;

% 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

set(handles.slider1,'Value',sliderValue1);set(handles.slider2,'Value',sliderValue2);set(handles.slider3,'Value',sliderValue3);set(handles.slider4,'Value',sliderValue4);set(handles.slider5,'Value',sliderValue5);set(handles.slider6,'Value',sliderValue6);

% Creo ed configuro la porta seriales = 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! ! sx! ! ax! ! pz%! ! ny ! ! sy ! ! ay! ! py%! ! nz! ! sz! ! az! ! pz%! ! edit25!! edit 26! edit 27! 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)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of nx as text% str2double(get(hObject,'String')) returns contents of nx as a double

% --- Executes during object creation, after setting all properties.function nx_CreateFcn(hObject, eventdata, handles)% hObject handle to nx (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


function sx_Callback(hObject, eventdata, handles)% hObject handle to sx (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of sx as text% str2double(get(hObject,'String')) returns contents of sx as a double





% --- Executes during object creation, after setting all properties.function sx_CreateFcn(hObject, eventdata, handles)% hObject handle to sx (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


function ax_Callback(hObject, eventdata, handles)% hObject handle to ax (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of ax as text% str2double(get(hObject,'String')) returns contents of ax as a double

% --- Executes during object creation, after setting all properties.function ax_CreateFcn(hObject, eventdata, handles)% hObject handle to ax (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


function px_Callback(hObject, eventdata, handles)% hObject handle to px (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of px as text% str2double(get(hObject,'String')) returns contents of px as a double

% --- Executes during object creation, after setting all properties.function px_CreateFcn(hObject, eventdata, handles)





% hObject handle to px (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


function ny_Callback(hObject, eventdata, handles)% hObject handle to ny (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of ny as text% str2double(get(hObject,'String')) returns contents of ny as a double

% --- Executes during object creation, after setting all properties.function ny_CreateFcn(hObject, eventdata, handles)% hObject handle to ny (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


function ay_Callback(hObject, eventdata, handles)% hObject handle to ay (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of ay as text% str2double(get(hObject,'String')) returns contents of ay as a double

% --- Executes during object creation, after setting all properties.function ay_CreateFcn(hObject, eventdata, handles)% hObject handle to ay (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called






function py_Callback(hObject, eventdata, handles)% hObject handle to py (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of py as text% str2double(get(hObject,'String')) returns contents of py as a double

% --- Executes during object creation, after setting all properties.function py_CreateFcn(hObject, eventdata, handles)% hObject handle to py (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


function nz_Callback(hObject, eventdata, handles)% hObject handle to nz (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of nz as text% str2double(get(hObject,'String')) returns contents of nz as a double

% --- Executes during object creation, after setting all properties.function nz_CreateFcn(hObject, eventdata, handles)% hObject handle to nz (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called

% 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

function sz_Callback(hObject, eventdata, handles)% hObject handle to sz (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of sz as text% str2double(get(hObject,'String')) returns contents of sz as a double

% --- Executes during object creation, after setting all properties.function sz_CreateFcn(hObject, eventdata, handles)% hObject handle to sz (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


function az_Callback(hObject, eventdata, handles)% hObject handle to az (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of az as text% str2double(get(hObject,'String')) returns contents of az as a double

% --- Executes during object creation, after setting all properties.function az_CreateFcn(hObject, eventdata, handles)% hObject handle to az (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called






function pz_Callback(hObject, eventdata, handles)% hObject handle to pz (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of pz as text% str2double(get(hObject,'String')) returns contents of pz as a double

% --- Executes during object creation, after setting all properties.function pz_CreateFcn(hObject, eventdata, handles)% hObject handle to pz (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


function edit25_Callback(hObject, eventdata, handles)% hObject handle to edit25 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)




function edit26_Callback(hObject, eventdata, handles)% hObject handle to edit26 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB





% handles structure with handles and user data (see GUIDATA)
















function sy_Callback(hObject, eventdata, handles)% hObject handle to sy (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of sy as text% str2double(get(hObject,'String')) returns contents of sy as a double

% --- Executes during object creation, after setting all properties.function sy_CreateFcn(hObject, eventdata, handles)% hObject handle to sy (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called


%-------------------------------------------------------------------------%%! ! Funzione utilizzata per aggiornare la matrice inversa a video%%-------------------------------------------------------------------------

function upgradematrix(hObject, eventdata, handles)global sliderValue1 sliderValue2 sliderValue3 sliderValue4 sliderValue5 sliderValue6;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

teta0 = (sliderValue1-1500)/const;teta1 = (sliderValue2-720)/const;teta2 = (sliderValue3-960)/const;teta3 = (sliderValue4-1500)/const;teta4 = (sliderValue5-1500)/const;teta5 = (sliderValue6-1500)/const;

% La funzione mtot restituisce la matrice inversa e la memorizza in matrixmatrix = 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)% eventdata reserved - to be defined in a future version of MATLAB% handles structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of teta11 as text% str2double(get(hObject,'String')) returns contents of teta11 as a double

% --- Executes during object creation, after setting all properties.function teta11_CreateFcn(hObject, eventdata, handles)% hObject handle to teta11 (see GCBO)% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called




% --- Executes during object creation, after setting all properties.function teta21_CreateFcn(hObject, eventdata, handles)% hObject handle to teta21 (see GCBO)





% eventdata reserved - to be defined in a future version of MATLAB% handles empty - handles not created until after all CreateFcns called









% 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








% 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













%-------------------------------------------------------------------------%! ! ! 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 inversaword0 = num2str((str2num(get(handles.teta11,'String'))*(const/57.29577951))+1500,'%6.0f');word1 = num2str((str2num(get(handles.teta21,'String'))*(const/57.29577951))+720,'%6.0f');word2 = num2str((str2num(get(handles.teta31,'String'))*(-const/57.29577951))+960,'%6.0f');word3 = num2str((str2num(get(handles.teta41,'String'))*(const/57.29577951))+1500,'%6.0f');

% Genero le stringhe da inviare ai servo motoriWord0 = ['#0P',word0,'S',speed1];Word1 = ['#1P',word1,'S',speed2];Word2 = ['#2P',word2,'S',speed3];Word3 = ['#3P',word3,'S',speed4];

% Invio le stringhe precedentemente create ai servo motorifprintf(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.word0 = num2str((str2num(get(handles.teta12,'String'))*(const/57.29577951))+1500,'%6.0f');word1 = num2str((str2num(get(handles.teta22,'String'))*(const/57.29577951))+720,'%6.0f');word2 = num2str((str2num(get(handles.teta32,'String'))*(-const/57.29577951))+960,'%6.0f');





word3 = num2str((str2num(get(handles.teta42,'String'))*(const/57.29577951))+1500,'%6.0f');

% Genero le stringhe da inviare ai servo motoriWord0 = ['#0P',word0,'S',speed1];Word1 = ['#1P',word1,'S',speed2];Word2 = ['#2P',word2,'S',speed3];Word3 = ['#3P',word3,'S',speed4];

% Invio le stringhe precedentemente create ai servo motorifprintf(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'));set(handles.teta21,'String', num2str(inv(1,2)*57.29577951,'%6.2f'));set(handles.teta31,'String', num2str(inv(1,3)*57.29577951,'%6.2f'));set(handles.teta41,'String', num2str(inv(1,4)*57.29577951,'%6.2f'));

set(handles.teta12,'String', num2str(inv(2,1)*57.29577951,'%6.2f'));set(handles.teta22,'String', num2str(inv(2,2)*57.29577951,'%6.2f'));set(handles.teta32,'String', num2str(inv(2,3)*57.29577951,'%6.2f'));set(handles.teta42,'String', num2str(inv(2,4)*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'hardwareinit;

% Lancio il file che cura l'Interfaccia grafica e la comunicazione seriale RobotArmControl;





Dipartimento di Ingegneria Elettronica ed Elettrica di ...pisano/FlorisMarras_Tesina.pdf ·...

Documents

Transcript of Dipartimento di Ingegneria Elettronica ed Elettrica di ...pisano/FlorisMarras_Tesina.pdf ·...