Salve,
nel poco tempo libero, continuo ancora a divertirmi con i pic, e pensavo di provare a realizzare un piccolo sistemino PID.
Mi sono documentato su vari siti, e o anche trovato un pdf che in modo semplice spiega una funzione PID base.
www.fabbrimarco.com/droboitalia/Le%...D%20facile1.pdfQuindi, ho preso dal cassettino dei componenti un accelerometro a 3 assi, un servo RC e un pic 12F, giusto per fare i primi test.
L'accelerometro è fissato su una barretta metallica attaccata al perno del servo, che è poi comandato dal pic 12F. Così, uno spostamento del servo mi modifica l'inclinazione dell'accelerometro. Monitorando un solo asse, quello X per esempio, posso sapere se l'accelerometro è parallelo al suolo oppure no, e di conseguenza modificare la posizione del servo per riportare l'accelerometro alla posizione corretta.
Il tutto tramite una funzione PID naturalmente...
Il servo è controllato in pwm, aumentando o diminuendo la parte alta dell'onda quadra posso decidere in che posizione deve stare.
Supponiamo che, con il registro del pwm caricato con "72" il servo stia nella posizione centrale, e che questa posizione centrale sia la posizione in cui l'accelerometro è perfettamente parallelo a terra.
A questo punto, il mio procedimento sarà:
- Acquisisco il dato fornito dall'accelerometro
- calcolo l'errore
- lo inserisco nella parte proporzionale funzione PID
- in base all'uscita della funzione vario la posizione del perno del servo RC
Pero ora sto provando con la sola parte Proporzionale.
Di seguito un pò di codice in C:
CODICE
unsigned int Kp = 72;
unsigned int Max_Out = 79, Min_Out = 65;
//Acquisisco il dato dall'accelerometro
GO = 1;
while(GO);
Acc = ((ADRESL + (ADRESH << 8)));
//Calcolo dell'errore
Err = ((Acc*10) / 327);
//Proporzionale
Pf = (Kp * Err);
//Uscita
Out = ((Pf)/10);
//Massimi e minimi dell'uscita
if(Out > Max_Out)
Out = Max_Out;
if(Out < Min_Out)
Out = Min_Out;
if(Err == 0)
Kp = Out;
--> Invio l'uscita alla funzione che regola il PWM
Naturalmente, prima di inviare alla funzione che gestisce il pwm il valore, verifico che non esca dal range consentito dal servo, nel qual caso limito il valore entro il suo range.
Moltiplico e divido per 10 per aumentare la risoluzione nel lavorare i dati.
Veniamo ad un esempio pratico, ma prima qualche dato sull'accelerometro.
In stato di "zero", l'accelerometro mi fornisce 1.6V in uscita, e ha una sensibilità di 800mV/G
Quindi, quando sono su Zero, sul modulo ADC mi ritrovo con:
Risoluzione: Vcc (5) / (2^10) = 0,004882812 V
Valore in Decimale = Vin / Ris. = 1.6/0,004882812 = 327
A questo 327, che indica lo zero dell'accelerometro, dovrà corrispondermi un "72" nella funzione del PWM.
Prendiamo il caso in cui l'accelerometro sia parallelo al suolo:
- Abbiamo in ingresso: Acc = 327
- Err = ((Acc*10) / 327);
- Err = 10 ( 1 nel caso non aumentassi la risoluzione moltiplicando per 10)
// Funzione P
- Pf = (Kp * Err);
- Out = Pf/10
A questo punto, mi pare ovvio che, non avendo 10 come errore, che corrisponde ad un 1, per avere 72 in uscita e mantenere quindi il servo in questa posizione debba avere un Kp di 72, e su Out avrò, giustamente 72.
Ora, non sto a fare anche un esempio nel caso l'accelerometro non sia nella posizione giusta, comunque questa parte di codice funziona perfettamente, il PIC mi mantiene l'accelerometro parallelo al terreno.
Il problema è che è un pò instabile, e non raggiunge quasi mai uno stato di quiete. Quindi ho cercato di inserire anche una parte D e una parte I usando le seguenti formule:
- If = (If +(Ki * Err));
- Df = (Kd * (Err/Err1));
Err1 = Err;
- Out = ((Pf + If + Df)/10);
Ma a questo punto, a meno che non vada su coeficenti Ki e Kd inferiori allo 0, il servo non mi trova più la posizione giusta perchè il valore Out si distacca troppo dal valore in cui vi è lo Zero.
Quindi, arriviamo al problema
Probabilmente, ormai sono passati 3 anni dall'ultima volta che ho fatto sistemi, e non mi riesce più di modificare e adeguare le formule per la parte P, I e D per rendere stabile il sistema, o forse procedo in maniera errata nella scelta dei coefficenti...
Insomma, chiedo a voi se avete una qualche linea da seguire per procedere nel progettino, consigli su come procedere e come affrontare il problema, insomma, un vostro parere
Se poi trovate che i sistemi PID ancora non mi sono abbastanza chiari, se avete link o testi postate pure
Grazie
Edited by TheMrLucas - 3/7/2012, 20:57
C'è solo un problema con quello: se mi ricordo bene il sistema viene controllato da un PID non lineare, o peggio ancora da un controllo ad hoc. Mi pare usassero una scheda con non so che controllore, comandata da un programma Scilab.
