Controllo dei Manipolatori Industriali Esercitazioni Sperimentali 2002/03 Manipolatore IMI.

Transcript

1 Introduzione Le esercitazioni sperimentali hanno luogo nei locali del Laboratorio di Robotica (LabRob), utilizzando il manipolatore planare a due bracci prodotto dalla IMI (USA). Le esercitazioni avranno luogo in 2 coppie di ore. Ogni gruppo potrà eventualmente incrementare le ore in Laboratorio prenotandole direttamente, secondo le modalità concordate con il docente e gli esercitatori. Il manipolatore oggetto dell'esercitazione è comandato da due motori brushless NSK a presa diretta (privi di motoriduttori), controllati da un sistema digitale di controllo basato su DSP, denominato OpenDSP, costruito interamente al Politecnico do Torino. Esso è collegato mediante una linea parallela ad un PC, il quale gestisce l'interazione uomo-macchina attraverso un interfaccia grafica in ambiente Matlab. Per ulteriori spiegazioni, consultare la pagina web: L utente può realizzare algoritmi di controllo digitale e scaricarli sul DSP, che si incarica della loro esecuzione e gestisce l hardware del manipolatore, sotto la supervisione del PC. Prerequisiti necessari Conoscenza di nozioni di base del sistema operativo Windows. Capacità di lavorare in ambiente Matlab e Simulink. Capacità di scrivere semplici programmi in C. Organizzazione dei dati sul PC utilizzato Ogni squadra dispone di una cartella denominata c:\esercitazioni-imi\squadran in cui lavorare e salvare i propri files. Il simbolo N varia da 1 a 11 a seconda della squadra di appartenenza. La squadra 5, ad esempio, dovrà usare la cartella c:\esercitazioni-imi\squadra5. Prima di entrare nell'ambiente di gestione del robot è necessario ridefinire la configurazione della cartella di lavoro e del path con il comando Matlab >> squadra(n). In Figura 1 è presentata la struttura della directory di lavoro dell ipotetica Squadra0 Figura 1: la directory di lavoro Nelle sottocartelle di ogni squadra, illustrate nelle Figure 2a) 2d), si può trovare una copia di ognuno dei file Matlab o del DSP, che saranno utilizzati dalle interfacce grafiche, oppure per la simulazione e il controllo. Ogni squadra potrà quindi modificarli e utilizzarli nella propria versione. Per modificare le leggi di controllo è necessario creare una versione personalizzata del file IMI_PID_basic.c nella propria cartella, salvandola con un nome diverso e ricompilandola. 1/16

2 Figura 2a) Figura 2b) Figura 2c) Figura 2d) 2/16

3 Strumenti L ambiente di lavoro è Matlab 6.1 (o successivi) e gli strumenti che si utilizzeranno sono stati realizzati in questo ambiente. Il programma che gestisce il manipolatore si chiama IMIConsole.m e risiede nella cartella c:\esercitazioni-imi\squadran\imiconsole 4.1 dove N è il numero della squadra a cui si appartiene. Altri due programmi, IMIReference.m ed IMIExecute.m, servono rispettivamente alla pianificazione ed alla esecuzione delle traiettorie. Essi si trovano nelle directories c:\esercitazioni-imi\squadran\imireference 2.2 c:\esercitazioni-imi\squadran\imiexecute 2.4 Il comportamento del manipolatore viene simulato in ambiente Simulink dal file IMI_LuGre.mdl, contenuta nella directory c:\esercitazioni-imi\squadran\simulatore. Le sottocartelle sono replicate per ognuna delle squadre in modo che si possano modificare i file senza interferenza con le altre squadre. La versione originale si trova nella cartella relativa ad una fittizia Squadra0 (c:\esercitazioni-imi\squadra0) in modo da poter rimediare ad eventuali errori. Svolgimento Le esercitazioni sono suddivise nelle due seguenti parti: 1. Analisi del manipolatore Cinematica, pianificazione ed esecuzione di traiettorie 2. Controllo del manipolatore Parte 1 Analisi del Manipolatore Planare IMI Cinematica, Pianificazione ed Esecuzione di Traiettorie Scopo Acquisire dimestichezza dell hardware e del software di gestione del manipolatore planare; pianificare ed eseguire traiettorie cartesiane a scelta; analizzare e riportare su grafico le variabili relative a tali traiettorie; preparare i campioni del riferimento per far eseguire la stessa traiettoria al simulatore e all apparato reale. Una presentazione dell architettura di gestione è disponibile in rete all indirizzo Fase 1 tutorial.pdf. Si prende contatto con il programma di gestione del manipolatore. 1. Accensione del manipolatore, verifica dello stato dell hardware e avvio del software IMIConsole. 2. Procedura di homing o azzeramento del manipolatore. 3. Analisi delle voci del menù di gestione e prove di funzionamento. 4. Pianificazione di traiettorie mediante IMIReference: memorizzazione di traiettorie, secondo le modalità movimento punto-punto cartesiano, movimento punto-punto giunti, movimento circolare. 5. Simulazione delle traiettorie sul modello Simulink del manipolatore. 6. Raccolta dati, comprendente i riferimenti, le posizioni misurate, i comandi agli attuatori o altri segnali significativi. 3/16

4 7. Gestione dei dati raccolti mediante programmi Matlab, analisi e trattamento dei dati, nel tempo ed eventualmente in frequenza, analisi degli errori; plot delle variabili interessanti nel tempo, nello spazio giunti e nello spazio cartesiano Si utilizza IMIReference per costruire i riferimenti per eseguire varie traiettorie, in giunti e cartesiane; i riferimenti verranno prima simulati sul modello Simulink del manipolatore, quindi eseguiti dal manipolatore (Fase 2). Il manipolatore è presentato in Figura 3a); mentre in Figura 3b) è schematizzata una possibile traiettoria di riferimento. Figura 3a) Schema del manipolatore Figura 3b) Esempio di traiettoria di riferimento Notate che l asse z del riferimento utente R 0 (vedi Figura 4) ha direzione verticale (normale al pavimento) con verso entrante nel pavimento, per cui le rotazioni positive avvengono in senso orario rispetto a chi guarda dall alto il manipolatore. I sistemi di riferimento sui bracci, il riferimento assoluto e le coordinate x e y sono perciò controintuitivi. Fig. 4 4/16

5 Più in dettaglio il lavoro consiste nel: 1) Pianificare traiettorie punto-punto cartesiane e nei giunti. I campioni del riferimento saranno generati da IMIReference mediante le routine s_prof.m, imi_rett_q.m e imi_rett_p.m rispettando specifiche assegnate di velocità, accelerazione, alfa e campionamento. I riferimenti sono espressi in radianti. Attenzione : la generazione dei campioni si farà sempre con un passo di campionamento di 0.01 secondi per evitare un vettore di punti campionati eccessivamente lungo; infatti il DSP ha risorse di memoria limitate. Un algoritmo di microinterpolazione presente sul DSP provvede alla generazione dei punti intermedi necessari a riportare il campionamento a 1 ms. Guardando il codice Matlab di IMIReference (IMIReferenceCallbacks.m) si può comprendere quali passi vengono fatti per ottenere la pianificazione. Occorrerà, poi, verificare fuori linea i risultati, predisponendo delle prove simulate con Simulink prima di fornire i dati al robot (vedi successivo punto 5). 2) Rieseguire il punto 1) pianificando traiettorie circolari con centro, raggio e arco sotteso definiti nell interfaccia. La pianificazione della traiettoria sarà fatta utilizzando la funzionalità di generazione di un cerchio sul piano x-y; i file utilizzati da IMIReference sono: s_prof.m e imi_circ.m. 3) Verificare che i movimenti pianificati non abbiano configurazioni singolari e che non si raggiungano i finecorsa, che nel caso del robot reale valgono circa ± 2.15 rad (123 ) per entrambi gli angoli. 4) Salvare su un file.mat le pianificazioni contenenti tutti i parametri della pianificazione nella variabile curvilinea s, nonché i tempi di campionamento t k e i riferimenti q i (t k ) per utilizzarli come ingressi di riferimento sia per il simulatore (vedi successivo punto 5), sia per il manipolatore reale (vedi successiva Fase 2). 5) Per provare le traiettorie utilizzare i programmi di simulazione del manipolatore sviluppati in ambiente Simulink (presenti in c:\esercitazioni-imi\squadran\simulatore). L elenco e l utilizzo dei programmi di simulazione è riportato al termine di questo documento. 6) Analizzare e riportare su grafico, mediante Matlab, i dati relativi alle coordinate giunto e cartesiane (soprattutto velocità ed errori di traiettoria). Per effettuare le simulazioni è possibile procedere secondo i seguenti passi: a) eseguire da IMIReference la pianificazione delle traiettorie e salvare in un file.mat la traiettoria che si vuole simulare; b) aprire il simulatore con il comando Matlab >> IMI_LuGre c) inizializzare i parametri numerici, lanciarndo con un doppio click dal simulatore il file 1) parametri; d) lanciare con un doppio click dal simulatore il file 2) riferimenti, che carica e prepara una traiettoria da eseguire; e) eseguire la simulazione Simulink. Aprite pure i blocchi componenti, ma attenzione a come li modificate: potrebbe non funzionare più nulla! Potete fare riferimento alle note Controllo e Simulazione al termine di questo documento Fase 2 Si utilizzano i riferimenti creati nella Fase 1 per eseguire le traiettorie sul manipolatore reale. Il manipolatore è controllato da un controllore PID di default, il cui buon funzionamento è stato verificato sperimentalmente. La fase consiste nel: 1) Fornire in ingresso al DSP mediante IMIExecute il file.mat di traiettoria creato nella fase precedente. Come detto, l algoritmo di controllo utilizzato sarà quello di default, IMI_PID_basic.c, presente nella cartella di lavoro di ogni squadra. 5/16

6 2) Raccogliere i dati delle movimentazioni utilizzando le funzionalità di acquisizione offerte dal programma. 3) Paragonare i dati reali con quelli ottenuti dal programma di simulazione con grafici dei confronti. Parte 2 Controllo del Manipolatore Planare IMI Scopo Progettare, simulare e codificare in C un algoritmo di controllo digitale per il controllo del manipolatore. Si possono utilizzare ed modificare le strutture di controllo fornite in parametri.m. Occorre prestare estrema attenzione nello sperimentare l'algoritmo di controllo, per non danneggiare persone o cose. Fase 1 1. Utilizzare l architettura di controllo di coppia (torque mode) dei motori NSK, facendo riferimento al manuale del manipolatore (in inglese) e ad altro materiale a disposizione presso il LabRob. NOTA: gli azionamenti NSK possiedono altri modi di funzionamento, ma nell'esercitazione si farà esclusivamente uso del torque mode. 2. Progettare i due controllori dei bracci secondo la tecnica del controllo a giunti indipendenti, con controllori di tipo PID. Utilizzare il toolbox sisotool di Matlab. Per le specifiche e le procedure, fare rifermento la sezione Controllo e Simulazione al termine di queste note. 3. Procedere alle simulazioni, da condursi in ambiente Simulink, dopo aver scelto una traiettoria di riferimento, tra quelle memorizzate nella prima esercitazione. Si utilizzi sia il modello semplificato lineare, sia il modello fine non lineare con attrito descritto in IMI_LuGre.mdl. Si facciano simulazioni sia con i controllori continui sia con quelli discretizzati. Fase 2 1. Si traducano le fdt discrete dei due controllori così progettati in algoritmi di controllo da realizzare con un programma in C, che deve avere la struttura generale dell algoritmo fornito in IMI_PID_basic.c 2. Scaricare su OpenDSP il codice così creato e procedere all'analisi sperimentale, da correlarsi con la precedente simulazione, considerando i seguenti punti: a) utilizzare come traiettoria di riferimento la stessa usata per la simulazione; b) acquisire le grandezze: posizione angolare, errore angolare e valore dei comandi; c) paragonare in ambiente Matlab i dati simulati con i dati acquisiti dall esperimento. Materiale di supporto Manuale d'uso del software IMI (Tutorial ed help in linea) Manuale originale del manipolatore IMI in inglese (solo versione cartacea) presso LabRob Tesi sulla realizzazione del sistema di controllo (solo versione cartacea) presso LabRob Tesi varie sul manipolatore (solo versione cartacea) presso LabRob Programma in C di controllo digitale contenuto nei file IMI_PID_basic.c Software di simulazione Simulink contenuto in c:\esercitazioni-imi\squadran\simulatore 6/16

7 Norme di sicurezza Per evitare danni alle persone o alle cose, lo studente non deve avvicinarsi né lasciare avvicinare altre persone al manipolatore quando il programma IMIConsole è attivo. Per evitare danni al manipolatore, si invita lo studente a provare le traiettorie, i nuovi parametri del controllo o nuovi algoritmi in ambiente simulato, prima di applicarli al manipolatore reale. È comunque buona norma prepararsi ad attivare il tasto rosso per lo stop di emergenza ogniqualvolta si esegue una nuova operazione sul manipolatore reale. È opportuno ricordare che l acquisto del manipolatore è avvenuto grazie a contributi di origine studentesca e quindi un danneggiamento dell apparato si traduce in uno spreco del denaro degli studenti e delle loro famiglie. 7/16

8 Controllo e Simulazione Note per l esecuzione dei programmi di progetto, simulazione e realizzazione dei controllori del doppio braccio IMI a cura di Nicola SMALDONE Un buon progetto di controllo prevede le seguenti fasi: 1) La costruzione di un modello semplificato lineare tempo-invariante e l eventuale stima dei parametri numerici incogniti; in queste esercitazioni si assumono parametri nominali contenuti nel file parametri.m in c:\esercitazioni-imi\squadran\simulatore. Questo modello serve per il progetto del controllore. 2) Il progetto del controllore. 3) La validazione del controllore su un modello fine in simulazione, solitamente diverso da quello semplificato usato per il progetto. In queste esercitazioni si è utilizzato un modello non lineare con descrizione dell attrito mediante modello LuGre. 4) La realizzazione del controllo sul manipolatore reale e l acquisizione dei dati per validare il progetto del controllore. Questa procedura prende il nome di prototipazione rapida degli algoritmi di controllo. I modelli Simulink presenti nella cartella c:\esercitazioni-imi\squadran\simulatore permettono di seguire le fasi precedentemente descritte. 1. Modello semplificato Poichè si opera con un architettura a giunti indipendenti e si pongono gli azionamenti in Torque Mode, ipotizzando quindi che si comportino come dei trasformatori ideali tensione-coppia, si assume semplicemente che ogni braccio sia disaccoppiato dall altro e modellato da un doppio integratore: 1 Js dove J i è il momento d inerzia del braccio i-esimo. I valori dei parametri sono contenuti nel file parametri.m. 2. Progetto del controllore In queste esercitazioni si assume di utilizzare un architettura a giunti indipendenti e su ogni giunto una rete di compensazione PID classica, con polo di chiusura, rappresentata dalla fdt: s 1 C () s = K + K + K ( s+ p ) s PID P D I c i 2 ( K + K ) s + ( K + K p ) s+ K = ss ( + p ) = 2 P D I P c I 2 as + bs+ c ss ( + p ) c c 8/16

9 Viene fornita inizialmente una coppia di controllori di default i cui parametri sono contenuti nel file parametri.m le cui fdt sono chiamate rispettivamente C1_start e C2_start; quindi si lancia da Matlab il comando >> sisotool e si progetta il controllore in modo da raggiungere le seguenti specifiche: a) Margine di fase relativo al progetto semplificato non inferiore a 60 gradi. b) Banda passante relativa al progetto semplificato tra 50 rad/s e 70 rad/s per non eccitare le dinamiche parassite elastiche. c) Massimo errore tra riferimento e misura non superiore a 10-2 rad per il giunto 1 e non superiore a 1, rad per il giunto 2, in simulazione sul modello fine, utilizzando la traiettoria di riferimento Basic.mat fornita in c:\esercitazioni-imi\squadran\simulatore. Occorre prestare attenzione al fatto che il modello fine comprende l attrito di primo distacco, che tende a generare un errore a regime non trascurabile. d) Errore non superiore a rad per entrambi i giunti a partire dal tempo t f = 10s, in simulazione sul modello fine, utilizzando la traiettoria di riferimento Basic.mat. e) Verifica che le prestazioni ottenute sul modello fine in simulazione siano rispettate dalla risposta reale dell impianto. Occorre progettare sia controllori continui sia controllori discreti, ottenuti da sisotool con il metodo della trasformata bilineare di Tustin. È opportuno che al termine essi siano memorizzati con un nome convenzionale, ad esempio C1, C2, per i controllori continui, C1d, C2d per i controllori discretizzati. I controllori continui potranno venire utilizzati solo in fase di simulazione, mentre i controllori discretizzati dovranno essere tradotti in algoritmi per la codifica in C. 3. Validazione in simulazione Come detto sopra, la validazione dovrà avvenire verificando il buon comportamento dei controllori continui e discreti sul modello fine. Esiste la possibilità di utilizzare in simulazione anche il modello semplificato che è stato usato per progettare i controllori, ma l utilizza di questo modello in simulazione ha solo lo scopo didattico di verificare che le specifiche siano soddisfatte, cosa che viene comunque assicurata dalla teoria del controllo lineare. In ambiente Simulink occorre eseguire il file IMI_LuGre.mdl che apre la finestra illustrata in Figura 5 9/16

10 Figura 5 Per inizializzare i parametri numerici del modello occorre cliccare due volte sul blocco 1) Parametri.m e successivamente scegliere il riferimento cliccando due volte sul blocco 2) Scelta riferimenti. A questo punto potete già simulare utilizzando il modello fine, contenuto nel blocco Dinamica non lineare (vedi Figura 6), con il controllore contenuto nel blocco Controllore PID Figura 6: blocco dinamica non lineare Nella libreria Plant_models.mdl (vedi Figura 7) sono contenuti i blocchi utili per la simulazione: il modello semplificato (dinamica lineare), il modello fine (dinamica non lineare), il controllore PID discreto e il controllore PID continuo (vedi Figure 8a e 8b). 10/16

11 Figura 7: i blocchi disponibili Figura 8a: Blocco controllore PID discreto 11/16

12 4. Validazione sperimentale Figura 8b: Blocco controllore PID continuo Una volta che il controllore sia stato validato in simulazione, occorre tradurlo in un algoritmo da codificare all interno di un file C, che verrà poi compilato e scaricato sul DSP. Per fare questo è stato predisposto un file di default, al cui interno è presente una sorta di maschera in cui l utente carica i valori numerici dei vari guadagni; ad esempio, se il controllore discreto ha la seguente forma: si può scrivere ( ) ( ) u k bz + bz+ b b + bz + bz Cz () = = = e k z az a az az uk ( ) = auk ( 1) auk ( 2) + bek ( ) + bek ( 1) + bek ( 2) Per ottenere i valori di a i e b i basta esportare dall interfaccia sisotool al workspace Matlab la fdt discretizzata progettata, ad esempio C2d. È sufficiente dare il comando tf(c2d) per ottenere la rappresentazione nella forma polinomiale di cui sopra.. Utilizzando sisotool, non è possibile variare le caratteristiche del controllore operando direttamente su KP, KD, KI e p c ; qualora si volesse osservare gli effetti sul progetto di variazioni di questi guadagni, la procedura da seguire è la seguente: 1. Fissati opportuni valori ai guadagni e al polo di chiusura K P, K D, K I e p c, si costruisce la fdt C di tentativo con i comandi Matlab: s = tf('s'); C = Kp+Ki/s+Kd*s/(s+pc); 2. Si apre sisotool e si importa C; quindi si verifica che il sistema controllato sia stabile e si variano i parametri, che in sisotool sono i poli, gli zeri e il guadagno del compensatore; quando si raggiunge un risultato soddisfacente si esporta il compensatore nel workspace di Matlab, chiamandolo, ad esempio, C_new. 12/16

13 3. Si utilizza la funzione [Kp_new,Ki_new,Kd_new,pc_new] = tf2pid(c_new)per calcolarsi i nuovi parametri KP, KD, KI e p c. 4. Si modificano questi nuovi parametri e mediante la Ctf = pid2tf(kp,ki,kd,pc) si ottiene un nuovo controllore, ripetendo ciclicamente i punti 2, 3, 4 fino a raggiungere un risultato soddisfacente. Ovviamente questa procedura può non essere eseguita, se ci si limita a variare poli, zeri e guadagno della rete di compensazione. Ottenuto un compensatore ragionevole, da sisotool si utilizza la funzionalità che permette la discretizzazione e, come detto si ottengono i parametri della fdt discreta che verranno codificati nel programma in C. 13/16

14 Parametri numerici del doppio braccio planare IMI Il file Parametri.m contiene i seguenti valori dei parametri % PARAMETRI MANIPOLATORE IMI % Far eseguire prima di iniziare le simulazioni in Simulink del manipolatore IMI I1 = 0.267; % Inerzia rotore motore 1 [kg m^2] I2 = 0.334; % Momento centrale di inerzia braccio 1 [kg m^2] I3 = ; % Inerzia rotore motore 2 [kg m^2] I3c = 0.04; % Inerzia statore motore 2 [kg m^2] I4 = 0.063; % Momento centrale di inerzia braccio 2 [kg m^2] Ip = 0; % Momento di inerzia del carico [kg m^2] M1 = 73; % Massa motore 1 [kg] M2 = 9.78; % Massa braccio 1 [kg] M3 = 14; % Massa motore 2 [kg] M4 = 4.45; % Massa braccio 2 [kg] Mp = 0; % Massa carico [kg] % L1 = 0.359; % precedente Lunghezza braccio 1 [m] L1 = ; % Lunghezza braccio 1 [m] % L2 = 0.24; % precedente Lunghezza braccio 2 [m] L2 = ; % Lunghezza braccio 2 [m] L3 = 0.136; % Distanza dall' asse di rotazione del CG braccio 1 [m] L4 = 0.102; % Distanza dall' asse di rotazione del CG braccio 2 [m] % L'equazione dinamica del manipolatore vale % M(q)qpp + C(q,qp)qp + g(q) + F(qp) + T = tau % % M = matrice di massa, C matrice di Coriolis, g = coppia di gravità, F = coppia di attrito % T = carico esterno, tau = coppia di comando % q = posizione, qp = velocità, qpp = accelerazione % % M(q) = [p1 + 2*p3*cos(q2) p2 + p3*cos(q2)] % [p2 + p3*cos(q2) p2 ] % % C(q,qp)qp = [ - qp2*(2*qp1 + qp2) p3*sin(q2) ] % [ - qp1*qp2*p3*sin(q2) ] % % F(qp) = [ coppia_attr1 ] calcolata nel file attrito1 % [ coppia_attr2 ] calcolata nel file attrito2 % dove: p1 = I1 + I2 + I3c + I3 + I4 + Ip + (M3+M4+Mp)*L1^2 + M2*L3^2 + M4*L4^2 + Mp*L2^2; p2 = I3 + I4 + Ip + M4*L4^2 + Mp*L2^2; p3 = M4*L1*L4 + Mp*L1*L2; TorqueLimit1=225.4; % coppia massima motore 1 TorqueLimit2=36.2; % coppia massima motore 2 % Errore iniziale (per le c.i. del simulatore) ep1 = 0; ep2 = 0; % PARAMETRI DELL'ATTRITO % Coefficienti di steady-state: % alfa0 + alfa1*exp(-omega/omegas1) + alfa2*(1-exp(-omega/omegas2)) + alfa3*omega + alfa4*omega^2 alfa.n1 = [ e+001, e+001, e+001, e-001, e-001]; 14/16

15 alfa.p1 = [ e+001, e+001, e+001, e-001, e-001]; alfa.n2 = [ e+001, e+001, e+001, e-001, e-002]; alfa.p2 = [ e+000, e-001, e-001, e-001, e-002]; omegas.n1 = [ e-001, e-001]; omegas.p1 = [ e-001, e-001]; omegas.n2 = [ e-001, e-001]; omegas.p2 = [ e-001, e-001]; % Coefficienti dinamici: sigma0.p1 = 26000; sigma0.n1 = 55500; sigma0.n2 = 12600; sigma0.p2 = 12600; sigma1.p1 = 700; sigma1.n1 = 1000; sigma1.p2 = 70; sigma1.n2 = 70; % Coefficienti dei polinomi approssimanti l'attrito (solo g(v), no viscoso) CoefficientiLin.p1 = [ e-004, e-002, e- 001, e+000, e+001, e+001, e+002, e+002, e+002, e+002, e+002, e+001, e+001, e+000]; CoefficientiLin.n1 = [ e-004, e-003, e- 001, e+000, e+000, e+001, e+001, e+002, e+002, e+002, e+002, e+001, e+001, e+000]; CoefficientiLin.p2 = [ e-005, e-003, e- 002, e-001, e+000, e+001, e+001, e+001, e+001, e+001, e+001, e+001, e+000, e+000]; CoefficientiLin.n2 = [ e-005, e-003, e- 002, e-001, e+000, e+000, e+001, e+001, e+001, e+001, e+001, e+001, e+000, e+000]; % CONTROLLORE PID classico - Valori di default Kp1_start = 600; Ki1_start = 150; Kd1_start = 8000; pc1_start = 1000; Kp2_start = 30; Ki2_start = 500; Kd2_start = 2300; pc2_start = 1000; % LTI models s = tf('s'); 15/16

16 J1 = p1+2*p3; J2 = p2; G1 = 1/(J1*s^2); G2 = 1/(J2*s^2); C1_start = Kp1_start + Ki1_start/s + Kd1_start*s/(s+pc1_start); C2_start = Kp2_start + Ki2_start/s + Kd2_start*s/(s+pc2_start); % Formula inversa per ricavare i guadagni del PID % C_start= [(Kp+Kd)*s^2+(Ki+Kp*pc)*s+Ki*pc] / [s^2+s*pc] = [a*s^2+b*s+c] / [s^2+s*pc] % 1) Ki=c/pc, 2) Kp=(b-Ki)/pc, 3) Kd=a-Kp. Ts = 1e-3; 16/16