AutoBalance - PID Controller

Hi all! :smiley:
I'm building an AutoBalance Robot, one of those robots that are able to maintain the balance with two wheels.

The goal of this project, for now, is only to maintain the balance on a plan.
Without climbs, descents, external forces and without the control of the movement.

The mechanical part is done.
Unfortunately I do not have a photo on hand, so I put a render to give you an idea of how it's made:

The "robot" is composed of a Arduino ONE, two analog sensors SHARP (GP2Y0A21YK0F), two gearmotors (SBGM9) and a driver motor made ??by me (if you want i will put the circuit diagram).
The first floor is empty for now, there will be the batteries. (For now I'm doing the tests with flying leads)

The problem is... the robot does not maintains the balance! :smiley:

I copied the part of the PID controller from here: http://schianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf (at the end of the document there is the skecth), the rest is made by me...

I don't know if the problem is in the sketch, or that I'm not able of handling a PID or that the two gear motors are not suitable (with a very low value of PWM they don't move...), and i must change them with two servomotors... (I hope not...)

Here there is the Sketch:

Main Program:

// Define sensors pins
#define sensore1 A0                           // Define pin sensor 1
#define sensore2 A1                           // Define pin sensor 2

// Define motors pins
#define motore1 3                             // Define the PWM pin of motor 1
#define motore2 5                             // Define the PWM pin of motor 2

// Variables for distances front and rear
int distanzas1 = 0;                             // Value read from sensor 1
int distanzas2 = 0;                             // Value read from sensor 2

// Counters support
int C;                                             // Counter support
int J;                                             // Counter support

// Variables for the filtering of the measures
int arrays1[10];                             
int arrays2[10];               
int n = 20;                                     // number of measurements
int somma = 0;                              // Variable for the calculation of the sum
int appoggio;                                 // Variable support for sorting the array

// Variables to calculate the orientation
int orientamento = 0;                      // Orientation
int previousorientamento = 0;           // Previous orientation

// variables for the calculation of the PID
float time = 0;                               // Current time
float previousTime = 0;                   // Previous time
float interval = 0;                           // Time taken for the execution of a program cycle

//  PID Variables
float P = 0;                                  
float I = 0;                               
float D = 0;                             
float PID = 0;                                

// Gains of the PID coefficients
float kP = 12;                                
float kI = 430;                            
float kD = 20;                             

void setup() 
{
    Serial.begin( 9600 );                   
    pinMode( motore1, OUTPUT );          
    pinMode( motore2, OUTPUT );               
}

void loop()                                 
{  
  controllomotori( getPID() );
  
  Serial.print( "Distanza S1 =  " );                        // Print to the  monitor all data
  Serial.print(  analogRead( sensore1 ));                       
  Serial.print( "  DS1 Filtrata =  " );    
  Serial.print( distanzas1 );  
  Serial.print( "  Distanza S2 =  " );    
  Serial.print(  analogRead( sensore2 ));                       
  Serial.print( "  DS2 Filtrata =  " );    
  Serial.print( distanzas2 ); 
  Serial.print( "  Orientamento =  " );    
  Serial.print( orientamento ); 
  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( "  To Motori =  " );
  Serial.println( ( 255 / 2 ) + PID );
  
  previousorientamento = orientamento;         // Previous orientation  
}

Data Acquisition:

int getdistanzas1()                             // Algorithm for the acquisition of the distance s1
{
  for ( C = 1; C <= n; C++ )                          // Read  20 values ??from sensor 1, and writes them in an array
  {
    arrays1[C] = analogRead( sensore1 );
  }  
    
  for ( C = 1; C <= n; C++ )                         // Sorts the array in ascending order
  {
    for ( J = (C + 1); J <= n; J++ )
    {
      if ( arrays1[C] > arrays1[J] )
      {
        appoggio = arrays1[C];
       	arrays1[C] = arrays1[J];
	arrays1[J] = appoggio;
      }
    }
  }
  
  somma = 0;
  
  for ( C = 6; C <= ( n - 5 ); C++ )                 // Does not consider the highest and the lowest values
  {
    somma = somma + arrays1[C];
  }
  
  distanzas1 = somma / ( n - 10 );                // Make the average of the values
  
  return( distanzas1 );                          
}   




int getdistanzas2()                                    // Algorithm for the acquisition of the distance s2 ( as the sensor 1)
{
  for ( C = 1; C <= n; C++ )                     
  {
    arrays2[C] = analogRead( sensore2 );
  }  
    
  for ( C = 1; C <= n; C++ )                    
  {
    for ( J = (C + 1); J <= n; J++ )
    {
      if ( arrays2[C] > arrays2[J] )
      {
        appoggio = arrays2[C];
       	arrays2[C] = arrays2[J];
	arrays2[J] = appoggio;
      }
    }
  }
  
  somma = 0;
  
  for ( C = 6; C <= ( n - 5 ); C++ )           
  {
    somma = somma + arrays2[C];
  }
  
  distanzas2 = somma / ( n - 10 );           
  
  return( distanzas2 );                          
}

PID Controller, and Orientation:

int getPID()                                                 // Algorithm for the calculation of the PID
{
  getorientation();                                                // Invokes the function to obtain the orientation  
  previousTime = time;                                           // Save the time of the previous cycle
  time = millis();
  float interval = time - previousTime;                      // Calculate the time it takes to run a cycle
  
  P = orientamento * kP;                                          
  I = I + interval * kI * P;                               
  D = kD * ( orientamento - previousorientamento ) / interval;   
  
  PID = P + I + D;                                             
  
  if( PID >= 255/2 ) PID = 255/2;                              // If the value exceeds 255/2, the value is set to 255/2
  if( PID <= -255/2 ) PID = -255/2;                           // If the value is less than -255 / 2, the value is set to -255 / 2

  
  if(PID <= 1 && PID > 0) PID = 0;                            // Approximates the PID to 0
  if(PID >= -1 && PID < 0) PID = 0;                               
  
  return( PID );                                                  
}

void getorientation()                                              // Algorithm for calculating the orientation
{ 
  orientamento = getdistanzas1() - getdistanzas2();    // Difference between the distances detected by the two sensors

}

Motors Control:

void controllomotori ( int PID )                        // Algorithm for controlling two motors
{
  analogWrite( motore1, ( 255 / 2 ) + PID );          
  analogWrite( motore2, ( 255 / 2 ) + PID );        
}

The data acquisition works well enough, but i'm going to optimize it.

The robot try to get the balance, but absolutely does not maintain it...

What do you think?
Where can be the problem? The PID? The Gear Motors? Some problems in the Sketch?

Thanks very much in advance. :smiley:

p.s. Sorry for my english, i hope it is understandable :slight_smile:

PID loops require tuning so I suspect that your first guesses for kP, kI, and kD are not optimal.

johnwasser:
PID loops require tuning so I suspect that your first guesses for kP, kI, and kD are not optimal.

I tried to do some tuning... but, How can i tune it? It has to be adjusted only with experimental tests?

Try a Google search for: balancing robot PID tuning

I don't know about the code for these things, but there is a good resource on balancing bots
here, and 40-50 additional links, so someone in there probably talks about these matters.

http://www.geology.smu.edu/~dpa-www/robo/nbot/

johnwasser:
Try a Google search for: balancing robot PID tuning

Hehehe I'll read some stuff for sure! :smiley:

My principal worry is how understand where is the problem!

oric_dan(333):
I don't know about the code for these things, but there is a good resource on balancing bots
here, and 40-50 additional links, so someone in there probably talks about these matters.

nBot, a two wheel balancing robot

Thank you very much!
I found a similar project there, but unfortunately the link doesn't work... (and with the web search of that project i don't find nothing)
However it's very interesting (and more professional than mine)

Thank you all for the answers!

Soon i'll make a video while it works!

I have another question, which are the equations of the P, I and D?
Mine are right? I have to limit the I?

I'm trying to tune it but seems that there is something wrong...

You might want to start by eliminating I and D (set kI and kD to 0.0). Put your robot on its side and hold a board near the wheels so that the distance sensors measure "level". Your wheels should stop. As you tip the board slightly the wheels should start driving toward the closer side. If that test doesn't work (wheels run while 'level' or run in the wrong direction while 'tilted') you need to correct your control system.

johnwasser:
You might want to start by eliminating I and D (set kI and kD to 0.0). Put your robot on its side and hold a board near the wheels so that the distance sensors measure "level". Your wheels should stop. As you tip the board slightly the wheels should start driving toward the closer side. If that test doesn't work (wheels run while 'level' or run in the wrong direction while 'tilted') you need to correct your control system.

This works fine!
He "try" to keep the balance, but after one or two oscillations he falls... ( with only P, with PI, PD, PID... I tried a lot of values of kp, ki and kd but nothing, in any configuration)

All I know about PID is that it takes a lot of fine tuning to get the correct P,I,D gain values
for a particular situation. John is right. I would start with P, and then add in D only. Lastly, do
I after the others are set well. Break the problem into parts.

For tuning D, you might try just giving the top of the frame a quick push, ie, a sort of
impulse function, and playing with the D gain value until it responds properly. Also, you need
to be sure you have the "sign" of the gains correct. You want negative feedback, and not
positive feedback.

oric_dan(333):
All I know about PID is that it takes a lot of fine tuning to get the correct P,I,D gain values
for a particular situation. John is right. I would start with P, and then add in D only. Lastly, do
I after the others are set well. Break the problem into parts.

For tuning D, you might try just giving the top of the frame a quick push, ie, a sort of
impulse function, and playing with the D gain value until it responds properly. Also, you need
to be sure you have the "sign" of the gains correct. You want negative feedback, and not
positive feedback.

Hmm ok! I'll try and i'll let you know!
Thank you very much.

Is it possible that i don't get the balance because the DC motors at low values of the PWM ( 127 +- 30/40 ) don't move?

edit.I'm going to set these values with the minimum value with which the motors work...

It's very common that motors do not start moving until the PWM duty cycle gets past some
minimum threshold. I assume you'd set this value based upon how fast you want the robot
to move, and then use PID to superimpose on that to maintain balance.

oric_dan(333):
It's very common that motors do not start moving until the PWM duty cycle gets past some
minimum threshold. I assume you'd set this value based upon how fast you want the robot
to move, and then use PID to superimpose on that to maintain balance.

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

And, I found a big problem!
The serial communication took too long time, without it, the time by which the robot reacts has increased dramatically!

I still have to balance the PID, but now it can keep the balance for about 20 seconds!

Thank you very much for your help! :slight_smile:

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.

bensonlo:
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 :slight_smile:

--

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! :slight_smile:

Main Program:

// 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:

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

GetError:

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:

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 :slight_smile:

--

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