Problemi col PID

Buona sera a tutti,

sto avendo qualche problema col mio robot bilanciante (vedi allegati per foto e codice), quindi voglio usare due PID a cascata per una forte stabilità anche se spinto, come si vede in molti video.

I progetti e tutorial di Kas http://forum.arduino.cc/index.php?topic=8871.0 e Lauszus mi ha aiutato e ispirato molto, essendo il mio robot molto semplice a quest’ ultimo.

So che la corrente è proporzionale alla coppia e la velocità (RPM) è proporzionale alla tensione. So anche che ho bisogno di 3 cicli chiusi su posizione, velocità e coppia per un controllo forte. Ho letto che gli anelli diventano due perché posso presumere la coppia costante. Supponendo che cosa?
Ho pensato ad un primo anello PID cui uscita è l’angolo di inclinazione desiderato del robot e questo viene immesso in un secondo ciclo PI utilizzato per l’equilibrio **PWM = PI (theta_target, theta)**e questo viene inviato ai motori.

edit: Voglio essere chiaro … In assenza di carico e con solo Kp, se tengo il robot nelle mie mani, le ruote si muovono anche con i comandi PWM lievissimi: se 800 è Max e 8 è minimo, le ruote si muovono a 16. Se 0 è la stabilità, a 0,4, senza carico, le ruote si stanno già muovendo.
Con carico, cioè appoggiato sul tavolo, il robot sta cadendo prima di riuscire a fare raggiungere alle ruote la velocità necessaria a salvarsi dalla caduta. Quindi, se il sensore IMU rileva che l’angolo di inclinazione è di 20 o -20, mentre le ruote stanno raggiungendo la velocità necessaria, il robot è già caduto.
Se cambio il valore Ki 0-1, il robot va tutta velocità anche quando l’errore angolo è quasi 0.
Aumentare Kp fino a che si verifica oscillazone, non sembra essere il problema, perché robot è lento a mettere in pratica i comandi. Ho subito pensato a quante volte un comando arriva: la frequenza è un comando ogni 10 millisecondi e PWM impostato a 10kHz manipolando Timer5 sulla mia Mega.

bal.ino (13.7 KB)