Home

Documento PDF

1. Figura 2 3 Schema elettrico della scheda d interfaccia La scheda viene alimentata a 15V e viene collegata all alimentatore con gli appositi connettori in posizione 6 A In posizione 6 D C B rappresentato dove viene collegato il connettore piatto da 50 pin degli ingressi e delle uscite analogiche della Sensoray 626 in posizione 5 C B A il connettore piatto da 50 pin degli ingressi e delle uscite digitali 2 2 LAYOUT GENERALE 15 Tutti gli ingressi e le uscite utilizzati vengono collegati tramite apposite piste ad un bus a 26 vie che fa comunicare la schede con le due schede driver che sempre un connettore piatto a 26 pin posizione 4 3 D C Le uscite analogiche disponibili perch non utilizzate per questo robot sono state portate ad un apposito connettore in posizione 5 6 C Il connettore da 26 pin degli encoder viene collegato in posizione 2 D C Tiene fino a tre encoder diversi che vengono collegati separati per comodit e sono stati previsti i collegamenti per tutti e tre anche se ne vengono utilizzati solo 2 Sotto a questi connettori prevista alimentazione per gli encoder che viene ovviamente collegata a tutti e tre I finecorsa induttivi vengono letti in posizione 5 4 3 A B e i loro segnali vanno a pilotare la base di un transtor npn il valore del segnale viene poi letto da col lettore e collegati ai connettori della Sensoray 626 come uscite p
2. 0 gt iteratore_link1 Figura 3 35 Subsystem emergenze temperature gt emergenza bisogna ricalibrare il robot una volta terminata Quindi in questo sottosistema avvengono le seguenti assegnazioni e Stato 0 per entrare nello stato di emergenza e calibrazione 2 che il valore che indica manipolatore calibrato e iteratore link 0 per far riniziare correttamente algoritmo di interpo lazione della posizione del link 2 e iteratore_link1 0 per far riniziare correttamente algoritmo di interpo lazione della posizione del link 1 Quando I uscita della porta or torner a zero emergenza sar finita e si riprender il ciclo dell macchina a stati 3 2 DESCRIZIONE DEL PROGRAMMA 67 La seconda categoria di emergenza considerata in emergenza alimentazione finecorsa meccanici pulsante emergenza Figura 3 36 Si prende in considerazione una sola variabile emergenza_meccanica e quindi non t0 function CONTROLLO EMERGENZA MECCANICA emergenza_meccanica 4 emergenza Figura 3 36 Subsystem emergenze emergenza alimentazione finecorsa meccanici pulsante emergenza presente la porta OR del caso precedente ma la logica la stessa quando questa emergenza si attiva assume il valore 1 e va ad abilitare il subsystem emergenza Questa tipologia di emer
3. Figura 2 8 Schema di un controllo in retroazione con azione in avanti e driver in corrente 2 3 2 Controllo Centralizzato Il controllo centralizzato si basa sul principio di controllare ogni link in base anche ai movimenti degli altri Infatti su robot a presa diretta come questo opportuno non considerare dei sem plici disturbi gli effetti di accoppiamento dei link ma compensarli nell erogazione della coppia del motore Per far questo per necessaria una conoscenza maggiore del modello dinamico del robot Figura 2 9 Come si pu vedere i feedback di posizione convergono tutti in unica struttura centralizzata e non ognuno in un controllore decentralizzato quindi i riferimenti vengono calcolati da una strategia unica che mira ad un effetto globale In altri termini se ad esempio previsto che un link stia fermo ma che gli altri si muovano la coppia erogata su quel link non sar nulla ma tiene conto degli effetti che gli altri link hanno I feedback a disposizione sono di posizione risultato quindi conveniente realiz zare un controllo a dinamica inversa 22 2 ARCHITETTURA PER IL CONTROLLO POSIZIONE REALE IF T RI mef a gt MOTORE 2 POSIZIONI PIANIFICATE RIFERIMENTO gt C gt MOTORE 1 RIFERIMENTO prvero MOTORE 0 POSIZIONE REALE Figura 2 9 Schema del controllo centralizzato del robot Questo metodo tratto dallo studio nel libro di testo Robotica 2
4. simulazione del tempo per la pianificazione 10 Function Call Generator function calcolo posizione function controllo fine pianificazione Figura 3 13 Subsystem Calcolo riferimento calibrazione posizione di home calcolo posizione link 2 e 1 Il sottosistema caratterizzato da un funnction call generator che ordina tre sottosistemi per effettuare il calcolo della pianificazione La pianificazione scelta quella a profilo trapezoidale di velocit prevede un tempo totale T_calibrazione 2 s un tempo di accelerazione ta 0 6 s un tempo di decelerazione td 0 6 s questi valori vengono letti dal file Matlab Nel primo function call subsystem simulazione del tempo per la pianificazione Figura 3 14 viene incrementata la variabile t del valore corrispondente ad ogni ciclo di simu lazione quindi questa variabile serve a simulare il tempo che passa a partire da quando inizia la pianificazione cio come se la pianificazione partisse all istante zero Questo permette i calcoli corretti 3 2 DESCRIZIONE DEL PROGRAMMA 43 t0 function 0 001 Figura 3 14 Subsystem Calcolo riferimento calibrazione posizione di home allineamento link2e1 calcolo posizione link 2 e 1 gt simulazione del tempo per la pianificazione Nel secondo function call subsy
5. 2 gt calibrazione 1 gt emergenza_posizione 0 Pi iteratore_link2 0 iteratore_link1 Figura 3 42 Subsystem emergenze controllo posizione gt emergenza Stato 0 per entrare nello stato di emergenza calibrazione 2 per indicare che il robot gi calibrato e ad emergenza finita 3 2 DESCRIZIONE DEL PROGRAMMA 75 non bisognera ricalibrare emergenza_posizione 1 per indicare quale emergenza stata generata teratore_link1 ed iteratore_link2 0 perch se si era nello stato 4 e quindi si stava interpolando l algoritmo deve essere resettato Quest emergenza non prevede quindi la ricalibrazione ma solo il bloccaggio del manipolatore fino al termine dell emergenza La quinta categoria di emergenze considerata in controllo finecorsa indut tivi Figura 3 43 Al suo interno troviamo un subsystem enabled che viene attivato solo se la va LO function Stato h 4 a finecorsainduttivi Figura 3 43 Subsystem emergenze controllo finecorsa induttivi riabile Stato 4 Questa condizione viene posta perch i finecorsa induttivi come stato preceden temente spiegato vengono utilizzati dall algoritmo di calibrazione e quindi fanno 76 3 CONTROLLO DEL ROBOT attivare lo stato di emergenza solo se siamo nello stat
6. encoder_link1 che corrispondono alle uscite delle retroazioni degli schemi di controllo in cui sono stati aggiunti la funzione di trasferimento del motore per poterlo simulare I tre ingressi analogici per leggere i monitor di corrente degli LMD dei tre motori dato che non sono stati utilizzati era indifferente come venivano utilizzati Figura 4 1 La figura mostra solo un esempio delle diverse simulazioni che sono state prova Figura 4 1 Subsystem Ingressi Sensoray 626 per la simulazione te Una prima verifica stata osservare se lo logica della macchina funzionasse e quindi ci fosse il passaggio attraverso tutti gli stati Quindi sia i passaggi generati internamente al programma sia quelli comandati dal programma Matlab 4 1 SIMULAZIONI 107 La verifica in realt visiva durante l esecuzione per i cambiamenti che avven gono sull interfaccia al cambiamento da uno stato all altro Si mostra comunque il grafico della variabile Stato esplicativo di quali possono essere i passaggi Figura 4 2 Figura 4 2 Grafico della variabile Stato Fondamentale stato simulare attivazione dei finecorsa induttivi necessari per la
7. theta_in quindi delta_theta 0 che moltiplicato per un qualsiasi valore dell iteratore da sempre zero e quindi il riferimento rimane fermo all ul tima posizione inviata da Matlab Esattamente analogo quello che avviene per calcolare il riferimento per il link 1 Viene usata la stessa logica e gli stessi blocchetti cambiando nei nomi delle variabili il numero 2 con il numero 1 Per il calcolo del riferimento nel link 0 si passa al quarto function call subsystem posizione link 0 Figura 3 31 viene calcolata la differenza tra la posizione inviata da Matlab posizione_link0 e la posizione in cui si trova la base mobile posizione_link0_attuale considerate sempre in numero di passi cos il risultato proprio il numero di passi che deve fare il motore stepper Allora il modulo della differenza viene salvato nella variabile passi link0 ed il numero di passi che viene inviato al controllo che si occuper di farli eseguire A seconda se era risultato un numero positivo o negativo tramite un if viene assegnato alla variabile direzione_link0 rispettivamente 1 o 0 Termina cos la spiegazione del function call subsystem calcolo riferimento Ora vengono spiegati tutti gli altri sottosistemi della figura 3 1 in ordine dal pi alto al pi basso che l ordine secondo il quale vengono eseguiti da Simulink 3 2 2 Ingressi Sensoray626 Il primo Function Call Subsystem prevede la lett
8. dove si trova il link realmente in quel primo periodo Quindi per il primo centesi mo di secondo la posizione finale e iniziale coincidono e questo garantisce che non si debbano mai interpolare posizione troppo lontane tra di loro ma solo quello previste dalla pianificazione Nel secondo function call subsystem calcolo del delta theta Figura 3 29 viene fatta la sottrazione tra theta_fin e theta_in e il risultato diviso per 10 LO function CALCOLO DELTA THETA theta_fin Ne rad delta_theta 0 m theta_in Figura 3 29 Subsystem Calcolo riferimento calibrazione funzionamento normale gt theta di riferimento del link2 calcolo del delta theta questo viene salvato nella variabile delta theta Cos abbiamo trovato il valore costante con cui si pu incrementare la posizione iniziale per raggiungere quella finale in 10 volte In pratica consiste nel rendere 10 volte pi piccola l ampiezza del gradino che verrebbe dato come riferimento al controllore Nel terzo function call subsystem calcolo riferimento pid link2 Figura 3 30 3 2 DESCRIZIONE DEL PROGRAMMA 59 viene calcolato il riferimento di posizione per il controllore secondo I algoritmo ui CALCOLO RIFERIMENTO PER IL CONTROLLO theta_fin SCOPE CHE MOSTRA IL CORRETTO FUNZIONAMENTO DELL ALGOR
9. 1 elseif posizione di home Figura 3 5 Subsystem Calcolo riferimento gt calibrazione zione letta dagli encoder a quella assoluta Fatta questa operazione per tutti e tre i link vengono portati contemporanea mente nella posizione di calibrazione detta posizione di home Viene utilizzata una nuova variabile calibrazione che serve a passare dalla cali brazione di un link ad un altro Se calibrazione 2 si sta calibrando il link 2 Figura 3 6 Viene dato un incremento a posizione link di 0 02 convertito in radianti rappresentato dalla costante incremento_calibrazione e con i blocchi utilizzati questo incremento viene dato ogni volta alla posizione che stata incrementata precedentemente fornendo I effetto desiderato La posizione viene incrementata ogni ciclo di simulazione quindi ogni 0 001 s L incremento volutamente piccolo perch questa fase non deve dare problemi di controllo e deve essere il pi preciso possibile 34 3 CONTROLLO DEL ROBOT if Action Port ALGORITMO INIZIALIZZAZIONE LINK 2 TEA n rad eR ee posizione_link2 wa posizione link2 incremento_calibrazione finecorsa_dx_link2 pe attivazione finecorsa Figura 3 6 Subsystem Calcolo riferimento gt calibrazione chiusura gomito Come si pu notare dalla figura l incremento di posizione viene sott
10. o function theta_in theta_fin delta_theta function calcolo di theta finale e iniziale Function Call Generator function calcolo del delta theta function calcolo riferimento Figura 3 27 Subsystem Calcolo riferimento calibrazione funzionamento normale gt theta di riferimento del link2 dall interpolazione stato necessario strutturare il suo interno con un altro function call generator infatti anche se la logica molto semplice il numero di blocchetti utilizzati e scrivendoli ordinati dall alto verso il basso non venivo eseguiti in ordine corretto da Simulink Questo viene specificato perch soprattutto all inizio pu essere fonte di molti errori Per effettuare l interpolazione si considera la nuova posizione da raggiungere e la posizione di partenza in cui si trova il link Queste vengono memorizzate nel primo function call subsystem calcolo di theta finale e iniziale Figura 3 28 La nuova posizione banalmente quella appena inviata da Matlab quella attuale si ottiene inserendo 10 blocchi memory in uscita dalla variabile posizione_link2 3 2 DESCRIZIONE DEL PROGRAMMA 57 10 function MEMORIZZAZIONE POSIZIONE INIZIALE E FINALE conteggio2 e gt conteggio2 rad posizione_link2 P the
11. e encoder link e encoder link2 e Stato e posizione link e posizione_link2 con ovvio significato della variabili Vengono inviati i due riferimenti di posizione reali del link 1 e 2 e la posizione 3 2 DESCRIZIONE DEL PROGRAMMA 103 del link 0 che sono i dati con i quali il programma Matlab va a rappresentare il manipolatore nell interfaccia Quindi all utente appare il reale movimento e non quello pianificato Ovviamente per il motore passo lavora in catena aperta e quindi non si ha il feedback di posizione e allora la posizione inviata e utilizzata per la rappresenta zione quella pianificata Si inviano inoltre le posizioni pianificate dei motori in corrente continua per da re la possibilit al programma Matlab di tracciare nell interfaccia la traiettoria desiderata e la si pu confrontare con il movimento reale dei link Per il link 0 questo non ha senso che venga fatto Infine viene inviato lo stato della macchina a stati finiti per permettere anche al programma Matlab di sapere in quale situazione si trova il robot L invio di questo dato indispensabile in quanto fa parte degli algoritmi con cui viene modificata l interfaccia Capitolo 4 Test Per verificare il corretto funzionamento del programma sono state previste di verse prove sia solo a livello software sia solo con le schede elettroniche Non stato possibile provare il programma in modo completo su
12. e Stato 2 calibrazione vengono calibrati tutti e tre i link e Stato 3 gt calibrato il robot ha finito la calibrazione adesso pu entrare nel funzionamento normale e Stato 4 in funzione il robot si muove a seconda dei comandi dell utente Stato 0 emergenza Quando si verifica un problema nel funzionamento che discuteremo nel Function call subsystem emergenze lo stato viene posto a zero e si passa attraverso que sto subsystem che non modifica direttamente i riferimenti ma come vedremo pi avanti impone che la tensione erogata ai motori sia zero e fa entrare l elettronica 30 3 CONTROLLO DEL ROBOT nel circuito d emergenza Figura 3 3 Al suo interno viene assegnato il valore 1 alla variabile emergenza che la 9 case STATO DI EMERGENZA Action Port 1 emergenza temp0a P gt temp0b P gt tempi gt 0 temp2 P gt emergenza_velocita gt NOR emergenza_induttivite2_ ____ Il emergenza_meccanica _ __ fine stato di emergenza emergenza_encoder _____ gt emergenza induttivo 2 __ Figura 3 3 Subsystem Calcolo riferimento gt emergenza variabile che verr mandata all elettronica e che quindi permetter di entrare nel circuito d emergenza Ed present
13. si basa sull individuazione di una legge di controllo funzione dello stato del manipolatore che linearizzi intero sistema e che sia stabilizzante La legge di controllo diventa u B g d n q d 2 6 con u vettore di controllo B q matrice delle inerzie j vettore delle accelerazioni controllate e n q q vettore dei termini di Coriolis A tal fine stato calcolato il vettore delle accelerazioni come i da Kp da 9 Kp da 9 2 7 con gq vettore delle accelerazioni desiderate per i motori ga g vettore degli errori di velocit dei motori ga q vettore degli errori di posizione dei motori e Kp Kp matrici diagonali Le matrici di controllo diagonali rendono il sistema disaccoppiato e quindi ad un errore di posizione di un motore corrisponder la correzione della componente di accelerazione del motore stesso Per calcolare i valori dei guadagni di queste matrici si partiti dai valori teorici Kp diag 2 wni 0 2EWnn t Kp diag w w 2 8 k nn 2 3 TEORIA DEL CONTROLLO 23 e poi sono stati tarati sperimentalmente osservando i grafici dell inseguimento di posizione Il vettore delle accelerazioni ottenuto di dimensioni 2x1 in quanto i motori in catena chiusa da controllare sono 2 ma per il calcolo delle coppie viene aggiunta anche I accelerazione del link 0 in quanto anche questa va ad influire nel modello dinamico Non avendo nessun feedback di posizione viene utilizzat
14. stato descritto nell algoritmo di interpolazione gli iteratori si azzerano ogni volta che vengono ricevuti i dati ed inoltre se si memorizzassero i dati anche negli istanti in cui non vengono ricevuti si rischierebbe di memorizzare dei valori errati come degli zeri 80 3 CONTROLLO DEL ROBOT m function mm AA posizione link0 rad y posizione link 1 rad v posizione link 2 P driver_link2 AA driver_link1 P azione in avanti RICEZIONE DEI DATI DALL INTERFACCIA MATLAB Pj controllo v stato desiderato Packet Input Pj end effector mm cs y vel link 0 mm cs 2 P ace link 0 rad cs Pj vel ink 1 rad es 2 Pacc link 1 rad cs P vel link 2 rad es 2 P ace link 2 gt double ricezione_dati memorizzazione dati matlab Packet Input Standard Devices UDP Protocol DO8h Figura 3 47 Subsystem ricezione dati matlab Come gi stato spiegato i dati vengono inviati da Matlab attraverso l ogget to timer alla frequenza di 100 Hz ma questo non corrisponde a ricevere i dati a quella frequenza Infatti alcuni pacchetti dati vengono persi per i problemi di rete e relativi al protocollo di trasmissione utilizzato altre volte si accumulano nel buffer e poi vengono letti ad intervalli di tempo anche minori al centesimo di se
15. 2 B 1 1 DA m0112 cos 2 Oz Imyalysen O2 O O92 FE moa losen Q O A 5 Queste sono le coppie che agiscono rispettivamente nel link 2 e nel link 1 Per calcolare la coppia che devono erogare esattamente i due motori si utilizza il principio dei lavori virtuali C m20O m2 Tin2 m20Om2 r Ce A 6 Cmid Ori Im10m100Om1 ci dividendo poi le due equazioni rispettivamente per 60 3 60 1 e considerando che i motori lavorano in presa diretta quindi sia i rapporti di trasmissione sia i rendimenti sono pari ad 1 si ottiene Cm2 Ime In 4 Oo C A 7 Cmi Imi tio Mma 2m30119c08 03 Iz 4 O1 Ct Si fa presente che il secondo termine in entrambe le equazioni rappresenta la coppia resistente che agisce nel rispettivo motore Ora viene aggiunta l influenza della base mobile La posizione e la velocita della base mobile non vanno ad influenzare la coppia da erogare ma la sua accelerazione esercita una forza in direzione opposta su entrambi i membri Queste forze sono rappresentate in figura come se agissero sul baricentro dei due membri Ricordando la prima legge di Newton l accelerazione quella della base mobile e la massa quella dei membri La coppia data dalla forza per il braccio quindi il contributo del carrello in termini 120 A ANALISI DINAMICA INVERSA DEL MANIPOLATORE Figura A 2 Schema manipolatore con le forze agenti dovute all accelerazione della b
16. CONTROLLO POSIZIONE rad encoder_link2 Pl gt ju sa gt Au 10 kg2r elseif ut 1 a Action Action posizione_link2 emergenza no emergenza rad encoder_link1 E gt ju ivo gt Au 10 kg2r elseif ut 1 a Action Action posizione_link1 emergenza no emergenza Figura 3 41 Subsystem emergenze controllo posizione tana dalla posizione desiderata Se questo succede infatti significa che siamo andati fuori controllo e c il rischio di provocare danni al robot 74 3 CONTROLLO DEL ROBOT Per entrambi i link viene fatta la seguente operazione si sottrae la posizione desiderata a quella letta dagli encoder e ne viene calcola to il modulo se questo superiore ad un certo valore scelto il sistema entra in emergenza Il valore scelto oltre il quale l errore di posizione non pu andare di 10 gradi Finch errore inferiore a tale valore grazie ad un blocchetto if si entra nel subsystem no emergenza in cui viene assegnato il valore 0 alla variabile emergenza posizione Se l errore invece supera tale soglia si entra nel subsystem emergenza Figura 3 42 in cui vengono effettuate le seguenti assegnazioni Action Action Port 0 P Stato
17. Digita Output RI ee Output direzione_link2_13 driver_link2_18 Sensoray Sensoray Model 626 auto Model 626 auto Anal Digital tensione linkt p nalog ldriver_tensione_correntet p Pota Output I Output tensione_link1_2 driver_link1_10 Sensoray Sensoray Model 626 auto Model 626 auto direzione_link1 p Digital regen poo pa Output Output direzione_link1_9 emergenza_23 Sensoray Sensoray Model 626 auto Model 626 auto i M tensione _link0 Analog Output tensione_link0_1 Sensoray Model 626 auto direzione_linko Digital Output direzione_link0_16 Sensoray Model 626 auto Figura 3 63 Subsystem generatore uscite per inviarle sono rispettivamente Analog output e Digital output I parametri da impostare in questi blocchetti sono e selezionare la scheda di acquisizione dati desiderata Sensoray626 e scegliere il Sample time 1 e scegliere l Output channel che varia da uscita ad uscita e per le uscite analogiche scegliere su block input signal Volts su quelle digitali channel mode bit 100 3 CONTROLLO DEL ROBOT e impostare a zero il valore iniziale e finale onde evitare di dare segnali danneggianti in ingresso o in uscita Le uscite che vengono inviate sono e tensione link che il modulo della tensione per la movimentazione del motore 2 Output channel 3 e direzione_link2 fornisce il segno della tensione per la movimenta
18. calibrazione per poter passare dallo stato 2 allo stato 3 A degli istanti temporali precisi l attivazione fittizia di questi finecorsa permette tutti i passaggi della calibrazione Ecco il grafico dei passaggi della variabile calibrazione Figura 4 3 Per poter simulare il controllo sono stati modellizzati i motori e ne sono state Figura 4 3 Grafico della variabile calibrazione 108 4 TEST calcolate le funzioni di trasferimento La funzione di trasferimento utilizzata la seguente Km 1 M s 4 1 s 1 sTm s eo con Km e kv back EMF constant di dimensioni volt s Tm Ralm costante di tempo meccanica La funzione di trasferimento del motore 1 diventa 16 55 1 M s 4 2 8 09043025 ti 2 La funzione di trasferimento del motore 1 diventa 0 5883 1 M s 4 3 9 507e 005s 1 s Viene sottolineato che per poter utilizzare queste funzioni di trasferimento stato necessario discretizzarle infatti il programma Simulink avendo un clock di esecu zione diventa un sistema discreto Sono anche stati progettati dei controllori PD secondo il metodo dell allocazione dei poli appositi per le funzioni di trasferimento dei motori e poi discretizzata con il metodo della discretizzazione esatta Gli schemi di retroazione del controllo decentralizzato in questa fase di simula zione sono quindi diventati Figura 4 4 4 1 SIMULAZIONI 109 CONTROLLO DECENT
19. costanti con delle traiettorie pianificate con profilo trapezoidale di velocit Si arrivati a far rispondere i motori correttamente a fargli inseguire quindi i riferimenti Non stato per possibile testare il programma perch si dovuto aspettare che venissero montate anche tutte le sicurezze come i finecorsa e inoltre stato ag giunto all ultimo momento il quadro e quindi stato prima necessario testare il funzionamento di quello Conclusioni In questo progetto di tesi stato realizzato un programma per il controllo di manipolatore planare a tre gradi di libert utilizzando il software Simulink Il lavoro svolto comprendeva questi principali punti e progettazione degli schemi di controllo e gestione degli Input e Output col sistema reale e comunicazione dati con un altro calcolatore tramite protocollo UDP per la ricezione delle traiettorie pianificate I risultati ottenuti sono stati positivi La comunicazione stata realizzata correttamente riuscendo a gestire i problemi causati dall inaffidabilit del protocollo Le traiettorie ricevute vengono interpolate correttamente dal programma la mac china a stati finiti cambia il suo stato correttamente sia per i passaggi interni al programma sia per quelli imposti via comunicazione L invio delle posizioni controllate rappresentative quindi di quelle reali all in terfaccia Matlab vengono visualizzate correttamente e corrispondono al corrett
20. in un primo calcolatore Il controllo serve a far inseguire determinate traiettorie ai motori in modo da farli muovere il pi simile possibile al movimento desiderato La pianificazione di queste traiettorie avviene in un programma M atlab che gira in un secondo calcolatore dove inoltre stata programmata una GUI che rappresenta inter faccia per utente Da ora chiameremo il calcolatore in cui gira il programma M atlab calcolatore A e quello in cui gira il programma Simulink calcolatore B Nel B stata installata la scheda di acquisizione dati Sensoray 626 che permette l invio dei dati dal calcolatore alla parte fisica e viceversa La Sensoray 626 collegata tramite appositi connettori ad una scheda elettronica denominata interfaccia che funge da collegamento con le schede elettroniche dei driver e i motori Lo schema di quanto descritto il seguente Figura 2 1 10 2 ARCHITETTURA PER IL CONTROLLO Figura 2 1 Schema dell architettura del controllo Nel calcolatore A gira il pro gramma Matlab dove vengono pianificate le traiettorie Nel calcolatore B gira il programma Simulink dove avvengono i calcoli del controllo 2 1 Comunicazione Matlab Simulink I due calcolatori collegati in rete comunicano tra di loro per lo scambio dei dati utilizzando il protocollo UDP User Datagram Protocol stato preferito il protocollo UDP al TCP in quanto i dati trasmessi contempo raneamente sono di quantit r
21. posizione del link1 1 2 Descrizione dei cinque progetti Per dare al lettore un idea del lavoro generale necessario al completo funziona mento del manipolatore vengono elencate e descritte brevemente qui di seguito tutte e cinque le parti in cui il progetto stato diviso che corrispondono a cinque progetti di tesi Progettazione e costruzione del parte meccanica del manipolatore Questo progetto di tesi consisteva nella scelta della componentistica di tutta la parte meccanica dai motori al materiale da utilizzare per i link nel disegno in tre dimensioni del robot utilizzando SolidWorks e nella costruzione della parte fisica Sono stati scelti due motori in continua per la movimentazione dei link 1 e 2 ed un motore passo per la movimentazione della base lineare La scelta si effettuata principalmente considerando peso dimensioni e la coppia erogabile dal motore Implementando un programma in grado di mostrare la coppia necessaria per ef fettuare i vari movimenti e considerando i worst case stato possibile decidere quanta coppia devono poter erogare i motori Osservando cos la coppia massima di picco la coppia massima continua e la cop pia di stallo stato possibile scegliere i motori corretti per il progetto tra tutti quelli consultati Considerando sempre anche il prezzo dei motori per la scelta I motori sono stati posizionati all inizio di ogni rispettivo link si capisce quindi come la scelta del
22. realizzazione di una macchina a stati finiti che permette elaborazione dei riferimenti di posizione e la gestione delle emergenze Lo scambio degli Input e Output con il sistema reale avviene tramite la scheda di acquisizione dati Sensoray 626 Le traiettorie desiderate per movimentare il robot sono pianificate da un altro calcolatore e comunicate al controllo tramite l apertura di un socket che utilizza il protocollo di trasmissione UDP La frequenza di invio 100 Hz Il programma si occupa quindi di interpolare le posizioni ricevute e di mandarle ai motori Viene implementato un controllo sia di tipo decentralizzato sia centralizzato Figura 1 Immagine del manipolatore realizzato Introduzione Il progetto che verr spiegato nella seguente tesi tratta della progettazione in Simulink del controllo di un robot lavoro che unito ad altri quattro progetti ne ha permesso la progettazione e la realizzazione Il lavoro stato realizzato da cinque studenti del corso di ingegneria dell Auto mazione dell Universit di Paodva Il robot scelto un manipolatore scara Selective Compliant Assembly Robot Arm a due bracci dotato in aggiunta di una base mobile che si muove lungo una guida lineare L utilizzo di questa tipologia di manipolatore molto sviluppato in ambito in dustriale permette infatti qualsiasi movimento sul piano orizzontale La presenza della base mobile rende il robot a tre gradi di libert port
23. ricevuti ma alcuni vengono persi e quindi bisogna considerare anche questa evenienza nell algoritmo per interpolazione La comunicazione tra i due calcolatori c da sempre ma i riferimenti di posizione 54 3 CONTROLLO DEL ROBOT che vengono inviati dal programma Matlab non vengono mai letti fino a quando Stato non diventa 4 Quando questo avviene vengono rinviate inizialmente a Simulink le posizione di home dei tre link cos il robot rimane nella posizione di calibrazione Questo continua ad avvenire fino a quando utente non dar i comandi di movimentare il manipolatore allora le posizione ricevute da Simulink cambieranno a seconda della traiettoria pianificata da Matlab ci sar l interpo lazione di posizione e verranno inviati i nuovi riferimenti ai motori che faranno muovere il robot Questo subsystem strutturato con un function call generator Figura 3 25 case Action Port INTERPOLAZIONE PER IL LINK 1 E 2 CALCOLO PASSI LINK 0 iteratori y function 0 Function Call theta di riferimento del link2 Generator function theta di riferimento del link1 y function posizione link0 Figura 3 25 Subsystem Calcolo riferimento calibrazione funzionamento normale Nel primo function call subystem iteratori Figura 3 26 vengono calcolati due iteratori che non sono altro che due contatori u
24. stata creata un immagine del robot stesso e l utente potr vederne i mo vimenti in tempo reale Infatti stato previsto che la lettura degli encoder venga comunicata al program ma e con questi dati viene rappresentata l immagine del robot che si muover quindi nello spazio operativo rappresentato A sinistra sono presenti i men dell interfaccia dove possibile selezionare quali operazioni far fare al robot Le possibilit consentite sono far fare all end effector un moto punto punto o un percorso ad n punti scegliere la configurazione con cui deve muoversi il manipo latore se con gomito sinistro o gomito destro scegliere la modalit di controllo se centralizzato o decentralizzato e in questo caso anche se utilizzare o no la cor rezione dell azione in avanti decidere la modalit di funzionamento dei driver dei motori in continua se in tensione o in corrente inoltre sono previste delle possibilit condizionate di mandare il robot in calibrazione e in emergenza Per effettuare i movimenti sono state pianificate delle traiettorie ed il movimento pianificato viene temporizzato ed inviato al controllo attraverso utilizzo di un oggetto timer La pianificazione effettuata segue una legge con profilo trapezoidale di velocit o a polinomio cubico o a polinomio di quinto grado Importante stata anche la scelta di come far muovere il robot infatti l intro duzione della base con movimento lineare porta ad
25. tensione_linkO 0 P passi linko Figura 3 58 Subsystem Calcolo controllo controllo carrello invio al motore dei passi rimanenti L aggiornamento di posizione permette di conoscere qual la posizione del link ogni millesimo di secondo invece che ogni centesimo con i dati che vengono invia ti da Matlab e si pu gi aggiornare immediatamente senza aspettare di inviare realmente la tensione al motore perch lavorando questo motore in catena aperta si pu solo tenere conto del riferimento di posizione voluto Si conclude cos il controllo per la base Per il controllo per gli altri due link si entra nel secondo function call subsy stem controllo link1 e2 Figura 3 59 al suo interno si possono verificare due casi e dipendono dalla variabile controllo inviata da Matlab I due casi possibili sono il controllo decentralizzato che corrisponde al valore 0 94 3 CONTROLLO DEL ROBOT o function if ut 0 elseif u1 1 elseif if controlo ui controllo centralizzato controllo decentralizzato Figura 3 59 Subsystem Calcolo controllo controllo link1 e2 della variabile e il controllo centralizzato che corrisponde al valore 1 La teoria che sta dietro a questi due tipi di controllo stata spiegata nel capitolo precedente ora viene mostrato come sono stati implementati i
26. vlinko rif vel link 0 conversione mm in passi TR Ci kmm2p gt _a_linko_rif acc link 0 Ra aude conversione mm in passi Ga gt _v_linki_rif vel link 1 13 Pa linkt_rif ace link 1 14 di v_link2 rif vel link 2 15 P a link2_rif acc link 2 MEMORIZZAZIONE DEI DATI DELL INTERFACCIA MATLAB Figura 3 48 Subsystem ricezione dati matlab memorizzazione dati matlab e i riferimenti di posizione per i tre link ma questi entrano in un altro sub system enabled che viene attivato solo quando Stato 4 Infatti negli stati precedenti i riferimenti di posizione vengono calcolati internamente al programma Simulink basta guardare algoritmo di calibrazione se si sovrapponessero anche questi dati che sarebbero tutti zero perch nel pro gramma Matlab non viene pianificata nessuna traiettoria si sbaglierebbero tutti i valori urante il funzionamento normale si entra quindi nel subsystem abilitazio D te il f t l t di nel subsyst abilit ne memorizzazione posizioni Figura 3 49 le posizioni memorizzate sono 82 3 CONTROLLO DEL ROBOT T Enable C41 1 kp2m 1000 gt posizione_linkO ae SE ne posizione linkO SET conversione mm in passi 2 P posizione_link1 posizione link 1 3 gt posizione_link2 posizione link 2 Figura 3 49 Subsystem ricezione dati matlab memorizzazione dati matlab gt abilitazione memorizzazi
27. 7 ta x t ui ta ta vmax_link2_cal gt 5 rad delta_fc_cal_link1 90 kg2r gt P posizione_link1 vmax_link1_cal_ _ amax_link1_cal 0 5 ta x ta Figura 3 17 Subsystem Calcolo riferimento calibrazione posizione di home gt allineamento link2el calcolo posizione link 2 e 1 calcolo posizione fase velocita costante Il riferimento viene calcolato per entrambi i link in questo sottosistema Oin Amax SONO i medesimi dei precedenti e Umax VMax_link1_cal per il link 1 Umax VMax_link2_cal per il link 2 e ta lt t lt T calibrazione td Quando il tempo assume valori maggiori a T_calibrazione td si entra nel sottosistema del calcolo della posizione in fase di decelerazione Figura 3 18 viene dato dall equazione 3 2 DESCRIZIONE DEL PROGRAMMA 47 FASE A VELOCITA COSTANTE Action Port amin_link2_cal e T_calibi gt gt x t T_calibi O t T calib rm pl x x t T_calibi LC __ x t Figura 3 18 Subsystem Calcolo riferimento calibrazione posizione di home allineamento link2el calcol
28. FERIMENTO _ _ _ _____ gt DRIVER 0 gt MOTORED Figura 2 5 Schema di un controllo di tipo decentralizzato Ogni asse ha il suo controllore specifico che agisce solo per quel membro 18 2 ARCHITETTURA PER IL CONTROLLO Il motore passo lavora in catena aperta e come si pu vedere dall immagine non ha un feedback di posizione Entrando nel maggior dettaglio del controllo le due retroazioni che si vedono nell immagine hanno in realt la seguente struttura generale Figura 2 6 Si tratta di uno schema a retroazione negativa il cui ingresso il riferimento di coppia_resistente 7 A riferimento_di_posizione gt e WA Di PA gt Figura 2 6 Schema generale di un controllo in retroazione posizione desiderato C il controllore da progettare e M il processo da control lare che in questo caso sono i due motori D il disturbo che agisce sul sistema L uscita la posizione reale a cui arriva il motore Lo scopo del controllore quello di far corrispondere la posizione reale con quella desiderata rendendo cos l errore di posizione nullo Sono previsti due schemi basati su questo principio nel programma uno per mo tore Vengono quindi progettati due controllori ognuno specifico per un motore che devono essere in grado di far inseguire al motore il riferimento di posizione che riceve in ingresso I co
29. I PRIMO MOTORE mi 0 495 kti 0 0603 kvi 1000 158 60 2 pi 1000 Ral 1 16 Imi 138 10 7 kmi 1 kvi Tmi Rai Imi kt1 kvi Mi t kmi Tml 1 0 Mid c2d M1 T tustin Mipoli pole Mid Mizeri gainMi zero Mid DATI SECONDO MOTORE m2 0 170 kt2 0 0538 kv2 178 60 2 pi 1000 Ra2 2 52 Im2 34 5 10 7 km2 1 kv2 Tm2 Ra2 Im2 kt2 kv2 M2 tf km2 Tm2 1 0 M2d c2d M2 T tustin M2poli pole M2d M2zeri gainM2 zero M2d di di d d d a da di d d d de kg massa del motore i Nm A costante di coppia volt s back EMF constant ohm terminal resistance kom 2 inerzia del motore s costante di tempo funzione di trasferimento del motorel funzione di trasferimento del motorel discreta poli zeri e guadagno kg massa del motore 2 Nm A costante di coppia volt s back EMF constant ohm terminal resistance kom 2 inerzia del motore sj costante di tempo funzione di trasferimento del motore funzione di trasferimento del motore discretizzata poli zeri e guadagno Figura B 3 Immagine file Parametri utilizzato per caricare le costanti utilizzate nel programma Parte C Bibliografia 1 S Co Instruction Manual Sensoray Model 626 January 2004 2 B Siciliano L Sciavicco L Villani and G Oriolo Robotica Milano McGraw Hill Terza Edizione
30. ITMO D INTERPOLAZIONE delta_theta iteratore_link2 Figura 3 30 Subsystem Calcolo riferimento calibrazione funzionamento normale gt theta di riferimento del link2 calcolo riferimento posizione_link2 theta_in delta_theta iteratore_link2 Diventa ora chiara l utilit di aver creato gli iteratori infatti andando a moltiplicare il valore del delta creano sempre il riferimento successivo e quan do iteratore_link2 10 theta_in delta iteratore_link2 proprio uguale a theta fin Si detto per che si deve considerare anche il fatto che non sempre il pacchetto dati arriva a Simulink quindi la posizione verrebbe aggiornata ad esempio dopo 0 02 sec o 0 03 secondi o comunque dopo un tempo non conosciuto ma superiore al centesimo di secondo Ma algoritmo funziona lo stesso infatti vero che il valore degli iteratori supe 60 3 CONTROLLO DEL ROBOT rerebbe il valore 10 se non si riceve un pacchetto dati e quindi si potrebbe pensare che theta_in assumerebbe valori pi grandi di theta_fin infatti inizialmente era stato pensato che il valore dell iteratore tornasse ad 1 non solo se si ricevevano dati ma anche se l iteratore assumeva il valore 11 Ma questa condizione non serve perch dopo 10 cicli se non si riceve nessun dato theta_fin
31. Questo switch attua quindi l effetto dello stato di emergenza se siamo nello stato 92 3 CONTROLLO DEL ROBOT if Action Port passi_linkO P passi linko max_passi_clock max passi clock pa Mm tensione linko Figura 3 57 Subsystem Calcolo controllo controllo carrello invio al motore del max n di passi possibili di emergenza la tensione che deve erogare il motore passo nulla La tensione rimane nulla anche durante lo stato messa in marcia Se invece la differenza tra passi_link0 e max_passi_clock risulta essere negativa vuol dire che i passi richiesti sono fattibili in un solo clock di simulazione e allora si entra nel subsystem invio al motore dei passi rimanenti Figura 3 58 vengono memorizzati nella variabile che invier la tensione al motore tutti i passi previsti convertiti in tensione dal guadagno kpassi2v Anche qui previsto lo switch che nello stato di emergenza impone la tensione da inviare al motore pari a zero Per quanto riguarda aggiornamento di posizione viene sommato o sottratto a posizione_link0_attuale la tensione che deve essere inviata al motore riconvertita in numero di passi e considerando anche il segno 3 2 DESCRIZIONE DEL PROGRAMMA 93 else Action Port T__ passi_linkO ar su M Stato gt 0
32. RALIZZATO LINK 2 gt M M Stato gt H gt lu gt P tensione link Vret_a_link2 o gt gt Pidirezione_link2 7 trae posizione_link2 gt p aes gt x Zerosta 2 C2poli on ev gt pda i _ 2 M2poli posizione link2 M2 PID z encoder _link2 rad encoder_link2 CONTROLLO DECENTRALIZZATO LINK 1 Stato _ gt an gt tensione inki Vret_a_linki si Pidirezione_linkt v rad posizione_link1 A zeros 2 v gt gt gt y y E Ctpoli Las 28 VY 2 Mipoli posizione link Cpdt L Mi rad encoder linkt PIDIA encoder_link1 Figura 4 4 Schema del controllo decentralizzato in fase di simulazione Durante la fase di calibrazione i risultati ottenuti dal controllo sono stato mol to soddisfacenti si tratta pur sempre di una simulazione ma inseguimento di posizione risultato pienamente soddisfacente Vengono ora mostrati i grafici delle posizione dei due link durante la fase di cali brazione mettendo a confronto la traiettoria pianificata e quella rea
33. Subsystem emergenze controllo velocita stimata a 4 rad s Finch la velocit non supera quella massima I uscita del blocchetto relational operator zero e grazie al blocchetto if si entra nel subsystem no emergenza in cui viene memorizzato emergenza_velocita 0 che se appena finita l emer genza permette il passaggio della macchina a stati dallo stato di emergenza al succesivo durante il funzionamento normale invece conferma solo che va tutto bene Quando la velocit supera il limite massimo si entra nel subsystem emergenza Il subsystem ha struttura analoga a quelli precedenti Questa emergenza non altera la lettura encoder e quindi non si deve ricalibrare il robot Allora avvengono le seguenti assegnazioni nel sottosistema e Stato 0 per entrare nello stato di emergenza e emergenza svelocita 1 per far attivare l emergenza 3 2 DESCRIZIONE DEL PROGRAMMA 73 e calibrazione 2 che il valore iniziale della variabile e verranno quindi calibrati tutti e tre i link e iteratore_link2 0 per poter reinterpolare correttamente la posizione del link 2 e iteratore_link1 0 per poter reinterpolare correttamente la posizione del link 1 La quarta categoria di emergenze considerata in controllo posizione Figura 3 41 Questa emergenza controlla che la posizione reale dei motori non sia troppo lon LO function
34. UNIVERSIT DEGLI STUDI DI PADOVA FACOLT DI INGEGNERIA Finito di scrivere il giorno 22 luglio 2010 utilizzando PTRX 2 UNIVERSIT DEGLI STUDI DI PADOVA FACOLT DI INGEGNERIA DIPARTIMENTO DI INNOVAZIONE MECCANICA E GESTIONALE TESI DI LAUREA TRIENNALE IN INGEGNERIA DELL AUTOMAZIONE CONTROLLO DI MANIPOLATORE A TRE GRADI DI LIBERT RELATORE CH Mo PROF ING GIULIO ROSATI LAUREANDO ROMERES DIEGO ANNO ACCADEMICO 2009 2010 ai miei genitori e ai miei fratelli che hanno saputo sopportarmi in questi anni e ad Alessia Indice Sommario Introduzione 1 1 Descrizione del robot 1 2 Descrizione dei cinque progetti a ore i as Dee 3_ Controllo del robot 3 1 Introduzione Simulink 3 2 Descrizione del programma 3 2 1 Calcolo riferimento 3 2 2 Ingressi Sensoray626 3 2 4 Ricezione dati Matlab 3 2 5 Calcolo azione in avanti 3 2 6 _ Calcolo controllo 3 2 3 __ Emergenze i Li IX XI 10 12 12 14 16 17 21 3 2 7 Generatore Uscite 2 00 98 3 2 8 Comunicazione invio dati matlab 4 Test 105 inerti 105 113 17 121 125 VIII Sommario stato progettato il controllo di un manipolatore planare a tre gradi di libert tramite il software Simulink Il controllo avviene in tempo reale utilizzando Real Time Windows Target per migliorare le prestazioni la simulazione gira ad 1kHz Il programma incentrato sulla
35. _dx_link2_ Encoder Input Sensoray Model 626 auto Encoder_link2_5 Encoder Input Sensoray Model 626 auto Encoder_link1_4 Digital Input finecorsa_sx_link2_6 Sensoray Model 626 auto Digital Input Sensoray Model 626 auto Digital Input finecorsa_dx_link1 fi finecorsa_sx_link0_ Sensoray Model 626 auto 4 Digital Input inecorsa_sx_link1 Sensoray Model 626 auto Digital Input Sensoray Digital Input Model 626 auto ofiset_link2 tacche encoder A rad gt ke2r gt offset linkt tacche encoder Y rad ke2r A gt gt 5 gt 3 gt 2 gt finecorsa_dx_linkO_1 Figura 3 32 Subsystem in cui vengono memorizzati tutti gli ingressi della Sensoray Sensoray Model 626 auto encoder_link2 encoder_link1 finecorsa_sx_link2 finecorsa_dx_link2 finecorsa_dx_link1 finecorsa_sx_link1 finecorsa_sx_linkO finecorsa_dx_link0 Digital Input emergenza_meccanica_ Sensoray Model 626 auto Digital 24 emergenza_meccanica Input temp0a_15 Sensoray del 626 auto Digital temp0a Input tempob_11 Sensoray Model 626 auto _ Digital temp0b Input tempi_12 Sensoray del 626 auto Di
36. a accelera zione desiderata del link Calcolando in modo dinamico grazie alla dinamica inversa le coppie da erogare con queste accelerazioni e le posizioni e le velocit reali si ottiene un controllo che tiene conto dell effetto di tutti gli accoppiamenti dei link Il problema di questo controllo per che bisogna conoscere con grande accura tezza i parametri del modello dinamico perch sia un controllo efficiente Inoltre potrebbero sorgere problemi perch richiede molti calcoli eseguiti in modo dinamico e quindi il software potrebbe non riuscire ad eseguirli alla frequenza richiesta Anche per questo motivo il programma realizzato viene eseguito in tem po reale utilizzando Real Time Windows Target Capitolo 3 Controllo del robot In questo capitolo verr spiegato come stato programmato il controllo cosa implica fare il controllo di un manipolatore qual la logica utilizzata e il significato dei blocchi Simulink 3 1 Introduzione Simulink Il software Simulink si basa sull utilizzo di funzioni rappresentate da dei blocchi gi esistenti nelle librerie del software La scrittura del codice per via grafica lo rende un programma particolarmente adatto alla rappresentazione degli schemi a blocchi Il programma realizzato suddiviso in diversi livelli infatti tramite i blocchetti subsystem viene creato un nuovo livello in cui possono essere eseguite delle fun zioni a parte Il livello pi alto
37. a encoder encoder_link2 e la diffe renza viene memorizzata nella variabile offset_link2 Questo valore indica quanto la lettura encoder sfasata dalla posizione reale per il sistema di riferimento con siderato Il riferimento di posizione ha cos subito un gradino di ampiezza delta_fc_cal_link2 che non sarebbe accettabile in termini di controllo Allora offset_link2 viene sommato a encoder link2 cos anche il valore della posizione reale ha subito nu mericamente questo incremento e l errore di posizione che quello che viene controllato rimane invariato La posizione del link torna quindi a coincidere con quella del sistema di riferi mento considerato Terminata quindi questa prima fase si passa alla chiusura del link 1 Figura 3 8 36 3 CONTROLLO DEL ROBOT Il procedimento analogo al precedente viene dato lo stesso incremento di elseif Action Port ALGORITMO INIZIALIZZAZIONE LINK 1 rad posizione_link1 gt gt posizione_link1 R e incremento_calibrazione finecorsa_sx_link1 le attivazione finecorsa Figura 3 8 Subsystem Calcolo riferimento gt calibrazione chiusura spalla posizione al link1 ma questa volta viene sommato perch viene chiuso verso il finecorsa induttivo sinistro viene assegnata la nuova posizione a posizione link1 calcolato l offset e sommato alla lettura e
38. a variabile v_link1_rif 84 3 CONTROLLO DEL ROBOT e l accelerazione pianificata del link 1 memorizzata nella variabile a_link1_rif e la velocit pianificata del link 2 memorizzata nella variabile v_link2 rif e accelerazione pianificata del link 2 memorizzata nella variabile a_link2_rif 3 2 5 Calcolo azione in avanti La teoria di questa tecnica stata spiegata nel capitolo precedente in questa sezione vediamo come stata implementata con Simulink Ricordiamo solamente che una tecnica che va a compensare le componenti che causeranno errore di inseguimento che esiste sia di tipo centralizzato che decen tralizzato e che il suo valore cambia se si utilizza un driver controllato in tensione o in corrente Figura 3 50 Il subsystem diviso da un function call generator in due sottosistem uno per il t0 function ABILITAZIONE AZIONE IN AVANTI Stato gt gt 2 aje azione in avanti Figura 3 50 Subsystem Calcolo azione in avanti 3 2 DESCRIZIONE DEL PROGRAMMA 85 calcolo della coppia resistente uno per lo specifico calcolo dell azione in avanti negli omonimi sottosistemi Il calcolo della coppia resistente viene posto in questo subsystem perch come noto serve questo valore per il calcolo dell azione in avanti centralizzata Poi i risultati ottenuti verrano utilizzati anche in altre parti del program
39. aggiungere le posizioni desiderate utilizzando tutti e tre link Inoltre viene superato il problema di alcune posizioni singolari che verrebbero a crearsi L end effector una matita che pu essere alzata o abbassata e permette di scrivere nello spazio operativo Viene ora mostrata un immagine che schematizza il manipolatore reale Figura 1 1 I giunti del manipolatore quindi sono 1 PROGETTO Figura 1 1 Schematizzazione del robot il linkO che verr anche chiamato carrello o base mobile a cui verr as sociata una posizione lineare x che indicher la posizione assoluta lungo la vite a ricircolo di sfere espressa in numero di passi o mm Le dimensioni del link sono 100 mm 100 mm La lunghezza della vite verr espressa o col numero di passi necessari alla base mobile per percorrere tutta la guida considerando il finecorsa indutti vo a sinistra il passo zero e il fine corsa destro l ultimo passo per un totale di 3000 passi o in mm per un totale di 300 mm da induttivo a induttivo il linkl a cui associata la posizione angolare 01 vedere figura positiva per una rotazione in senso antiorario e il cui zero corrisponde al link posto 1 2 DESCRIZIONE DEI CINQUE PROGETTI 3 lungo asse delle ascisse il link2 a cui associata la posizione angolare 02 vedere figura positiva per una rotazione in senso antiorario ma una posizione relativa perch dipende anche dalla
40. ando cos una rindondanza nei movimenti che ne ha permesso lo studio per arrivare ad un ottimazzazione L obbiettivo del progetto di realizzare il robot in modo che sia in grado di tracciare delle lettere o delle traiettorie attraverso dei punti predeterminati nello spazio operativo costituito da un foglio A4 A tale scopo stato previsto anche il movimento verticale rigido di una matita che potr essere alzata o abbassata La matita l end effector del robot Ecco una foto rappresentativa del tipo di manipolatore sz lt T gt fe Figura 2 Foto di un manipolatore scara Obiettivi All interno della realizzazione del robot questo progetto di tesi ha come obbiet tivo il controllo del movimento del robot e la gestione delle emergenze Si vuole progettare una macchina a stati finiti che permetta di calcolare i riferi menti di posizione corretti durante il funzionamento normale durante la calibra zione e durante le emergenze Per mantenere i motori controllati si vuole implementare un controllo di posizio ne sia di tipo centralizzato sia di tipo decentralizzato Per ottenere questi obbiettivi importante la gestione degli input e output del sistema reale e la comunicazione con un altro calcolatore da cui vengono ricevute le traiettorie pianificate Organizzazione della tesi La tesi suddivisa in quattro capitoli Nel primo capitolo viene illustrato il robot e come stato organizzato e suddivis
41. ase mobile di coppia per i due motori C20 mglasen 0 O2 A 8 Cio m2 lgsen 0 00 a1sin 01 mylisin 01 Sono state determinate tutte le componenti per il calcolo delle coppie quindi le coppie che il motore 2 e il motore 1 devono erogare per inseguire le traiettorie sono Cina Ina L 4 63 La m90113c08 03 6 mya1l sen 0 O Malysen O1 02 Cm1 Imi Tho ma 2m90 12c08 99 I 4 1 DA m30 13 cos O 2myaylysen O2 O O92 mailsen 02 97 m lasen O1 O2 asin O1 m h sin 01 A 9 Appendice B File Parametri In questa appendice viene solo riportato il file Matlab in cui vengono definite delle varibili utilizzate nel programma 122 B FILE PARAMETRI SETESTEEESETESETETESTSESETESESELESESLESTESLESTSTSTESTETELESESESESESETETETELESETESESEESELESESTELELE LESS 43434 PARAMETRI iii A A A A A LA A A A A A A A A A A A A AAA s 88t tttt ttttttttttttttttstttstttssttstttttititttitttttttttttttttttttttsstsstttsttsttttttt clc clear all close all T 0 001 s periodo della simulazione utilizzato anche per il compionamento della discretizzazione CONVERSIONI A kr2g 180 pi conversione da radianti a gradi kg2r pi 180 conversione da gradi a radianti DATI PER LA PIANIFICAZIONE DELLA TRAIETTORIA IN CALIBRAZIONE T_calibrazione 2 s tempo del movimento ta 0 6 s tempo di accelerazione td 0 6 s tempo di deceler
42. azione CALIBRAZIONE incremento calibrazione 0 02 kg2r rad incremento angolare dato ai link2 e 1 nell algoritmo di calibrazione passi calibrazione 1 passi ms incremento di passi al millisecondo per il motore passo in fase di calibrazione EMERGENZE max no lettura encoder 50 n massimo di volte accettabile per cui la lettura encoder non cambia valore ma i riferimenti di p v_max link2 4 rad sec velocit massima consentita al link2 v_ max linki 4 rad sec velocit massima consentita al linki DATI ROBOT mtot 2 360 kg massa totale del robot GUIDA LINEARE fine guida 3000 numero di passi da fine corsa sx a fine corsa dx della guida meta _guida fine_guida 2 numero di passi tra un fine corsa della guida lineare e il centro della guida Figura B 1 Immagine file Parametri utilizzato per caricare le costanti utilizzate nel programma Parte A 123 LINK 2 a2 9 1 Lu lunyhessa del Liuk2 12 0 078 m distanza dal baricentro del link2 al giunto di colegamento con il linki delta rc cal linkz 155 kg2r rad delta di posizione che c tra um fine corsa induttivo del link2 e la posizio LINK 1 ai 0 12 m lunghezza del linki 11 0 0003 m distanza dal baricentro del link1 al giunto di colegamento con il link0 delta fc cal linki 125 kg2r rad delta di posizione che c tra un fine corsa induttivo del linki e la posizio DRIVER Gv 8 guada
43. brazione dei due link 1 e 2 Si vede un primo subsystem if che dipende dalla varibile j Serve per far eseguire il subsystem calcolo vmax amax amin solo una volta j inizializzata a 0 viene eseguito quindi il subsystem citato alla fine del quale viene assegnato il valore 1 a j cos sempre durante lo stesso ciclo viene eseguito anche il subsystem calcolo posizione link 2 e 1 e il primo non verr pi eseguito serve infatti solo a calcolare dei valori costanti che rimarranno invariati Figura 3 12 Per il calcolo della velocit massima dell accelerazione massima e dell accele Action CALCOLO VELOCITA MASSIME E ACCELERAZIONI MASSIME E MINIME Action Port delta_fc_cal_link2 T_calibrazione ta td 2 P vmax_link2_cal vmax_link2_cal Px I amax_link2_cal ta P vmax_link2_cal P x H amin_link2_cal td P delta_fc_cal_link2 T_calibrazione ta td 2 P vmax_link2_cal vmax_link2_cal x __ amax_link2_cal ta ala vmax_link2_cal Px __ amin _link2_cal td P 1 gt j Figura 3 12 Subsystem Calcolo riferimento calibrazione posizione di home calcolo vmax amax amin razione minima sono state utilizzate la seguenti equaz
44. che mandavano alle uscite analogiche e digitali dei segnali inizialmente molto semplici come delle co 4 1 SIMULAZIONI 111 stanti fino ad emulare delle traiettorie pianificate simili a quelle che il programma dovr inviare I programmi di prova utilizzati sono come il seguente Figura 4 8 Nell esempio riportato entrano delle costanti nelle uscite analogiche che pote 0 6 gt i 1 gt o 2 gt i 1 gt o Analog Output 3 1 gt Analog Output Sensoray 1 gt o Model 626 auto 4 gt i Digital 1 ai P Output Digital Output Sensoray i ro Model 626 auto gt Scope 1 gt o rae nalog Input Analog Input Scopet 1 gt o jensoray Model 626 auto gt Scope Digital Input Digital Input Scopes Sensora Model 626 auto Figura 4 8 Programma utilizzato per testare le schede elettroniche e i motori vano essere variate durante l esecuzione della simulazione grazie all inserimento del blocchetto slider gain Si potuto cos osservare grazie all utilizzo del tester se le schede elettroniche rispondevano correttamente Poi sono stati connessi anche i motori e si arrivati a sostituire le
45. condo Per risolvere questi problemi il blocchetto Packet Input ci permette di conoscere gli istanti esatti di ricezione dati sufficiente mettere la spunta sull opzione data ready del blocchetto che gli aggiunge un ulteriore uscita Questa uscita diventa 1 ogni volta che effettivamente viene ricevuto un pacchetto di dati nuovo e se no rimane a zero Questo dato memorizzato nella variabile ricezione_dati viene utilizzato per abi litare o no la lettura dei dati infatti tutte le uscite del blocchetto entrano nel 3 2 DESCRIZIONE DEL PROGRAMMA 81 subsystem enabled memorizzazione dati matlab che viene abilitato da questa variabile Figura 3 48 I dati che vengono ricevuti elencati in ordine dall alto verso il basso sono N Enable Stato P 4 y 1 P gt posizione linko posizione link0 P posizione link 1 posizione link 1 G P posizione link 2 posizione link 2 abilitazione memorizzazione matlab G gt Pdriver_tensione_corrente2 driver_link2 5 Pdriver_tensione_corrente2 driver_link1 Ce gt azione_in_avanti azione in avanti CE controllo controllo 8 P stato matlab stato desiderato T gt 0 ER SJ A 9 gt end effector stato end effector T a n nE Cio kmmop_
46. danneggiato senza accorgersene Il procedimento come si pu vedere dalla figura esattamente analogo per en trambi i link Per verificare questo si sottrae alla lettura encoder la lettura precedente che si tiene in memoria con il blocchetto memory Se la differenza nulla vuol dire che la il dato encoder non cambiato La differenza viene messa in ingresso ad un blocchetto comparatore che la uguaglia a 0 se la condizione verificata esce un 70 3 CONTROLLO DEL ROBOT 1 altrimenti uno 0 e questa uscita l ingresso di una porta and L altro ingresso della porta and dato dalla differenza tra la posizione di rife rimento del link comparata con un blocchetto comparatore a se diversa da 0 Quindi questo ingresso sar 1 tutte le volte che il riferimento di posizione cambia valore Per tanto se il riferimento di posizione cambia valore e quello dell encoder invece no l uscita della porta and diventa 1 e va ad incrementare un contatore Il contatore i per il link 2 e j per il link 1 Se l uscita della porta AND zero grazie ad un blocchetto if si entra nel subsy stem reset che azzera il contatore Il valore del contatore viene confrontato ogni volta con il valore massimo per cui questa situazione sia accettabile che viene caricato dal file Matlab con il nome di max_no _lettura_encoder Il valore massimo stato scelto essere 50 se il valore del contatore mino
47. e la possibilita di interruzione e 7 delle uscite digitali possono essere usate come contatori di overflow e gli I O digitali hanno connettori di tipo standard per industria e dotata di un watchdog temporizzato con la possibilit di scelta di numerosi periodi di reset del PCI bus e 6 contatori up down da 24 bit uniti in 3 coppie con ingressi che possono essere utilizzati in diversi modi 1x 2x 4x dagli ingres 14 2 ARCHITETTURA PER IL CONTROLLO si degli encoder incrementali dagli ingressi digitali dalle coppie di contatori di overflow dal clock del sistema o dal programma e controllo della carica della batteria di riserva Ni Cad e 16 ingressi analogici differenziali con 14 bit di risoluzione rate 15kHz e ogni canale pu essere programmato per 5V o 10V e 4 uscite analogiche con 13 bit di risoluazione rate 20 kHz programmati a 10V 2 2 2 Scheda elettronica d interfaccia Questa scheda elettronica raccoglie tutti i connettori collegati alla Sensoray 626 quelli che portano il segnale ai driver e quelli che arrivano dagli encoder quindi la scheda che permette il passaggio dei dati a tutte le corrette strutture Analizziamola osservando lo schema logico Figura 2 3
48. e memorizzato nella variabile controllo e il valore dello stato che il programma Matlab vorrebbe dare alla macchina a stati viene memorizzato nella variabile stato_ matlab Questo dato stato pensato non per seguire tutti i cambiamenti di Stato ma solo per cambiarne il valore per entrare nello stato di calibrazione e di funzionamento normale L algoritmo pensato stato che Matlab invii sempre lo stesso valore su questa variabile quando il cambiamento di stato nella macchina dipende dalla GUI invia il valore corretto quindi facendo la sottrazione tra il valore nuovo che arriva e il valore precedente ottenuto grazie al blocchetto memory se la differenza diversa da zero vuol dire che l ultimo dato inviato quello che deve assumere la variabile Stato e si entra in un subsystem in cui avviene assegnazione Stato stato_desiderato Questo semplice algoritmo permette il passaggio dallo stato 1 allo stato 2 e dallo stato 3 allo stato 4 e il passaggio allo stato 0 da qualsiasi stato per generare emergenza e il valore che va a decidere se end effector nel nostro caso la matita deve appoggiarsi al foglio e quindi scrivere oppure no rispettivamente 1 e 0 e la velocit pianificata del link 0 memorizzata nella variabile v_link0_rif e l accelerazione pianificata del link 0 memorizzata nella variabile a _link0 rif e la velocit pianificata del link 1 memorizzata nell
49. e sono il numero di passi che si vorrebbero 3 2 DESCRIZIONE DEL PROGRAMMA 91 LO function if u1 gt 0 passi_linkO gt Pui i alain else 4 else if max_passi_clock invioalmotoredeipassirimanenti invio al motore il max n di passi possibili pos_linkO_attuale P pos linkO attuale tensione_linkO direzione_linkO tensione_linkO Figura 3 56 Subsystem Calcolo controllo controllo carrello far fare e la costante max_passi_clock caricata dal file Matlab se il risultato maggiore di zero vuol dire che i passi che si vorrebbero far fare sono eccessivi Allora grazie all uso di un blocchetto if quando questo succede si entra nel subsystem invio al motore del max n di passi possibili Figura 3 57 al suo interno viene aggiornato il valore dei passi che diventa passi_link0 passi_linkO max_passi_clock che rimangono da fare e viene memorizzato nella variabile che invier i dati al motore il numero massimo di passi che pu essere fatto tensione link0 max passi clock convertito dal guadagno kpassi2v in volt Inoltre presente uno switch che legge la variabile Stato e in cui impostato che se la variabile maggiore di 1 allora viene inviato la tensione del numero di passi altrimenti la tensione a zero
50. e una porta nor con in ingresso tutte le possibili emergenze che va ad abilitare o no un altro subsystem la porta garantisce che fino a quando una delle emergenze attiva l uscita a zero quindi il subsystem fine stato di emergenza non viene attivato e si rimane in stato di emergenza Durante questo stato che viene comunicato sia all elettronica e sia al programma Matlab il manipolatore viene frenato dall elettronica fino al bloccaggio e la GUI disabilitata Quando invece non presente nessuna emergenza ma siamo comunque in questo stato significa che la fine dell emergenza stata letta nel cin questo ciclo di clock 3 2 DESCRIZIONE DEL PROGRAMMA 31 la porta nor d come uscita true che convertita in double permette di andare ad abilitare il subsystem fine stato emergenza Figura 3 4 Viene assegnato il valore 0 alla variabile emergenza per comunicare all elet a Enable FINE EMERGENZA 0 gt emergenza if u 2 calibrazione Put elseif ui 2 elseif if no ricalibrazione ricalibrazione Figura 3 4 Subsystem Calcolo riferimento emergenza fine stato di emergenza tronica che l emergenza finita e che per il programma possibile riprendere il funzionamento normale Inoltre si presentano due casi che sono i due possibili effetti delle diverse emergen ze infatti alcune portano al
51. e vengono in viati dal calcolatore A Figura 3 47 I dati vengono ricevuti tramite il blocchetto Packet input che appartiene pro prio alla libreria Real Time Windows Target quindi non ci sono problemi di incompatibilit e pu ricevere i dati alla frequenza con cui gira la simulazione I parametri da inserire in questo blocchetto sono e indirizzo IP dell altro computer la Local port e la Remote port si installa una new board con questi parametri che utilizzi il protocollo UDP e si crea cos il Socket tramite cui avviene la comunicazione e si possono ricevere i dati 3 2 DESCRIZIONE DEL PROGRAMMA 79 n Enable 0 P Stato 2 P calibrazione 4 P emergenza_induttivio 0 iteratore_link2 0 iteratore_link1 Figura 3 46 Subsystem emergenze controllo finecorsa induttivi gt finecorsa induttivi gt emergenza e il tempo di campionamento che stao posto anche questo a 1 quindi prova a ricevere i dati alla frequenza della simulazione e la dimensione del pacchetto in ricezione che essendo double si ottiene dal numero di dati ricevuti moltiplicati per 8 numero di bit per double e il numero di dati che vengono ricevuti che sono 15 Fondamentale la sincronizzazione tra la ricezione dei dati e il loro memorizza zione per utilizzo nel programma Come
52. ene sempre inviata e ricevuta da Matlab Quando viene inviato al calcolatore A Stato 1 sull interfaccia che era disa bilitata perch sicuramente lo stato precedente era quello di emergenza viene abilitato un pulsante Calibrazione Quando Il utente decide di premerlo vuol far partire la calibrazione allora Matlab invece di inviare di nuovo Stato 1 o invia Stato 2 Cos la macchina a stati pu entrare nello stato successivo Stato 2 calibrazione In questo stato avviene la calibrazione dell intero robot Il problema principale consiste nel fatto che gli encoder quando il robot viene acceso non possono conoscere la posizione assoluta in cui si trova il motore e segnano sempre 0 come valore iniziale indipendentemente dalla posizione in cui si trova Figura 3 5 La soluzione utilizzata stata quella di dare un piccolo incremento di posizione ai link fino ad arrivare ad un finecorsa induttivo quando questo viene attivato si memorizza la posizione letta dall encoder Conoscendo gi a priori qual la posizione assoluta del finecorsa induttivo si ha cos l offset per riportare la posi 3 2 DESCRIZIONE DEL PROGRAMMA 33 case Action Port CALIBRAZIONE if ul 2 y if chiusura gomito elseif u1 1 4 elseif calibrazione mu chiusura spalla elseif u1 0 elseif chiusura carrello elseif ui
53. er i due motori in continua La progettazione della scheda elettronica inizia con la scelta delle componenti dalla lettura dei data sheet che ha portato alla scelta dei seguenti integrati principali microcontrollore dsPic30F4012 e LMD18200 integrato principale dello stadio in potenza Successivamente viene fatto lo schema logico utilizzando il software Power Logic poi vengono progettate le piste con il software Power Pcb e programmato il mi crocontrollore Infine viene realizzata la scheda fisicamente stata realizzata una sola scheda elettronica per i driver di entrambi i motori e pensata in modo che sia esattamente simmetrica met per un motore met per P altro 1 2 DESCRIZIONE DEI CINQUE PROGETTI 5 I microcontrollori sono stati programmati in Simulink La scheda driver comunica attraverso appositi connettori con la scheda d in terfaccia e quindi con la scheda di acquisizione dati e con i motori e si occupa principalmente della gestione dei riferimenti di tensione per i due motori Progettazione e realizzazione del driver dei motore passo Questo progetto prevede la progettazione e la realizzazione del driver per il mo tore passo della scheda d interfaccia e della realizzazione del quadro elettrico La progettazione delle due schede elettroniche segue il procedimento generale de scritto per i driver dei motori in continua Il driver stato realizzato in una scheda elettronica a struttura speculare I p
54. er la scheda d interfaccia ed ingressi per scheda di acquisizione I transistor vengono alimentati dalla resistenza di pull up della Sensoray 626 Per le emergenze generate dall elettronica il segnale a 15V viene abbassato a 5V da una rete resistiva posizione 3 B e va direttamente a comandare il brake de gli LMD che fanno frenare il robot e va inoltre a pilotare la base di un transistor il cui collettore collegato ad un ingresso digitale della Sensoray 626 Anche in questo caso l alimentazione presa da una resistenza di pull up della Sensoray 626 L uscita digitale per le emergenze generate dal controllo va ad un apposito con nettore in posizione 4 B che va a pilotare lo stesso rel delle emergenze volute dall elettronica danno quindi lo stesso effetto solo che uno comandato dal pro gramma Simulink e uno dall elettronica Si pu quindi vedere la scheda con questo semplice schema che d una visione di tutti i suoi collegamenti Figura 2 4 16 2 ARCHITETTURA PER IL CONTROLLO Gestione Emergenze SCHEDA D INTERFACCIA Japosua 0 1 z Ingresso Finecorsa Induttivi 1 0 Analogici 1 0 Digitali Figura 2 4 Foto della scheda di acquisizione dati Sensoray626 2 3 Teoria del controllo Il programma per effettuare il controllo del robot si deve occupare della gestione di tutti i ca
55. er poter reinterpolare correttamente la posizione del link 1 e 0 per azzerare il contatore La terza categoria di emergenze considerata nel subystem controllo velo cit Figura 3 40 qui non solo viene controllato che la velocit del link 1 e 2 non superi quella massima ammissibile ma vengono anche calcolate le velocit e le accelerazioni Avendo un feedback solo di posizione dei due link ne viene fatta la derivata discreta per ottenere la velocit che viene memorizzata rispettivamente nelle va riabili v_linkl e v_link2 e queste vengono derivate ulteriormente per ottenere le accelerazioni memorizzate nelle variabili a_link1 e a_link2 Le velocit vengono confrontate con la velocit massima ammissibile che stata 72 3 CONTROLLO DEL ROBOT 10 function CONTROLLO VELOCITA rad rad ms K z 1 Tsz encoder_link2 P v_link2 if u1 0 gt Aut v_max_link2 elseif u1 1 y elseif i rad msn2 Ken a_link2 Tsz emergenza no emergenza rad rad s encoder_link1 p En P vink Tsz gt if u1 0 gt u1 v max linkt P elseif ut 1 Toy elseif if rad msn2 K 2 1 eD __ a linkt Tsz pr emergenza no emergenza Figura 3 40
56. era ogni motore a se stante come se non fosse parte del robot Per migliorare 2 3 TEORIA DEL CONTROLLO 17 le sue prestazioni stata implementata la tecnica dell azione in avanti Questa tecnica corrisponde all aggiunta di una componente di feedforward e pu essere a sua volta decentralizzata o centralizzata Per il calcolo dell azione in avanti centralizzata occorre per conoscere le coppie che vanno ad agire in ogni giunto diventa quindi necessario implementare il modello dinamico anche per il controllo decentralizzato Prima di passare a descrivere nel dettaglio i due tipi di controllo viene allora effettuata l analisi dinamica inversa del manipolatore 2 3 1 Controllo Decentralizzato L idea del controllo decentralizzato quella di controllare ogni link come se fosse un giunto indipendente staccato dal resto del robot L effetto che pu avere il movimento del resto del robot sul link in considerazio ne viene considerato come l azione di una coppia resistente come un disturbo quindi non si cerca di compensarne la causa ma solo l effetto finale Un controllo decentralizzato cos strutturato Figura 2 5 Questo schema rende l idea di come avviene il controllo del robot intero AZIONE IN AVANTI POSIZIONE REALE RIFERIMENTO CONTROLLO ASSE 2 gt MOTORE 2 B POSIZIONI AZIONE IN AVANTI PIANIFICATE C RIFERIMENTO POSIZIONE REALE CONTROLLO ASSE 1 _ MOTORE 1 Le RI
57. genza porta invece alla ricalibrazione di tutto il robot infatti se ad esempio il membro 0 arrivato su un finecorsa meccanico bisogna spegnere il quadro elettrico ma allora si spengono anche i motori e gli encoder non leggono pi la posizione L interno del subsystem emergenza analogo a quello del precedente ma cambia il valore assegnato a calibrazione Figura 3 37 68 3 CONTROLLO DEL ROBOT a Enable 2 calibrazione 0 gt Stato 0 PY iteratore_link2 0 gt iteratore linkt Figura 3 37 Subsystem emergenze temperature emergenza alimentazione finecorsa meccanici pulsante emergenza emergenza e Stato 0 per entrare nello stato di emergenza e calibrazione 2 che il valore iniziale della varibile e indica che il manipolatore deve essere calibrato e iteratore_link2 0 per far riniziare correttamente algoritmo di interpo lazione della posizione del link 2 e iteratore_link1 0 per far riniziare correttamente algoritmo di interpo lazione della posizione del link 1 Quando emergenza meccanica torner a zero l emergenza sar finita e si ripren der il ciclo dell macchina a stati Queste due categorie di emergenze sono quelle che possono essere generate dall 3 2 DESCRIZIONE DEL PROGRAMMA 69 elettronica ora ana
58. gital tempi Input temp2_14 Sensoray del 626 lauto Analog temp2 Input corrente_motore2_1 Sensoray Model 626 auto _ Analog corrente_motore2 Input corrente_motore1_2 Sensoray del 626 fauto Analog corrente_motore1 Input corrente_motore0_3 Sensoray Model 626 auto corrente_motore_stepper e finecorsa_sx_link2 finecorsa_dx_link2 finecorsa_dx_link1 finecorsa_sx_link1 finecorsa_sx_linkO finecorsa_dx_linkO per rilevare il passaggio sui finecor sa induttivi Che segnano 0 se il link non sopra il finecorsa induttivo 1 quando lo fa attivare I canali di ingressi digitali utilizzati sono rispettivamente 6 5 4 3 2 1 Ingressi digitali x6 e emergenza_meccanica corrisponde ad un rel che attiva il circuito d emer genza quindi serve per leggere se il sistema per qualche motivo entrato in emergenza La logica di funzionamento 1 se siamo in emergenza e quin di il manipolatore bloccato dall elettronica e non si pu farlo muovere 3 2 DESCRIZIONE DEL PROGRAMMA 63 via software 0 se non ci sono emergenze I possibili motivi per cui questo ingresso pu passare allo stato logico alto sono attivazione di uno qualsiasi dei finecorsa meccanici presseione del pulsante di emergenza non arriva l alimentazione ai motori Il canale di ingresso digitale util
59. glia mo far arrivare il carrello Se la differenza maggiore di zero vuole dire che la sua posizione ancora tra il finecorsa induttivo destro e la posizione desiderata e deve essere corretta Questa condizione continua fino al raggiungimento della posizione di home La correzione di posizione avviene all interno del subsystem allineamento link 0 in cui si entra grazie ad un blocchetto if solo quando la differenza precedente strettamente maggiore di 0 Figura 3 21 Viene inviato il numero che risulta dalla sottrazione come numero di passi da fare al controllo che si occuper di controllare se il numero di passi richiesti fattibile dal motore passp in un solo ciclo di clock e in caso contrario di suddivi derli in modo corretto e di aggiornare la posizione del link 0 Inoltre direzione_link0 posta uguale a zero perch adesso i passi devono essere fatti in direzione negativa in quanto il carrello deve per forza tornare indietro Alla fine della calibrazione dei link 1 e 2 si era detto che il robot era totalmente calibrato e si poteva passare allo stato successivo affermazione che sembrerebbe non considerare il tempo di calibrazione del link 0 Ma questo non esatto Il raggiungimento della posizione di home del link 0 sicuramente inferiore ai 2 sec pianificati per gli altri due link Infatti il carrello deve percorrere met guida che corrisponde a 1500 passi che eseguendoli a 5 passi al millisecondo corri
60. gno driver in tensione ki 5 6 guadagno nell anello di corrente driver f max passi 5000 passi secondo frequenza massima di passi che puo fare il motore stepper in un seci max passi clock f max passi 1000 passi msecondo massima frequenza di passi che puo fare il motore steppe PIC ten max pic Se V tensione massima dei pic ten min pic 0 V tensione minima dei pic kpassi2v ten max pic max passi clock conversione dei passi che deve fare il motore stepper in tensione ENCODER tacche_giro 4000 numero di tacche dell encoder per giro ke2r 2 pi tacche_ giro conversione del numero dato dalla lettura degli encoder alla posizione in radianti DATI PER IL CALCOLO DELLE COPPIE PER IL CONTROLLO CENTRALIZZATO I2a 0 00112 amp kq m 2 inerzia del link rispetto al giunto di unione con il linkl Ilo 0 00422 kg m 2 inerzia del linki rispetto al giunto di unione con il link0 I2g I2a m2 12 2 CONVERSIONI D kr2v_link2 5 delta fc cal link2 ke2v_linki 5 delte fc val linki kg2v_link2 kr2v_link2 kg2r kg2v linkl kr2v linki kg2r kp2m 9 9001 conversione da radianti a volt link2 cunversiune da radianti a vult Linki conversione da gradi a volt link2 conversione da gradi a volt linki conversione da passi a metri d d dP d dP Figura B 2 Immagine file Parametri utilizzato per caricare le costanti utilizzate nel programma Parte B 124 B FILE PARAMETRI DAT
61. i home 3 2 DESCRIZIONE DEL PROGRAMMA 39 La posizione di home scelta e il carrello viene portato al centro della guida lineare che corrsiponde al passo numero 1500 e cio alla posizione lineare 15 cm e il link 1 viene portato in posizione perpendicolare alla guida lineare 0 1 90 e il link 2 viene allineato con il link 1 6 2 0 Tutti e tre i link vengono fatti muovere contemporaneamente Figura 3 11 Il subsystem composto di due parti principali quella pi in alto che si occupa elseif j t amin_link2_cal amin_link1_cal amax_link2_cal amax_link1_cal vmax_link2_cal vmax_link1_cal Action Port ALGORITMO POSIZIONE DI HOME j Hu else ooo Action Action calcolo posizionelink2e1 calcolo vmax amax amin pos_link0_attuale es ul if u1 gt 0 If TO meta_guida gt n passi allineamento link 0 Figura 3 11 Subsystem Calcolo riferimento calibrazione posizione di home di portare nella posizione di home i link 1 e 2 la seconda pi in basso che si occupa del link0 Si descrivono queste due parti in modo separato ma ricordiamo 40 3 CONTROLLO DEL ROBOT che vengono eseguite entrambe ad ogni ciclo di simulazione finch il valore di calibrazione uguale a 1 Viene ora descitta la prima quella riguardante la cali
62. i posizione viene inviato anche a Matlab che con le posizioni cos ottenute pu rappresentare il manipolatore sulla GUI secondo i suoi movi menti reali e per questo motivo se l aggiornamento del disegno sulla GUI avviene ogni 0 04 secondi pi che sufficiente per far apparire il movimento continuo all 12 2 ARCHITETTURA PER IL CONTROLLO occhio umano 2 2 Layout generale Il programma Simulink prevede dei blocchetti appositi per l invio dei segnali voluti alla scheda di acquisizione da cui escono dei segnali fisici di tensione i blocchetti si chiamano Analog Output e Digital Output La scheda di acquisizione riceve poi i segnali del sistema reale che sono leggibili nel programma tramite i blocchetti Analog Input Digital Input ed Encoder Input Quindi questa scheda il collegamento tra il sistema reale ed il sistema virtuale Da questa partono due connettori piatti da 50 pin uno per gli ingressi e le uscite analogiche e uno per gli ingressi e le uscite digitali ed uno da 26 per la lettura encoder Questi connettori arrivano al quadro elettrico e vanno a collegarsi ad una scheda d interfaccia questa scheda elettronica che permette poi indirizzamento dei segnali nei corretti punti di applicazione Queste due schede elettroniche sono quindi fondamentali per la gestione degli ingressi e delle uscite e meritano di essere spiegate pi nel dettaglio 2 2 1 Sensoray 626 Ques
63. i seguito spiegato il significato di alcuni fondamentali e blocchetto Data Store Memory Serve per creare una variabile e inizializ zarla possibile impostare il nome della variabile e il suo valore iniziale La variabile pu essere poi richiamata con il blocchetto Data Store Read e sovrascritta con il blocchetto Data Store Write e blocchetto Function Call generator Viene utilizzato insieme al blocchetto demux e al blocchetto Function Call Subsystem permette di dare un ordi ne a questi specifici subsystem Infatti in generale disponendo dei blocchetti in un foglio di lavoro Simulink non si pu essere sicuri dell ordine con cui verranno eseguiti dal programma I vari subsystem verranno eseguiti alla massima velocit possibile per la simulazione e blocchetto Switch Case Viene utilizzato insieme al blocchetto Switch Case Action Subsystem ed comandato da una variabile quindi ad esempio un Data Store Read Al variare del valore della varibile questo blocco impone di eseguire il codice dentro ad uno specifico subsystem senza entrare negli altri Inoltre stato realizzato un file Matlab in cui sono stati memorizzati tutti i dati costanti utili al funzionamento 3 2 DESCRIZIONE DEL PROGRAMMA 27 Durante la lettura del codice si vedranno spesso delle costanti utilizzate queste vengono lette dal workspace in cui vengono caricate eseguendo il file m Il file si chia
64. i velocit e posizione si ottengono le accelerazioni nuove da imporre Questi ingressi entrano nel subsystem dinamica inversa Figura 3 62 al suo interno viene inizialmente calcolato il vettore delle accelerazioni di corre zione gpp secondo equazione qpp appa Kp gqpa qp Kp da 9 3 10 infine questo vettore delle accelerazioni insieme all accelerazione del link 0 alle posizioni e velocit reali dei link 1 e 2 vengono inserite nel modello dinamico e forniscono le coppie che i motori devono erogare per inseguire la traiettoria se condo le equazioni 98 3 CONTROLLO DEL ROBOT CALCOLO COPPIE PER MOTORI 1 E 2 theta link2 reale Imi Ho m2 a1 2 I2a 2 m2 at I2 eos u t u 6 a linkt 12a m2 a1 12 cos u 1 u 3 a link2 2 m2 a1 i2 sin u 1 u 2 u 5 link v_linkt m2 at I2 sin u 1 u 2 2 velocit link2 reale ne link2 controllata v_link2 theta lint reale m2 I2 sin u 4 u 1 m2 at sin u 4 mi It sin u 4 u 7 ED adpp Velocit link reale a linko Om Qs appd0 Im2 12a u 3 a link2 m2 ati2 sin u 1 u 5 2 vinkt 2a m2 at I2 cos u 1 u 6 a_linkt m2 I2 sin u 1 u 4 U 7 a Jinko mL Cm2 Figura 3 62 Subsystem Calcolo controllo controllo
65. idotta e il sistema UDP pi rapido e si adatta meglio a comunicazioni in tempo reale Il protocollo TCP comunque pi affidabile e garantisce l arrivo dei dati al de stinatario non cos per UDP e si sono dovuti implementare delle verifiche di ricezione dei dati per compensare la perdita dei pacchetti Gli standard del protocollo di trasporto non specificano come questo debba in teragire con gli applicativi Avviene tramite un interfaccia detta Socket in cui necessario inserire l indirizzo IP dell host destinatario per inviare i dati e nel nostro caso siccome si vogliono sia inviare sia ricevere i dati in entrambi gli host in ognuno si inserir l indirizzo IP dell altro uesto indirizzo serve per inviare i dati all altro calcolatore ma per arrivare alla 7 2 1 COMUNICAZIONE MATLAB SIMULINK 11 specifica applicazione necessario inserire anche la porta di comunicazione ogni programma avr la propria che prende il nome di Local port che quella in cui si ricevono i dati la Local port dell altro programma prende il nome di Remote port ed dove vengono inviati i dati I dati vengono inviati da Matlab a Simulink ad una frequenza di 100 Hz grazie alla creazione in Matlab di un oggetto timer che temporizza i vettori da inviare ed invia un elemento per ogni vettore ogni 0 01 secondi Purtroppo I invio dei dati con quell intervallo temporale non corrisponde ad una ricezione ana
66. il calcolo dell azione in avanti centralizzata Ki Ieq r Cr Vref a Fi if 4 3 9 Kt Per il motore 1 il procedimento e la logica esattamente lo stesso vengono uti lizzati gli stessi nomi della variabili cambiando il numero 2 in 1 la tensione dell azione in avanti viene quindi memorizzata nella variabile Vref_a_link2 90 3 CONTROLLO DEL ROBOT 3 2 6 Calcolo controllo In questo subsystem vengono implementati i due tipi di controllo visti per i link 1 e 2 e il controllo per il link 0 Figura 3 55 All interno del sotto sistema un altro function call divide subito il controllo dei function CONTROLLO LINK j function Function Call Generator controllo carrello function controllo link1 e2 Figura 3 55 Subsystem Calcolo controllo vari membri il link 0 verr calcolato in un modo e gli altri due che vengono con trollati in modo analogo in un altro Il primo function call subsystem controllo carrello Figura 3 56 si occupa di verificare che il numero di passi da far fare al motore passo passo per quel clock di simulazione non sia superiore a quello massimo possibile si effettua quindi in realt un controllo sulla frequenza dei passi da inviare e si aggiorna la variabile della posizione del link Il numero di passi per millisecondo che viene consentito di fare 5 Fatta la differenza tra passi_link0 ch
67. ion emergenza alimentazione finecorsa meccanici pulsante emergnza function 10 Function Call controllo funzionamento encoder Generator function controllo velocita function controllo posizione function controllo finecorsa induttivi Figura 3 33 Subsystem emergenze rappresentano le sei categorie di emergenze che si possono attivare Tutti i tipi di emergenza sono stati distinti per rendere pi leggibile in codice e capire meglio quale emergenza stata attivata Ma come stato accennato nel paragrafo Calcolo riferimento tutte le emergenze portano a far frenare i motori e si distinguono in due tipi quelle che portano alla ricalibrazione di tutto il robot e quelle invece che permettono di mantene re la lettura encoder e quindi non necessario ricalibrare i membri al loro termine La prima categoria d emergenza considerata in temperature Figura 3 34 viene controllata la temperatura dei quattro stati di potenza Gli integrati LMD gli integrti principali degli stadi di potenza erogano la corrente ai motori Avendo i motori una maggiore area di dissipazione del calore avranno temperatura inferiore a quella degli integrati quindi monitorando la corrente di 3 2 DESCRIZIONE DEL PROGRAMMA 65 D function CONTROLLO TEMPERATURE temp0a md 0 gt _ gt temp0b m 0 Il
68. ioni 3 2 DESCRIZIONE DEL PROGRAMMA 41 Vv fin din mar T alibrazione ta td 2 v Omar Poe 3 1 v Amin a dove T_calibrazione il tempo totale per cui stato pianificato il movimento ta il tempo di accelerazione e td il tempo di decelerazione q_fin q_in viene sosti tuito con la costante delta_fc_cal_link1 o delta_fc_cal_link2 rispettivamente per il primo e secondo membro che esprime appunto la differenza tra la posizione di calibrazione e la posizione del finecorsa quindi la posizione iniziale e finale della traiettoria in tensione Queste variabili vengono caricate dal file Matlab I calcoli sono analoghi per i due membri cambia solo il nome delle variabili uti lizzate I risultati delle equazioni sono salvati nelle omonime variabili Calcolate le velocit massime e le accelerazione massime e minime possibile pianificare la traiettoria per portare il manipolatore in posizione di home Conosciamo infatti la posizione iniziale e la posizione finale quindi sarebbe stato inadeguato far arrivare i link in posizione di home ancora con dei semplici incre menti Il tipo di pianificazione scelto a profilo trapezoidale di velocit Alla variabile j viene assegnato il valore 1 quindi si passa nel subsystem calcolo posizione link 2 e 1 Figura 3 13 42 3 CONTROLLO DEL ROBOT Action Action Port ALGORITMO POSIZIONE DI HOME LINK 1 E 2 function
69. izzato il 24 Ingresso digitale x1 e temp0a temp0b temp1 temp2 sono anche questi segnali d emergen za Si riferiscono alle temperature degli stadi in potenza dei tre motori il motore stepper relativo al link 0 utilizza due LMD e quindi ha bisogno da solo di due ingressi di temperatura che vengono chiamati a e b Funzionano con la logica inversa quando viene letto 0 vuol dire che la temperatura ha superato il massimo tollerabile quando viene letto 1 la temperatura dentro ai valori ammissibili I canali di ingressi digitali utilizzati sono rispettivamente 15 11 12 14 Ingressi digitali x4 e corrente motorel corrente motore2 corrente motore stepper sono i mo nitor di corrente che passa attraverso gli LMD I canali di ingresso analogici utilizzati sono rispettivamente 1 2 3 Ingressi analogici x3 Questo stato selezionato per essere il primo subsystem in quanto essendo il collegamento con ci che avviene nella realt ovvio che da quello che leggiamo da qui possiamo andare ad agire per modificare correggere o mantenere azione del manipolatore 3 2 3 Emergenze Il secondo Function Call Subsystem serve a controllare eventuali stati di emer genza Figura 3 33 E suddiviso a sua volta in un function call generator con sei subsystem che 64 3 CONTROLLO DEL ROBOT 10 EMERGENZE function v function temperature Y funct
70. l robot perch per problemi tecnici non stato assemblato in tempo e non stato testato il funzionamento del quadro elettrico con i motori Provare il programma direttamente sul robot costruito comunque altamente sconsigliabile in quanto si potrebbero generare problemi irreparabili sia per pro blemi del software che dell elettronica 4 1 simulazioni Le prime simulazioni fatte sono state fatte a livello software quindi con la comu nicazione dei due calcolatori senza utilizzo dell elettronica Si sono sostituiti tutti i blocchetti Analog input e Digital input presenti nel sub system Ingressi Sensoray 626 per leggere gli ingressi della scheda di acquisizione con dei segnali fittizi che potessero simulare quelli reali dovuti ai comportamenti del robot Al posto di tutti gli ingressi digitali sono stati posti delle specie di impulsi di durata almeno 0 001 s affinch possano essere letti dalla simulazione ottenuti tramite la differenza di gradini che si attivano in istanti temporali diversi In que 106 4 TEST sto modo viene simulato il passaggio dallo stato logico basso allo stato logico alto o viceversa di tutti gli ingressi Oppure nel caso in cui si volesse tenere un ingresso a livello logico solo alto o solo basso con delle costanti pari rispettivamente ad 1 o 0 Al posto degli ingressi analogici per la lettura di posizione dei link 1 e 2 sono sta te poste le variabili encoder_link2
71. la ricalibrazione totale del manipolatore altre invece non alterano la lettura encoder e la posizione del motore passo e quindi una volta terminate si conosce la posizione di tutti e tre i link e non serve quindi ricalibrare I due casi si distinguono grazie alla variabile calibrazione che come vedremo suc cessivamente 2 il valore iniziale e significa che il robot deve essere calibrato 2 che il valore che indica che il robot calibrato L assegnazione della variabile 32 3 CONTROLLO DEL ROBOT avviene quando viene generata ogni emergenza e cos si pu decidere il suo effetto Attraverso il blocco if se calibrazione 2 si passa nel sottosistema calibrazio ne in cui viene assegnato Stato 1 e si passa quindi nello stato di attesa per la calibrazione se calibrazione 2 si passa nel sottosistema no calibrazione f NO 3 1 toi in cui viene assegnato Stato 3 e si passa quindi nello stato di attesa per il funzionamento normale Stato 1 messa in moto Questo uno stato di attesa in cui il robot non in emergenza ed pronto per essere calibrato Non vengono quindi eseguiti calcoli o operazioni di alcun genere qui dentro Per entrare nello stato calibrazione non avviene un passaggio interno al programma Simulink ma comandato dal programma Matlab Come vedremo meglio nei Function call subsystem ricezione dati matlab ed nvio dati matlab la variabile stato vi
72. la schermata iniziale e via via si scende nei livelli inferiori In questa facciata sono presenti 8 subsystem del tipo Function Call dentro i quali saranno presenti altri subsystem e cos via Il programma viene fatto girare in tempo reale utilizzando Real Time Windows Target per permettere una maggiore velocit di esecuzione e quindi un controllo pi veloce stato scelto di far girare la simulazione alla frequenza di 1 kHz Per eseguire il programma in tempo reale necessario impostare i seguenti para 26 3 CONTROLLO DEL ROBOT metri selezionare la modalit External invece che Normal nel men Simulation selezionare Configuration parameters dentro cui nel men Solver impostare Fixed Step type al tempo di campionamento desiderato 0 001 s e nel men Real Time Workshop impostare System target file a rtwintgt tlc Per eseguire la simulazione necessario impostare Simulation mode ad Exter nal e premere in ordine Build Connect to Target e Start In tutti i blocchetti in cui era necessario impostare il tempo di campionamento stato messo 1 questo dato indica che viene ereditato il tempo di campionamento dai blocchetti precedenti se viene fatto cos in tutti i blocchetti tutti quanti alla fine erediteranno il tempo della simulazione e quindi 0 001 secondi Oltre i blocchetti pi comuni stato necessario ricorrere a diversi altri blocchetti e viene d
73. le Posizione link 1 Figura 4 5 Posizione link 2 Figura 4 6 posizione link reale posizione link 1 pianificata radianti tempo s Figura 4 5 Grafico della posizione controllata del link 1 nella fase di calibrazione 110 4 TEST Nel subsystem generatore uscite sono stati sostituiti tutti i blocchetti Analog radianti posizione link 2 reale posizione link 2 pianificata tempo s Figura 4 6 Grafico della posizione controllata del link 2 nella fase di calibrazione Output e Digtal Output con degli semplici Scope per poter osservare il com portamento delle uscite Permettendo cos di osservare i diversi casi e di andare a correggere il programma fino al corretto funzionamento Figura 4 7 10 function tensione_link2 gt end_effector gt direzione_link2 gt ldriver_tensione_corrente2 gt tensione_link1 gt ldriver_tensione_corrente1 gt direzione_link1 gt emergenza gt ee M tensione_linkO gt direzione_linkO gt Figura 4 7 Subsystem generatore uscite per la simulazione Per testare la schede sono stati utilizzati dei file di prova
74. le di home e disegna il manipolatore con queste posizioni e viene inviato Stato 4 al calcolatore B L utente da ora ha la possibilit di far muovere il robot secondo le possibilit previste dal programma Matlab Ad esempio premendo nello spazio operativo simulato nella GUI il robot si muover per raggiungerlo Per quanto riguarda il programma Simulink quando viene premuto il pulsante go si riceve Stato 4 la macchina a stati entra nel suo ultimo stato possibile Stato 4 funzionamento normale Questo stato si pu considerare il principale in quanto viene calcolato il riferi mento di posizione per tuttii movimenti voluti dall utente in condizioni normali Il problema principale nasce dal fatto che come vedremo meglio nel function call subsystem ricezione dati matlab i dati inviati da matlab vengono spediti a 100 Hz quindi ogni centesimo di secondo mentre la simulazione Simulink gira 1 kHz quindi dieci volte pi veloce Diventa quindi necessario effettuare un interpolazione tra le posizioni inviate da Matlab Se cos non fosse si perderebbe il vantaggio di far girare la simulazione sotto Real Time Windows Target a questa frequenza inoltre ci sarebbe il rischio di dare come riferimento di posizione dei gradini di ampiezza eccessiva che por terebbero a gravi problemi di controllo nell inseguimento Inoltre bisogna considerare che non sempre i pacchetti dati inviati riescono ad essere inviati o
75. linkl e2 controllo centralizzato abilitazione controllo centralizzato dinamica inversa Cm2 Im D 1 O2 DA Mya l2c05 Qz O Mga lysen Oz O Cmi Imi Lio ma 2mya I2c08 O2 In 4 O1 T24 mza lz cos92 gt mglosen 0 O2 z 2mga losen 93 0 si moaylosen Q2 O maz lasen 0 O2 aysin O mil sin 01 3 2 7 Generatore Uscite 2 3 11 In questo function call subsystem vengono inviate tutte le uscite calcolate alla scheda Sensoray 626 Come per il primo function call in cui si leggevano tutti gli ingressi della scheda anche in questo caso stato scelto di unire tutti i dati inviati invece di inviarli 3 2 DESCRIZIONE DEL PROGRAMMA 99 ogni volta che il segnale viene generato nei precedenti subsystem per essere sicuri che l invio avvenga nello stesso momento e per ottenere una maggiore leggibilit e comodit nel vedere le uscite inviate Figura 3 63 Le uscite inviate sono in tutto 10 3 analogiche e 7 digitali i blocchetti utilizzati o function M Anal EEEE tensione link2 p aog end_effector Digita Output Output tensione_link2_3 enc_effetctor_7 Sensoray Sensoray Model 626 auto Model 626 auto direzione_link2 p Distal ldriver_tensione_corrente2
76. lizziamo quelle che pu generare il programma Simulink La terza categoria di emergenze considerata in controllo funzionamento en coder Figura 3 38 serve per controllare che arrivino i dati di lettura degli encoder potrebbe infatti 10 function si SS CONTROLLO FUNZIONAMENTO ENCODER rad encoder_link2 r gt 0 gt sa gt AND gt gt i TT rac lposizione_link2 P 0 gt A L pu if ut 0 fe ee if rad reset encoder_link1 y P 0 gt AND pi gt j a td cn posizione_link1 di P 0 Ney j ali ut if u1 0 13 if gt oes if ut 0 gt pt reset elseif ut 1 max_no_lettura_encoder ___ Vv elseif TO emergenza encoder link2 no emergenza i moo H i if ut 0 gt __ ut elseif ut 1 max_no_lettura_encoder _ __ DI elseif it emergenza encoder linkt no emergenza Figura 3 38 Subsystem emergenze controllo funzionamento encoder succedere che si scolleghi il cavo di collegamento tra gli encoder e gli ingressi della Sensoray o che venga
77. loga sia per i problemi del protocollo UDP accennati prece dentemente sia perch l oggetto timer scrive i dati in un buffer e capita che per problemi si accumulino e vengano inviati poi con delta temporali di appena 0 001 secondi L invio dei dati da Simulink a Matlab avviene invece a 25 Hz questo perch non necessario ai fini dell utilizzo che ne far Matlab avere una frequenza maggiore In questo caso l invio avviene pi semplicemente infatti come vedremo meglio nel prossimo capitolo esiste gia un blocchetto apposito per invio dei dati Il programma Matlab riceve i dati con una funzione recive che viene chiamata da un altro oggetto timer alla frequenza di 25 Hz Dato che il calcolatore A comunica tramite la GUI con I utente e il calcolatore B tramite la Sensoray 626 al robot la comunicazione tra i due fondamentale sia per far muovere correttamente il manipolatore sia come feedback per l utente stesso Infatti tramite la GUI viene data la possibilit all utente di far muovere il ma nipolatore a seconda del movimento richiesto vengono pianificate le specifiche traiettorie e inviate al calcolatore B che invia i riferimenti di posizione ai driver che mandano i segnali di tensione ai motori per effettuare il movimento tramite gli encoder posti sui motori stessi passando attraverso la Sensoray 626 arriva al controllo un feedback di posizione con cui pu correggere l errore di posizione Questo feedback d
78. ma Il primo sottosistema ad essere eseguito calcolo della coppia resistente Figura 3 01 Vengono uniti in un unico vettore attraverso il blocchetto Mux che pu ricevere 0 function m2 at l2 sin u 1 u 5 2 gt _ gt v_link1 encoder_link2 zi Nm l2a m2 at l2 cos u 1 u 6 gt gt cr rad ms a _link1 coppia resistemte motore2 v_link2 m2 l2 sin u 1 u 4 u 7 m T rad msn2 a linko a_lin encoder_link1 I2a m2 a1 l2 cos u 1 u 3 rad ms v_link1 A a_link2 rad ms 2 2 m2 a1 l2 sin u 1 u 2 u 5 _ a_linkt vinkt v_link2 Nm crt a linko rit ee marat l2 sin u 1 ut I coppia resistemie motore v_link2 p m2 2 sin u 4 u 1 m2 a1 sin u 4 m1 l1 sin u 4 u 7 gt a_linko Figura 3 51 Subsystem Calcolo azione in avanti calcolo coppia resistente n ingressi e li unisce in un unica uscita tutti gli ingressi necessari le posizioni dei link 1 e 2 lette dagli encoder le rispettive velocit ed accelerazioni e acce lerazione del link 0 L uscita del Mux viene posta in ingresso a dei blocchetti fen che permettono al loro interno di scrivere tutte le aperazioni matematiche desiderate e richiamare 86 3 CONTROLLO DEL ROBOT i vari valori dell ingresso chiamandoli come
79. ma che se non fosse spiegata subito renderebbe in comprensibili diverse parti di tutto il resto del programma 28 3 CONTROLLO DEL ROBOT Stato posizione_link2 posizione_linkt pos_link0_attvale function passi_link0 posizione_linkO tensione_link2 tensione_linki ingressi sensoray tensione_link0 direzione_link2 direzione_linkt direzione_linkO function encoder _link2 encoder _linkt offset_link2 offset_linkt emergenze 7 9 V_link2_rif a_link2_rif v_link1_rif a link1_rif function vlinko_sif a_linko_sif vlink2 a_link2 ricezione dati matlab vinkt a_linkt controllo azione_in_avanti function 10 Vref_a_linki Vret_a_link2 Cm2 comi Function Call calcolo riferimento Generator cre Crt driver_tensione_corrente2 ldriver_tensione_correntet function stato_matlab iteratore_link2 iteratore_linkt ricezione_dati calcolo azione in avanti end_effector emergenza emergenza_meccanica emergenza_encoder function emergenza_induttivite2 emergenza_induttivio emergenza_posizione emergenza_velocita calcolo controllo temp0a tempob tempi temp2 function finecorsa_sx_link2 finecorsa_dx_link2 finecorsa_sx_link1 finecorsa_dx_link1 generatore uscite finecorsa_dx_linkO finecorsa_sx_link0 corrente_
80. ma parametri e viene riportato nell appendice B Il programma fatto il risultato di numerosi tentativi e cambiamenti che hanno portato ad un programma efficiente e abbastanza intuitivo da capire Bisogna per premettere che funzionando con un continuo ciclo del codice scritto spesso si incontreranno delle condizioni imposte per il corretto funzionamento e P utilizzo di alcune variabili non possibili da comprendere completamente sul momento perch verranno spiegate e definite successivamente Si avr quindi alla fine una corretta e completa visione del programma in tutte le sue parti 3 2 Descrizione del programma La schermata iniziale mostra esplicitamente qual la logica seguita Figura 3 1 Notiamo come prima cosa tutti blocchetti Data Store Memory nella parte destra sono inizializzazioni delle variabili utilizzate nel programma e sono posi zionati a questo livello per essere visibili anche in tutti i livelli inferiori quindi in tutto il programma Si vede poi il blocchetto Function Call Generator che chiama nell ordine i prin cipali subsystem del programma Questi sottosistemi rappresentano le operazioni principali del programma che ven gono eseguite al loro interno e ci danno quindi gi un idea dell logica seguita Verranno analizzati nell ordine dall alto verso il basso che l ordine con cui vengono eseguiti da Simulink ad eccezione del subsystem Calcolo riferimento Parte cardine del program
81. motore2 corrente_motoret function conteggio conteggio1 corrente_motore_stepper invio dati a matlab Figura 3 1 Schermata iniziale 3 2 1 Calcolo riferimento In questo function call subsystem viene calcolata la posizione di riferimento per tutti e tre i link sia nella fase di calibrazione sia in quella di funzionamento nor male che devono poi essere inseguite dal controllo Nel suo interno quindi stata implementata la macchina a stati finiti con I utilizzo del blocchetto switch case gi spiegata in via teorica nel capitolo pre cedente Figura 3 2 La variabile Stato comanda il passaggio da un case all altro diventando quindi una della variabili fondamentali del programma Come si pu vedere dalla figura sono stati assegnati a dei valori della variabile gli stati previsti e Stato 0 emergenza il robot non abilitato a muoversi 3 2 DESCRIZIONE DEL PROGRAMMA 29 0 function MACCHINA A STATI case 0 case emergenza case 1 case messa in moto Stato u case 2 1 case calibrazione case 3 t case calibrato case 4 t case funzionamento normale Figura 3 2 Subsystem Calcolo riferimento Utilizza una macchina a stati e Stato 1 gt messa in marcia il robot pronto per essere calibrato
82. n Simulink Se il tipo di controllo scelto quello decentralizzato si entra nel subsystem con trollo decentralizzato Figura 3 60 dove si presentano due schemi retroazionati visti nella teoria Spieghiamo ora lo schema di retroazione per il controllo del motore 2 L ingresso posizione link che rappresenta il riferimento di posizione deside rato che stato calcolato nel subsystem Calcolo riferimento a questo viene tolto il feedback di posizione che il valore letto dagli encoder encoder_link2 L errore di posizione entra nel controllore lo switch manuale permette la scelta di quale controllore utilizzare Sono date due possibilit una funzione di trasfe rimento di un controllore PD Cp progettata specificatamente per la funzione 3 2 DESCRIZIONE DEL PROGRAMMA 95 if CONTROLLO DECENTRALIZZATO LINK 2 _ _ _ _ VI Vref_a_link2 M rad la gt zeros z su Action Port posizione _link2 Pt _____ z C2poli ie Cpd M Stato gt jul gt 4 P tensione_link2 P PID z a rad 0 m encoder_link2 gt Pidirezione_link2 CONTROLLO DECENTRALIZZATO LINK 1 M Vref_a_link1 rad zeros z ss p
83. n condizioni di sicurezza con gli encoder che continuano a leggere la posizione Mentre nel secondo caso quando si entra nello stato di emergenza il motore stepper continua ad erogare una coppia che continua a tenere bloccato dove il carrello l unico modo per muoverlo togliere l alimentazione al quadro elettrico 3 2 DESCRIZIONE DEL PROGRAMMA TT Questo corrisponde al fatto che gli encoder non leggono pi la posizione mentre si sposta il manipolatore a mano e non si conosce pi la posizione del link 0 non avendo un feedback quindi necessario ricalibrare tutto il robot Nella prima porta OR sono posti come ingressi tutti e quattro i finecorsa in duttivi dei due link e la verifica se la posizione letta dagli encoder non oltre un finecorsa induttivo Per fare questo viene controllato se la lettura encoder inferiore alla posizione dei finecorsa destri e se maggiore dei finecorsa sinistri Queste condizioni sono necessarie perch tra il finecorsa induttivo e il finecorsa meccanico intercorre dello spazio in cui nessuno dei due tipi di finecorsa attivo se il membro arriva a quella posizione senza essere fermato deve attivarsi l emer genza L emergenza avrebbe dovuto attivarsi gi con induttivo ma questa un ulteriore sicurezza La porta OR garantisce che se si attiva una qualsiasi delle condizioni l uscita diventa true e convertita in double fa attivare il subsystem emergenza Figu
84. ncoder calibrazione viene impostata a 0 Si dunque scelto di far muovere il link 2 tenendo il link 1 chiuso e non portan dolo gi nella posizione di calibrazione questo richiederebbe infatti pi coppia per mantenere il link 2 in posizione Si passa quindi alla chiusura del link 0 Anche per questo link il procedimento analogo Figura 3 9 Questo link necessita della frequenza con cui fare i passi e della direzione in cui farli gli viene dato da fare 1 passo ms quindi la costante passi_calibrazione 3 2 DESCRIZIONE DEL PROGRAMMA 37 elseif Action Port ALGORITMO INIZIALIZZAZIONE LINK 0 passi_calibrazione passi_linkO 1 P direzione linko finecorsa_dx_linkO ah attivazionefinecorsalinkO Figura 3 9 Subsystem Calcolo riferimento gt calibrazione chiusura carrello 1 Quindi viene fatto muovere il pi lentamente possibile per avere maggiore precisione si perdono meno passi e si arriva esattamente nel primo passo che fa attivare il finecorsa induttivo Il sensore del finecorsa induttivo ha infatti ampiezza di circa 5 mm che corrispon dono a 50 passi e quindi se i passi fossero maggiori ad 1 si rischierebbe di arrivare ad un passo che fa attivare il finecorsa ma non quello iniziale e causerebbe uno sfasamento di passi considerarlo tale Viene inoltre assegnato a direzione link0 il valo
85. no per 3 2 DESCRIZIONE DEL PROGRAMMA 55 LO function ALGORITMO ITERATORI iteratore_link2 5 i iteratore_link2 iteratore_link1 a iteratore_link1 ricezione_dati 4 N azzeramento iteratori Figura 3 26 Subsystem Calcolo riferimento calibrazione funzionamento normale iteratori il link 1 iteratore_link1l e uno per il link 2 iteratore_link2 che aumentano i loro valori ogni volta che si entra in questo sottosistema di 1 Saranno utilizzati per interpolazione siccome il programma Simulink gira 10 volte pi velocemente di quanto vengono aggiornati i riferimenti di posizione da Matlab questi iteratori sono stati progettati per contare il numero di cicli che ha fatto Simulink dall ultimo aggiornamento dei riferimenti Infatti quando viene ricevuto un pacchetto dati nuovo ricezione dati diventa 1 e si attiva il subsystem enabled azzeramento iteratori al cui interno si assegna il valore 1 alle variabili iteratore_link2 ed iteratore_link1 e pu ripartire cos il conteggio Il valore posto ad 1 e non a zero perch si deve contare la volta in cui arriva il pacchetto nuovo di dati Nel secondo fucntion call sibsystem theta di riferimento del link2 Figura 3 27 viene calcolato il vero riferimento di posizione per il link 2 cio quello ottenuto 56 3 CONTROLLO DEL ROBOT
86. ntrollori utilizzati sono dei PID discreti semplici da utilizzare in quanto si devono tarare tre parametri sperimentalmente il guadagno proporzionale il gua 2 3 TEORIA DEL CONTROLLO 19 dagno derivativo e il guadagno integrativo Il problema di questo controllo che il disturbo che rappresenta la coppia re sistente che agisce sul link varia e quindi il controllore che non pu cambiare dinamicamente pu non controllare correttamente il movimento nei tempi desi derati Una tecnica per migliorare le prestazioni di questo tipo di controllo quella di inserire un azione in avanti Azione in avanti Nel sistema di controllo visto il controllore agisce solo a fronte di un errore d inseguimento altrimenti non produce tensione per prevenire errore Questa tecnica si occupa di aggiungere una tensione che vada a compensare le causa di errore prevedibili Prende il nome di azione in avanti perch agisce dopo il controllore Pu essere calcolata in modo decentralizzato o centralizzato e il calcolo varia a seconda se il driver utilizzato in tensione o in corrente Analizziamo prima il caso con driver in tensione Figura 2 7 con Gv guadagno del driver in tensione Per il calcolo dell azione in avanti decentralizzata si impone equazione Veta Gy Mis 2 1 e sostituendo la funzione di trasferimento del motore si ottiene N Ra leq Or Kv kt Gv Vefa 2 2 dove il primo termine se
87. o il lavoro XII Nel secondo capitolo viene invece spiegata l architettura del controllo cio qual il procedimento generale per effettuare il controllo come avviene la sua iterazione con la parte elettronica e meccanica del robot E la teoria e le tecniche che sono state utilizzate per progettare il controllo sulla movimentazione Nel terzo capitolo viene spiegato nel dettaglio il programma Simulink Quindi viene spiegato come sono state implementate nella pratica la teoria e le idee per lo sviluppo del controllo Nel quarto capitolo vengono descritti i test effettuati durante la progettazione XII Capitolo 1 Progetto In questa capitolo verr descritto il robot realizzato e 1 idea generale del lavoro svolto per arrivare al suo funzionamento Descrivendo il lavoro svolto dagli altri quattro tesisti che hanno collaborato al progetto 1 1 Descrizione del robot Il manipolatore realizzato uno SCARA acronimo di Selective Compliant As sembly Robot Arm a due membri dotato in aggiunta di una base mobile che permette il movimento del manipolatore lungo un asse lineare Il manipolatore grazie all aggiunta del terzo link quindi a tre gradi di libert L ausilio della base mobile porta ad una ridondanza nei movimenti infatti possibile raggiungere uno stesso punto dello spazio operativo con I end effector in diversi modi Questo ha portato ad un analisi per l ottimizzazione del movi mento per r
88. o funzionamento Mancano per le prove sperimentali del programma col robot Questo dovuto a problemi tecnici che hanno portato all assemblaggio del robot e al collegamento del quadro elettrico in ritardo rendendo impossibile effettuare alcun test Sono stati comunque testati i motori singolarmente simulando i riferimenti in viati dal programma ed hanno risposto correttamente 114 CONCLUSIONI Inoltre sono stati simulati tutti gli Input e Output e le risposte del programma sono state coerenti con quanto desiderato Questo non significa il raggiungimento del controllo dei motori dato il carico inerziale mancante e il sicuro funzionamento del programma per tutti le ulteriori complicazioni introdotte dal sistema reale completo Ma fa ben sperare per un proseguio del lavoro svolto Appendice A Analisi dinamica inversa del manipolatore Questa analisi ci permette note le posizioni le velocit e le accelerazioni di calco lare le coppie che devono erogare i motori per mantenere le traiettorie desiderate Consideriamo inizialmente i link 1 e 2 e successivamente studieremo anche il con tributo dato dal link 0 Per calcolare le coppie necessarie partiamo ipotizzando i due link separati dove forza di reazione vincolare che vincola il primo membro al carrello e si ri percuote opposta in quest ultimo R forza di reazione vincolare che vincola il secondo membro ad avere A nel membro 1 e questa forza si ripercu
89. o dell azione in avanti decentralizzata N Ra Ieq Or SS a 3 6 Gv L equazione per il calcolo dell azione in avanti centralizzata Vref a Raleg Ra C ee 3 7 Gu Se driver_tensione_corrente2 0 si entra nel subsystem link 2 driver in corren te Figura 3 54 Vee fia al suo interno prevista sia la possibilit di ottenere l azione in avanti centra if Action Port CALCOLO AZIONE IN AVANTI PER DRIVER IN CORRENTE LINK 2 Im2 12A ___ a_link2_rif _ gt Lp P ki kt2 gt H cre gt m P es lazione_in_avanti gt P Vref_a_link2 lazione_in_avanti gt H o cH 0 m Figura 3 54 Subsystem Calcolo azione in avanti gt link 2 driver in corrente lizzata decentralizzata o di non usare l azione in avanti analogamente al caso in tensione La tensione calcolata viene salvata sempre nella variabile Vref_a_link2 questo perch sar questa la variabile che verr presa in considerazione nel controllo e 3 2 DESCRIZIONE DEL PROGRAMMA 89 se si tratta di quella per il driver in tensione o in corrente questo dipende dalla scelta fatta dall utente L equazione per il calcolo dell azione in avanti decentralizzata Ki Ig Vre a i 3 8 L equazione per
90. o di funzionamento normale Se la condizione verificata si entra nel subsystem finecorsa induttivi Figura 3 44 Sono presenti due porte OR che dividono il subsystem in due casi uno per i link n CONTROLLO FINECORSA INDUTTIVI DURANTE IL FUNZIONAMENTO NORMALE Enable encoder lindo delta_fc_cal_link1 90 kg2r gt finecorsa_dx_link1 gt encoder lindo delta_fc_cal_link1 90 kg2r __ finecorsa_dx_link1 gt sea itut 0 OR double Put encoder lino elseif ut 1 lt k p v delta _fc_cal link ________ elseif i emergenza no emergenza finecorsa_dx_link2 gt encoder lin delta_fc_cal_link2 _ __________ gt finecorsa_sx_link2 gt linecorsa_dx_linkO gt ee if ut 0 OR double ut a elseif u1 1 finecorsa_sx_linkO gt Vv n Nn emergenza no emergenza Figura 3 44 Subsystem emergenze controllo finecorsa induttivi finecorsa induttivi 1 e 2 e uno per il link 0 Sono divisi i due casi perch il primo porta allo stato di emergenza ma non de ve essere ricalibrato tutto il robot in quanto entrati nello stato di emergenza la tensione erogata dai motori in continua nulla e i link possono essere spostati a mano per riportarli i
91. o posizione link 2 e 1 calcolo posizione fase di decelerazione O t Ofin 0 5 amin T 1 3 4 Il riferimento viene calcolato per entrambi i link in questo sottosistema valori utilizzati sono e Orin per il link 1 90 kg2r che corrisponde alla posizione di home per il membro specifico amin amin_link1_cal per il link 1 e Ofin per il link 2 0 la posizione di home prevede infatti il secondo link allineato con il primo 48 3 CONTROLLO DEL ROBOT amin amin link2_cal per il link 2 e T td lt t lt T Nel terzo function call subsystem controllo fine pianificazione Figura 3 19 viene controllato se la variabile t maggiore del valore 2 cio se il tempo della t0 function CONTROLLO FINE PIANIFICAZIONE t o ut if ut gt 2 Action posizione di home raggiunta Figura 3 19 Subsystem Calcolo riferimento calibrazione posizione di home allineamento link2el calcolo posizione link 2 e 1 calcolo posizione gt controllo fine pianificazione pianificazione supera quello previsto Se questo avviene significa che la posizione di home stata raggiunta o almeno i riferimenti di posizione per i motori hanno raggiunto quella posizione Poi dipende dalla precisione di inseguimento del controllo l errore della posizione reale rispetto a quella desiderata Quando questa condizione verificata viene a
92. oR double temp1 gt _ gt 0 gt Jl temp2 p o d emergenza Figura 3 34 Subsystem emergenze gt temperature quest ultimi si pu evitare anche di surriscaldare i motori I quattro segnali riferiti alle temperature sono digitali e indicano se il livello logico 1 che la temperatura entro i limiti per il funzionamento se il livello logico 0 che stata superata la temperatura limite corrispondente circa a 170 stato misurato che il motore a questa temperatura arriva circa a 150 Per il loro funzionamento a logica inversa nello schema Simulink stato previsto un blocchetto che uguaglia a zero questi ingresso digitali se l uguaglianza verificata e siamo quindi in emergenza l uscita del blocchetto diventa 1 se no zero Questo ci permette di considerarli come se funzionassero con la logica diretta Le uscite dei blocchetti comparatori sono gli ingressi di una porta OR che porta a false l uscita se tutte le temperature non sono in emergenza e a true se una invece si attiva La conversione in double permette I attivazione del subsystem 66 3 CONTROLLO DEL ROBOT emergenza Figura 3 35 Questa emergenza rientra nella tipologia che permette la lettura encoder e non a Enable 2 AA calibrazione Stato 0 iteratore_link2 i H
93. one posizioni il riferimento per la posizione del link 0 che indica la posizione succe siva che deve raggiungere il carrello Matlab la invia in mm e allora deve essere convertita in numero di passi secondo il guadagno kmm2p e viene salvata nella variabile posizione_link0 il riferimento per la posizione del link 1 espresso in radianti salvato nella variabili posizione_link1 il riferimento per la posizione del link 2 espresso in radianti salvato nella variabili posizione link2 e il valore da inviare al driver del motore 2 per scegliere il funzionamento in tensione o corrente Il primo corrisponde al valore 1 il secondo al valore 0 Questo dato viene memorizzato nella variabile driver_tensione_corrente2 3 2 DESCRIZIONE DEL PROGRAMMA 83 e il valore da inviare al driver del motore 1 per scegliere il funzionamento in tensione o corrente Il primo corrisponde al valore 1 il secondo al valore 0 Questo dato viene memorizzato nella variabile driver_tensione_corrente1 e il valore per utilizzare o no 1 azione in avanti 0 corrisponde a non utilizzare l azione in avanti 1 ad utilizzare l azione in avanti decentralizzata 2 ad utilizzare azione in avanti centralizzata Questo dato viene memorizzato nella variabile azione_in_avanti e il valore per decidere che tipo di controllo utilizzare 0 controllo decen tralizzato 1 controllo centralizzato Questo dato vien
94. osizione_link1 Pi a es z C1poli LEA Cpdi gt M Stato gt gt lul P tensione_link1 rad piel A encoder_link1 P PID z 0 m gt Pidirezione_link1 Figura 3 60 Subsystem Calcolo controllo controllo linkl e2 controllo decentralizzato di trasferimento del motore modellizzato che insegue correttamente le traiettorie quando deve controllare la funzione che simula il motore ma soffre delle impre cisioni legate alla modellizzazione e quando viene utilizzata per il controllo del motore reale non garantisce pi le stesse prestazioni Il controllore PID manuale permette di ritarare i suoi guadagni per tentativi e quindi osservando le risposte date dal motore fino al raggiungimento delle pre stazioni volute Dopo il controllore viene aggiunta la tensione data dall azione in avanti Vref_a_link2 la tensione che si ottiene da questa somma viene memorizzata in tensione _link2 ed quella che verr inviata al motore 2 Viene posto uno switch come gi per il carrello che controlla se Stato 1 se la condizione non verificata significa che si nello stato di emergenza quindi la tensione da inviare al motore 2 zero 96 3 CONTROLLO DEL ROBOT Per i motori in corrente continua una tensione pari a zero significa spegnere il motore non mantenerlo fermo come avveniva nel motore passo I link infatti non rimangono bloccati ma sono non controllati Ci
95. ote opposta nel primo membro e prende il nome di azione mutua Cy la coppia che agisce sul membro 2 e si ripercuote opposta nel membro 1 C la coppia che agisce sul membro 1 e si ripercuote opposta nel membro 0 Gi e Go sono i baricentri e le masse dei membri vengono chiamate m1 ed m2 Si possono cos ottenere le equazioni dinamiche dei due membri gt 1 001 Ci Ca OA A ERI xk Late A 1 Ir 6 912 Co GA A R xk 118 A ANALISI DINAMICA INVERSA DEL MANIPOLATORE g c2 X G1 G2 Figura A 1 Schema delle forze e delle coppie che agiscono sui link 1 e 2 dove Iio inerzia del membro 1 rispetto al punto O I2 inerzia del membro 2 rispetto al suo baricentro Diventa cos possibile calcolare le due coppie Ci Lo C M2 x k a E A 2 Ca ASIE Mg X k Mn MR sono rispettivamente il momento che la forza Rg esercita nel punto O e il momento che la forza R esercita su Go Sapendo che mag Ra A 3 calcolato ag si pu calcolare il momento di forza MG maGzZAN w A w AOA mgG3AN dj AOA mgG3AN 02 AG3A A 4 Ora possibile calcolare le coppie in funzione solo delle posizioni velocit ed accelerazioni 119 Il prodotto con il vettore k restituisce i moduli dei prodotti vettoriali in quanto siamo ortogonali al piano Le coppie ottenute sostituendo sono Co I 402 IA m3a l2c0s 9 O1 ma l2sen 03 Ci Lo mya 2mMza lzcos
96. peso fosse essenziale soprattutto per quello del link2 che au 4 1 PROGETTO menta l inerzia da spostare I motori inoltre lavorano in presa diretta non sono previsti quindi motoriduttori La lettura della posizione per il link 1 e 2 avviene tramite due encoder incremen tali posti sui motori che fungono quindi da trasduttori di posizione Il link 0 lavora invece in catena aperta quindi non c nessuno strumento che faccia da trasduttore Per movimentarlo si ricorso ad una vite a ricircolo di sfere che trasformasse il movimento rotatorio del motore in un movimento lungo un unico asse orizzontale Si era pensato di utilizzare una guida lineare dove farlo scorrere ma successiva mente si optato per l impiego di due sbarre cilindriche Inizialmente si era pensato di realizzare i membri del robot in acrilonitrile butadiene stirene ABS ma successivamente si deciso di utilizzare l alluminio poich le sue caratteristiche di peso e resistenza si adattavano meglio infatti essendo i mo tori posizionati sui link bisognava rendere i link pi leggeri e resistenti possibili per compensare Successivamente si passato alla progettazione con Solid Works del disegno 3D in modo da determinare precisamente le dimensioni di ogni componente Infine stato costrutito e montato ogni pezzo Progettazione e realizzazione dei driver dei motori in continua Questo progetto prevede la progettazione e la realizzazione dei driver p
97. piegato precedentemente invio dei dati deve avvenire ad una t0 function 12 34 Ft iii temporizzazione dei dati da inviare Figura 3 64 Subsystem invio dati matlab frequenza di 25 Hz allora il sottosistema in cui avviene I invio abilitato da un onda quadra il cui valore varia da 0 ad 1 Assume il valore 1 una volta ogni 40 cicli di simulazione e lo mantiene per un solo ciclo considerando infatti che il clock della simulazione 0 001 secondi si ottiene esattamente la frequenza desiderata Tutte le volte che l onda quadra assume il livello alto si entra nel subsystem temporizzazione dei dati inviati Figura 3 65 I dati vengono inviati tramite il pacchetto Packet Output che analogo al Pac ket Input utilizzato per ricevere i dati appartiene alla stessa libreria Simulink Real Time Windows Target e richiede i medesimi parametri 102 3 CONTROLLO DEL ROBOT JL Enable Sa mm pos_linkO_attuale 1000 kp2m gt rad encoder_link1 gt rad encoder_link2 gt Packet Output Stato gt rad posizione_link1 gt rad posizione_link2 gt Packet Output Standard Devices UDP Protocol DO8h Figura 3 65 Subsystem invio dati matlab temporizzazione dei dati da inviare Vengono inviati 6 dati che in ordine sono e pos_link0_attuale
98. ra 3 45 in cui vengono effettuate le seguenti assegnazioni Stato 0 per entrare nello stato di emergenza calibrazione 2 per indicare che il robot gi calibrato e ad emergenza finita non bisogner ricalibrare emergenza_induttivile2 1 per indicare quale emergenza stata generata teratore_link1 ed iteratore_link2 0 perch l algoritmo d interpolazione deve essere resettato Se invece non si attiva nessuna condizione si entra nel subsystem no emergenza in cui viene assegnato il valore 0 alla variabile emergenza_induttivile2 Nella seconda porta OR come ingressi posta solo la lettura dei due finecorsa del link 0 dato che non si ha un ritorno di posizione quando uno di questi due si attiva si entra nel subsystem emergenza Figura 3 46 in cui vengono effettuate le stesse assegnazioni del caso precedente tranne cali brazione 2 che permette la ricalibrazione di tutto il robot 78 3 CONTROLLO DEL ROBOT elseif Action Port 1 P emergenza_induttivite2 0 gt i Stato IS y calibrazione 0 i iteratore_link2 0 P iteratore_link1 Figura 3 45 Subsystem emergenze controllo finecorsa induttivi finecorsa induttivi gt emergenza 3 2 4 Ricezione dati Matlab In questo subsystem vengono ricevuti e memorizzati tutti i dati ch
99. ratto alla posizione in quanto stato scelto di chiudere il primo link verso il fine corsa induttivo destro quindi in accordo con il riferimento di posizione scelto e quindi con angoli che crescono positivamente in senso antiorario la pozione deve dimi nuire La combinazione di blocchi successivi serve per fermare la calibrazione infatti la variabile finecorsa dx link2 rileva quando il link2 arriva al finecorsa induttivo destro del link 2 e passando quindi da livello logico basso a livello logico alto abilita il subsystem attivazione finecorsa Figura 3 7 Al suo interno viene imposto calibrazione 1 per passare nella fase di chiusura del link 1 Come precedentemente accennato viene assegnato a posizione_link2 il valore no to in cui stato posizionato il finecorsa induttivo tramite la costante delta_fc_cal_link2 3 2 DESCRIZIONE DEL PROGRAMMA 35 a Enable 1 gt calibrazione rad delta_fc_cal_link2 P posizione_link2 rad gt P offset link2 encoder_link2 ZEN rad encoder_link2 Pi encoder link2 Las offset_link2 Figura 3 7 Subsystem Calcolo riferimento gt calibrazione chiusura gomito gt attivazione finecorsa 155 gradi converiti in radianti caricata dal file Matlab A questa posizione viene sottratto la lettur
100. re uscita dal blocchetto comparatore uguale a 0 e allora grazie all utilizzo del blocchetto if si entra nel subsystem no emergenza in cui viene posta emergenza_encoder 0 Questa variabile serve ad indicare nello sub system stato di emergenza se la lettura encoder genera emergenza o no Quando invece il contatore supera il valore massimo uscita del comparatore diventa 1 e si entra nel subsystem emergenza encoder link o emergenza encoder link1 rispettivamente per i due link Figura 3 39 Questo tipo di emergenza porta alla ricalibrazione del robot indipendentemente da quale encoder manda in emergenza Allora avvengono le seguenti assegnazioni e Stato 0 per entrare nello stato di emergenza e emergenza simulink 1 per far attivare l emergenza e calibrazione 2 che il valore iniziale della variabile e indica che il robot deve essere calibrato e iteratore_link2 0 per poter reinterpolare correttamente la posizione del link 2 3 2 DESCRIZIONE DEL PROGRAMMA 71 elseif Action Port 0 P Stato 2 P calibrazione AA 1 Pjemergenza_encoder 0 P iteratore_link2 0 iteratore_link1 Figura 3 39 Subsystem emergenze controllo funzionamento encoder gt emergenza e iteratore_link1 0 p
101. re 1 che corrisponde ad una di rezione positiva in quanto si deciso di farlo chiudere sul finecorsa induttivo destro Il motore passo ha infatti bisogno oltre alla frequenza di passi da fare anche la direzione in cui farli Quando il finecorsa si attiva la variabili finecorsa_dx_link0 abilita il subsystem 38 3 CONTROLLO DEL ROBOT enabled attivazione finecorsa Figura 3 10 non avendo il link 0 un encoder o un potenziometro e lavorando quindi in catena Tl Enable fine_guida pos_link0_attuale direzione_linkO calibrazione 1 AA Figura 3 10 Subsystem Calcolo riferimento calibrazione chiusura carrello gt attivazione finecorsa aperta non esiste il problema che l errore di posizione in ingresso al controllo non pu ricevere un gradino troppo ampio Allora sufficiente sovrascrivere la posizione del link imponendo quella del fi necorsa che salvata nella costante fine guida da ora in poi la posizione del carrello coincider con quella assoluta La direzione viene cambiata in 0 perch ora il carrello arrivato alla posizione massima concessa a destra e dovr tornare indietro A calibrazione viene sovrascritto 1 si pu passare alla fase di allineamento Tutti e tre i link sono sui finecorsa quindi in una posizione nota possibile effet tuare una traiettoria pianificata per portarli nella posizione d
102. rin cipali integrati della scheda sono microcontrollore dsPic30F4012 LMD18200 in tegrato principale dello stadio in potenza Il microcontrollore stato programmato in un linguaggio che si basa sul linguag gio C ma pensato apposta per i pic Nel programma stato pensato l algoritmo per non perdere passi da fare e per essere sincronizzato con i segnali ricevuti dalla scheda di acquisizione dati La scheda d interfaccia comprende i connettori per le connessioni con i due dri ver per la comunicazione con la scheda di acquisizione dati e per gli encoder Si pu quindi considerare il centro dello scambio dei dati Il quadro elettrico contiene tutta la parte elettronica ed inoltre il circuito di emer genza Nella parte esterna del quadro sono previsti gli ingressi per i connettori piatti della scheda di acquisizione dati e degli encoder e una pulsantiera in cui sono previsti i pulsanti di arresto e messa in marcia Pianificazione delle traiettorie da inseguire e progettazione di una GUI per l utente Questo progetto di tesi consiste nella pianificazione delle traiettorie per tutti i movimenti del robot e progettare un interfaccia Matlab GUI per l utente Il programma realizzato a tale fine viene fatto girare in calcolatore apposito che 6 1 PROGETTO chiameremo calcolatore A L interfaccia cos strutturata Nella parte destra rappresentato lo spazio operativo in cui port agire il robot ed
103. rollo centralizzato Figura 3 61 si vede chiaramente qual il tipo di schema utilizzato considerando posizioni velocit ed accelerazioni reali e desiderate dei dei link 1 e 2 e l accelerazione del link 0 tramite i calcoli di dinamica inversa si ottengono le coppie che devono erogare i due motori e convertite in tensioni vengono inviate ai driver Come spiegato nel capitolo precedente non vengono considerate la posizione e la velocit del link 0 perch non influiscono nel modello dinamico e neanche le accelerazioni reali dei link 1 e 2 perch a partire da quelle desiderate e dagli errori 3 2 DESCRIZIONE DEL PROGRAMMA 97 JL Enable rad posizione_link2 __ e __ gt rad ad posizione_linkt P rad cs v_link2_rif P Nm M H dp Cm1 gt Rat kti P tensione_link1 m rad cs v_link1_rif rad cs 2 a_link2_rif A rad cs 2 Hl adpp m a_link1_rif rad encoder_linwk2 _ gt rad j Pa encoder_link1 rad ms v_link2 Nm M Il Pap Cm2 Ra2 kt2 P tensione_link2 __ gt A rad ms v_link1 passi ms 2 a_linko_rt P apo dinamica inversa Figura 3 61 Subsystem Calcolo controllo controllo linkl e2 controllo centralizzato gt abilitazione controllo centralizzato d
104. rve a compensare la forza contro elettro motrice ed infatti proporzionale alle velocit ed il secondo fornisce la tensione necessaria a far erogare la coppia per il movimento del motore Per il calcolo dell azione in avanti centralizzata si aggiunge anche una compo nente dovuta alla coppia resistente generata appunto dal movimento degli altri link L azione in avanti diventa quindi 20 2 ARCHITETTURA PER IL CONTROLLO Figura 2 7 Schema di un controllo in retroazione con azione in avanti e driver in tensione KvO Ra leq Or I Ra Cr Vref a Ci Kt 2 3 Si fa presente che non sono stati considerati rapporti di riduzione e rendimenti perch i motori lavorano in presa diretta e quindi il valore di questi due parametri 1 I termini dipendenti da velocit ed accelerazione servono a correggere errore nel transitorio il terzo l unico invece che va ad influire nella correzione sul punto finale Per i driver in corrente azione in avanti si ottiene con lo stesso metodo Figura 2 8 Allora l azione in avanti decentralizzata diventa Ki Ia Vre a a 2 4 e azione in avanti centralizzata Ki I C Vref a if 1 7 2 5 Kt 2 3 TEORIA DEL CONTROLLO 21 cae eo Seba IE lan di KS ZA A a
105. si in cui si pu trovare e questo lo rende molto articolato ma l effetto finale che vuole ottenere quello di garantire il corretto movimento dei motori In questo paragrafo vengono descritte la teoria le tecniche e le idee utilizzate per l inseguimento dei riferimenti di posizione Il link 0 non avendo n un encoder n un potenziometro n un qualsiasi stru mento per avere un feedback sulla sua posizione lavora in catena aperta Quindi vengono mandati i riferimenti al motore passo che eroga delle coppie ma non si pu correggere l andamento del link e quindi aver certezza che sia stato fatto proprio il movimento desiderato L unica azione di controllo prevista per questo link quella di mandare un nu mero di passi da fargli fare inferiore al massimo numero di passi che il motore pu effettivamente fare e verificare sperimentalmente la frequenza di passi che pu eseguire senza perderne un numero elevato e cercare di non superala Per gli altri due motori che lavorano in catena chiusa sono stati implementati due tipi di controllo il controllo decentralizzato e il controllo centralizzato Il controllo centralizzato pi complicato da realizzare necessita di una buona conoscenza del modello dinamico del robot ed pi efficiente in termini di pre stazioni Il controllo decentralizzato pi semplice da realizzare in quanto non richiede un approfondita conoscenza del modello ma anche meno efficace in quanto consi d
106. significa che possono essere spo stati da forze esterne Questa caratteristica svantaggiosa viene per sfruttata per quando emergenza viene data dall attivazione di un finecorsa infatti cos possibile spostare ma nualmente il link 2 e anche I 1 La tensione inviata per divisa in modulo e segno questo perch il micro control lore della scheda elettronica del driver stato programmato per ricevere entrambi i dati Inoltre viene posto un saturatore tra 0 e 5 Volt che sono i limiti di tensione accettabili per il micro controllore Sperimentalmente si comunque visto che l attuazione necessaria a compiere i corretti movimenti non mai superiore ai 5 Volt Lo schema si presenta analogo per il controllo del motore 1 viene solamente cambiato il nome delle variabili il modulo della tensione salvato nella variabile tensione link e il rispettivo segno in direzione link1 Se il tipo di controllo scelto quello centralizzato si entra nel subsystem con trollo centralizzato in cui si trova un altro sottosistema abilitato dalla condizione Stato 2 Il controllo quindi pu avvenire in modo centralizzato solo durante il funzionamento normale questo perch nella fase di calibrazione non si conosco no i riferimenti di velocit ed accelerazione in quanto le traiettorie non vengono pianificate da Matlab Quando si in Stato 4 e controllo 1 si accede al subsystem abilitazione cont
107. sponde ad un tempo totale di 0 3 sec con ampi margini di sicurezza per eventuali modifiche temporali 3 2 DESCRIZIONE DEL PROGRAMMA 51 if Action Port ee gt passi_linkO n passi 0 gt direzione_linkO Figura 3 21 Subsystem Calcolo riferimento calibrazione posizione di home allineamento link 0 I movimenti dei link durante tutta la calibrazione quindi sia in fase di alli neamento ai finecorsa sia durante la posizione di home vengono rappresentati nei seguenti grafici Figura 3 22 3 23 3 24 52 3 CONTROLLO DEL ROBOT Figura 3 22 Grafico della posizione del link 2 durante la calibrazione Figura 3 23 Grafico della posizione del link 1 durante la calibrazione Figura 3 24 Grafico della posizione del link 0 durante la calibrazione 3 2 DESCRIZIONE DEL PROGRAMMA 53 Stato 3 calibrato Il robot calibrato e si entra allora in questo stato che uno stato di attesa infatti non presente alcun blocchetto al suo interno Attende che arrivi da Matlab il passaggio allo stato 4 Quando infatti al calcolatore A viene inviato Stato 3 nell interfaccia disa bilitata compare la possibilit per utente di premere il pulsante start la cui pressione abilita tutta l interfaccia Matlab legge le posizioni dei tre link che sono inviate da Simulink che corrispondono a quel
108. stem calcolo posizione Figura 3 15 Viene calcolato il riferimento di posizione dei due link che verr utilizzato come riferimento per il controllo Il calcolo come si vede avviene per tre situazioni distinte la fase di accelerazione la fase a velocit costante e la fase di decelerazione Si entra in una di queste fasi a seconda del valore della variabile t se t minore o uguale del tempo di accelerazione ta siamo nella prima fase se t maggiore del tempo di accelerazione ma minore o uguale del tempo totale per il movimento pianificato meno il tempo di decelerazione T calibrazione Siamo nella fase a velocit 44 3 CONTROLLO DEL ROBOT 10 function if ut lt 0 6 t pu elself elself Action Action Action fase di decelerazione fase velocita costante fase di accelerazione Figura 3 15 Subsystem Calcolo riferimento calibrazione posizione di home allineamento link2el calcolo posizione link 2 e 1 gt calcolo posizione costante se t maggiore del tempo totale per il movimento pianificato meno il tempo di decelerazione siamo nella fase di decelerazione Il calcolo della posizione in fase di accelerazione Figura 3 16 viene dato dall equazione 0 t Bin 0 5 amaz t 3 2 in cui fin corrisponde esattamente a quella del finecorsa induttivo che il nostro punto di par
109. ta scheda di acquisizione installata nel calcolatore B il mezzo che permet te la comunicazione tra il modello reale del motore e il programma virtuale in Simulink Figura 2 2 L installazione risultata essere molto semplice stata infatti solamente instal lata fisicamente la scheda all interno del computer nell apposita slot e fissata con una vite Inoltre per il suo funzionamento non stato necessario installare alcun driver aggiuntivo una volta inserita la scheda era perfettamente leggibile e utilizzabile da Simulink Alla scheda sono stati collegati due connettori piatti da 50 pin uno per gli ingressi 2 2 LAYOUT GENERALE 13 Figura 2 2 Foto della scheda di acquisizione dati Sensoray626 e le uscite digitali uno per gli ingressi e le uscite analogiche ed un connettore piatto da 26 pin per la lettura degli encoder secondo le caratteristiche della sche da questo connettore pu leggere fino ad un massimo di tre encoder La scheda utillizzata la revisione D e ricordando che la versione Matlab uti lizzata la R2010a si potuto constatare un ottima compatibilit Anche il funzionamento in tempo reale del programma utilizzando Real Time Windows Target non ha causato nessun problema con utilizzo della scheda Le principali caratteristiche della scheda tratte dal manuale di istruzioni ufficiale sono I e 48 canali I O digitali e 20 dei canali I O digitali hanno riconoscimento di margine
110. ta_fin gt I eI eli el RI RI Reali Ps di red conteggio gt P theta_in posizione_link2 _ 1 Figura 3 28 Subsystem Calcolo riferimento calibrazione funzionamento normale gt theta di riferimento del link2 calcolo di theta finale e iniziale ognuno di essi memorizza il valore della variabile per un ciclo di simulazione utiliz zandone 10 si memorizza la posizione proprio per 0 01 sec il tempo cio necessario all aggiornamento del riferimento di posizione teoricamente Cos da conservare il valore della posizione precedente Vengono cos memorizzate theta_fin e theta_in Per sorge il problema per il primo centesimo di secondo da quando si entra in questo stato quindi per i primi 10 cicli non arriva nessun valore a scrivere theta_in a causa dei blocchi memory e allora si farebbe interpolazione con il valore con cui la variabile inizializzata che ovviamente non pu andare bene Si introduce allora la variabile conteggio2 che conta semplicemente il numero di volte che si entra in questo subsystem e uno switch con la condizione che quando conteggio2 maggiore di dieci allora theta_in assume il valore che arriva dai blocchi memory prima assume direttamente il valore di posizione_link2 e cio 58 3 CONTROLLO DEL ROBOT
111. tenza e ama quella calcolata Questo valido sia per il link 1 che per il link 2 e in questo sottosistema viene il calcolato il riferimento per entrambi Utilizzado le variabili corrette le equazioni diventano 3 2 DESCRIZIONE DEL PROGRAMMA 45 Action Action Port FASE DI ACCELERAZIONE rad delta_fc_cal_link2 us gt posizione_link2 famax_link2_cal t x t rad delta_fc_cal_link1 90 kg2r 6 posizione_link1 amax_link1_cal t x t Figura 3 16 Subsystem Calcolo riferimento calibrazione posizione di home gt allineamento link2el calcolo posizione link 2 e 1 calcolo posizione fase di accelerazione e posizione_ link2 delta_fc_cal_link2 0 5 amax link2_cal t e posizione_ link1 delta_fc_cal_link1 90 kg2r 0 5 amax_link1_cal t Ricordiamo che queste equazioni valgono finch 0 lt t lt ta Quando il tempo diventa maggiore di ta si entra nel sottosistema del calcolo della posizione in fase a velocit costante Figura 3 17 viene dato dall equazione 0 t Oin 0 5 amaz ta Umax t ta 3 3 46 3 CONTROLLO DEL ROBOT Action FASE A VELOCITA COSTANTE Action Port rad delta_fc_cal_link2 an sar P posizione_link2 amax_link2_cal
112. ttivato il subsystem enabled po sizione di home raggiunta Figura 3 20 3 2 DESCRIZIONE DEL PROGRAMMA 49 giunti a questo punto il robot completamente calibrato possiamo dunque pas Action Action Port 2 calibrazione 3 Stato Figura 3 20 Subsystem Calcolo riferimento calibrazione posizione di home allineamento link2el calcolo posizione link 2 e 1 calcolo posizione gt controllo fine pianificazione posizione di home raggiunta sare allo stato successivo della macchina a stati Viene azzerato il valore della variabile t nel caso dopo qualche eventuale emer genza si debba ricalibrare il robot La variabile calibrazione viene portata al valore 2 che indica che il robot ca librato Quindi le assegnazioni fatte sono e Stato 3 e a Pe 0 50 3 CONTROLLO DEL ROBOT e calibrazione 2 Ricordando che il subsystem posizione di home era diviso in due parti viene ora descritta la seconda parte quella che si occupa di portare il link 0 nella posi zione di home Si faccia riferimento alla figura 3 11 Viene posta subito la condizione per controllare se il link 0 gi in posizione di home o no alla posizione attuale del link 0 si sottrae il numero di passi a cui corrisponde met guida rappresentato dalla costante meta_guida 1500 che dove vo
113. u n posizione nel vettore I calcoli implementati per i calcoli delle coppie resistenti sono dati dalle equazioni Ca DA mga 12c05 Qz O moaylosen Q2 O1 malasen 0O O Cri DA mza l2 cos02 2 2mza l sen 02 019 ma l2sen 03 g maz l2sen 0 92 asin 01 mil sin 01 3 5 Memorizzate nelle omonime variabile Nel secondo function call subsystem azione in avanti Figura 3 52 vengono distinti il calcolo dell azione in avanti per il motore 1 e per il motore t0 function CALCOLO AZIONE IN AVANTI om if u1 0 driver_tensione_corrente2 _ ut a __ elseif u1 1 elseif if link 2 driver in tensione link 2 driver in corrente if u1 0 driver_tensione_correntet _ut er src elseif u1 1 4 elseif it link 1 driver in tensione link 1 driver in corrente Figura 3 52 Subsystem Calcolo azione in avanti gt azione in avanti 2 e dalle variabili driver tensione corrente2 e driver tensione correntel inviate da Matlab un blocco if permette di entrare nel sottosistema per il calcolo dell azione in avanti per il driver in corrente o in tensione 3 2 DESCRIZIONE DEL PROGRAMMA 87 Prendiamo in considerazione il calcolo per il motore 2 Se driver_tensione_corrente2 1 si entra nel subsystem link 2 dri
114. una ridondanza dei possibili movimenti per raggiungere uno stesso punto Sono stati allora pensati degli algo ritmi di ottimizzazione del movimento per decidere come effettuare il movimento Controllo di manipolatore a tre gradi di libert Questo progetto quello che verr descritto in questa tesi e quindi lo vedremo nel dettaglio nei prossimi capitoli Consiste comunque nella progettazione di un programma utilizzando Simulink 1 2 DESCRIZIONE DEI CINQUE PROGETTI T per il controllo del manipolatore Quindi prevede la lettura di tutti i dati ricevuti dal sistema fisico dalla posizione dei link alle possibili emergenze nell interpretazione e gestione di tutti i casi in cui si pu trovare il robot nell invio di tutti i segnali necessari per il movimento sia comandati dall utente tramite l interfaccia sia quelli previsti all interno del programma stesso e nella progettazione del controllo per l inseguimento dei motori del movimento richiesto Il programma viene fatto girare in un calcolatore apposito che viene chiamato calcolatore B Capitolo 2 Architettura per il controllo In questo capitolo verr descritto come stato deciso di realizzare il controllo del robot per capire cosa stato utilizzato e comprendere il funzionamento generale Il controllo stato programmato con il software Simulink che bene si adat ta alla realizzazione degli schemi a blocchi Questo programma viene fatto girare
115. ura e la memorizzazione nelle variabili di tutti gli ingressi della Sensoray che sono gi stati elencati nel capitolo precedente Figura 3 32 3 2 DESCRIZIONE DEL PROGRAMMA 61 10 function CALCOLO NUMERO DI PASSI E DIREZIONE DEL LINK 0 a if u1 gt 0 posizione linko a bui else mi else pos_linkO_attuale gt lu gt passilinko Figura 3 31 Subsystem Calcolo riferimento calibrazione funzionamento normale posizione link 0 E stato scelto di leggere tutti gli ingressi in un unico sottosistema perch questo permette una migliore leggibilit del programma ed inoltre cos tutti gli ingressi vengono letti esattamente nello stesso momento permettendo una maggiore pre cisione Per poter leggere i valori dalla Sensoray 626 sono stati utilizzati blocchetti A nalog Input Digital Input ed Encoder Input in cui impostando un specifico canale di lettura leggono i segnali in ingresso alla scheda di acquisizione Le variabili utilizzate per memorizzare i vari ingressi sono e encoder_link2 ed encoder_link1 per la posizione rispettivamente del link 2 e 1 letta dagli encoder quindi la posizione reale dei membri I canali di ingresso encoder utilizzati sono rispettivamente il 5 e il 4 Ingressi encoder x2 62 CONTROLLO DEL ROBOT io function finecorsa
116. ver in tensio ne Figura 3 53 al suo interno prevista sia la possibilit di ottenere l azione in avanti centraliz elseif Q CALCOLO AZIONE IN AVANTI LINK 2 PER DRIVER IN TENSIONE Action Port k2 gt vlink2 rift __ Im2 12A x Ra2 kt2 es m adink2 rif ___ _ _ _ _ _ _ av P K 2 VI M lazione_in_avanti PH Add1 P Vref_a_link2 ca m 0 n Ta lazione_in_avanti P 0o md Figura 3 53 Subsystem Calcolo azione in avanti gt link 2 driver in tensione zata decentralizzata o di non usare l azione in avanti Infatti nel primo switch quello pi a sinistra posta la condizione che se azione_in_avanti 2 allora viene letto il valore della coppia resistente altrimenti questo termine posto a zero Questo equivale a scegliere tra centralizzata e decentralizzata in fatti come stato spiegato la prima tiene conto anche della coppia resistente e la seconda no Nel secondo switch posta la condizione che se azione_in_avanti 0 allora il valore di Vref_a_link2 non quello calcolato ma zero quindi il controllo in questo caso non utilizza il contributo di nessuna azione in avanti 88 3 CONTROLLO DEL ROBOT L equazione per il calcol
117. zione del motore 2 Segno positivo 1 segno negativo 0 Output channel 13 e tensione link che il modulo della tensione per la movimentazione del motore 1 Output channel 2 e direzione_link1 fornisce il segno della tensione per la movimentazione del motore 1 Segno positivo 1 segno negativo 0 Output channel 9 e tensione_linkO che il modulo della tensione per la movimentazione del motore 0 Output channel 1 e direzione_link0 fornisce la direzione in cui il carrello deve muoversi Dire zione positiva 1 direzione negativa 0 Output channel 16 e end_effector comanda il movimento verticale della matita La matita scrive sul foglio 1 la matita non scrive sul foglio 0 Output channel 7 e driver_tensione_corrente2 decide se utilizzare il driver del motore 2 con trollato in tensione 1 o controllato in corrente 0 Output channel 18 e driver_tensione_correntel decide se utilizzare il driver del motore 1 con trollato in tensione 1 o controllato in corrente 0 Output channel 10 e emergenza quando 1 fa azionare il circuito di emergenza quando 0 il robot pu funzionare Output channel 23 3 2 DESCRIZIONE DEL PROGRAMMA 101 3 2 8 Comunicazione invio dati matlab In questo subsystem avviene I invio dei dati verso il calcolatore A Figura 3 64 Come stato s

Documento PDF

Contents

Download Pdf Manuals

Related Search

Related Contents