Go Down

Topic: AutoBalance - PID Controller (Read 17519 times) previous topic - next topic


Apr 14, 2013, 12:38 am Last Edit: Apr 14, 2013, 12:53 am by Mosc Reason: 1

Hi Mosc,

> Got it, I must set the PID so that it exceeds these values!

Good to hear that you solve the problem!
What did you change in the code? May I see your updated code? Thanks.

Yes, of course you can!

Unfortunately I have not had much time in this period..
However I have changed a lot the robot, now I'm writing the relationship for the exam.

There there are, the datasheets, the DI, the electrical schematics, the part lists, the 3D models (with solidworks, and 2D with autcad for the mechanic parts), the product pyramid, the relationship, the software, the technical specification, the wiring list and the description of the wires (only for completeness xD).
For now all in Italian... I will post all when i take the exam :)


However, summarizing, the project has undergone considerable changes.

To adapt the output of the sensors to the ADC of arduino, and to make more dynamic the system, allowing it to move the zero point, now the difference between the two distances is performed by a differential amplifier, with Vshift adjusted with a trimmer.
Arduino gets directly the error signal.

Moreover, now the motors are supplied by 8V, in order to have more power.
And They are controlled by a single PWM.

Here is the software, it works well! :)

Main Program:
Code: [Select]
// Software AutoBalance
// Il programma legge l'errore
// Attraverso un controllo PID muove le ruote in modo da correggere tale errore.

// Definizione dei pin di INPUT
#define input A0                           // Definisce il pin a cui รจ collegato l'amplificatore differenziale

// Definizione dei pin utilizzati dai motori
#define motore 3                           // Definisce il pin PWM a cui sono collegati i motori

// Variabili per la gestione dell'Errore
float Errore = analogRead( input );        // Inclinazione dell'AutoBalance
float previousErrore = 0;                  // Inclinazione dell'AutoBalance al ciclo precedente

// Variabili temporali, per il calcolo del PID
float time = 0;                            // Tempo auttuale, dall'accensione del dispositivo
float previousTime = 0;                    // Tempo del precedente ciclo
float interval = 0;                        // Tempo impiegato per l'esecuzione di un ciclo di programma

// Variabili PID
float P = 0;                               // Variabile PROPORZIONALE  del PID
float I = 0;                               // Variabile INTEGRATIVA del PID
float D = 0;                               // Variabile DERIVATIVA del PID
float PID = 0;                             // Parametro PID

// Guadagni dei coefficenti PID
float kP = 0.6;                            // Coefficiente PROPORZIONALE del PID
float kI = 7;                              // Coefficiente INTEGRATIVA del PID
float kD = 0.02;                           // Coefficiente DERIVATIVA del PID

void setup()
//Serial.begin( 9600 );                    // Avvia la comunicazione seriale
pinMode( motore, OUTPUT );                // Imposta il pin del motore1 come uscita
void loop()                                  
 Serial.print( "  Errore =  " );          // Stampa dei dati
 Serial.print( Errore );
 Serial.print( "  Intervallo =  " );    
 Serial.print( interval );
 Serial.print( "  P =  " );    
 Serial.print( P );
 Serial.print( "  I =  " );    
 Serial.print( I );
 Serial.print( "  D =  " );    
 Serial.print( D );
 Serial.print( "  PID =  " );    
 Serial.print( PID );
 Serial.print( "  PWM =  " );
 Serial.println( 127 + PID );

 time = millis();                         // Salva il tempo corrente
 interval = (time - previousTime)/1000;   // Calcola il tempo impiegato per eseguire un ciclo in secondi
 getErrore();                             // Richiama la funzione getErrore() per ottenere l'errore
 getPID();                                // Richiama la funzione detPID() per calcolare i parametri del controllo PID
 controllomotori();                       // Richiama la funzione controllomotori() per il contorllo degli attuatori

 previousTime = time;                     // Memorizza il tempo attuale
 previousErrore = Errore;                 // Memorizza posizione attuale

Motor control:
Code: [Select]

void controllomotori ()                // Algoritmo per il controllo dei due motori DC
 analogWrite( motore, 127 - PID );    // Controlla il motore1 in base al PID

Code: [Select]

float getErrore()                      // Rilevazione della distanza, in ingresso il sensore da cui misurare
 int n = 50;                          // Numero di volte che si deve effettuare la misura, per il calcolo della media
 int array[n];                        // Array di appoggio per la rilevazione della distanza
 float somma = 0;                     // Variabile di appoggio per il calcolo della media
 int C;                               // Contatore di appoggio

 for ( C = 0; C < n; C++ )
   array[C] = analogRead( input );    // Salva n valori letti in un array di dimensione n
   somma = somma + array[C];          // Effettua la somma dei valori contenuti nell'array
 Errore =  somma / n ;                // Effettua la media, e salva il valore ottenuto nella variabile Errore

Code: [Select]

void getPID()                                       // Algoritmo per il calcolo del PID
 Errore = Errore - (511.5);                        // Elimina l'offeset
 P = Errore * kP;                                  // Calcola la variabile proporzionale
 I = I + (P * interval * kI) ;                     // Calcola la variabile integrativa
 D = kD * ( Errore - previousErrore ) / interval;  // Calcola la variabile derivativa
 PID = P + I + D;                                  // Calcola il PID
 if( PID > 127 ) PID = 127;                        // Se il valore supera 127, il valore e' impostato a 127  
 if( PID < -127 ) PID = -127;                      // Se il valore e' minore di -127, il valore e' impostato a -127  

I'm sorry for the comments in Italian, I didn't have time to translate them....

The main problems were:

The too high time used for the serial communication.
Without the comunication, the too low time that the loop take. You have to put a delay. (I did it by measuring the analog imput 50 times)
The low power in the motors
The dinamyc output of the sensors weren't 0-5V, in the distances where they work.
The arm of the structure has been calculated in such a way as to have an adequate sensitivity

And some others things that now i don't remember xD

If you need something you have only to ask :)


Sorry if i don't post all now, I'll post all when i'll finish the exam!


hi Mosc,

             how about the motor driver circuit diagram ? can u post it over here..... thanks

Go Up