Pages: 1 [2]   Go Down
Author Topic: AutoBalance - PID Controller  (Read 10562 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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 smiley

--

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! smiley

Main Program:
Code:
// 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:
void controllomotori ()                // Algoritmo per il controllo dei due motori DC
{
  analogWrite( motore, 127 - PID );    // Controlla il motore1 in base al PID
}
GetError:
Code:
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
}
GetPID:
Code:
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 smiley


--

Sorry if i don't post all now, I'll post all when i'll finish the exam!
« Last Edit: April 13, 2013, 05:53:33 pm by Mosc » Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 1
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

hi Mosc,

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

Pages: 1 [2]   Go Up
Jump to: