Arduino Forum upgrade scheduled for Monday, October 20th, 11am-4pm (CEST). Sorry for the inconvenience!
Pages: [1] 2   Go Down
Author Topic: AutoBalance - PID Controller  (Read 11288 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 all!  smiley-grin
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:

http://oi47.tinypic.com/2le303k.jpg

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-grin

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


p.s. Sorry for my english, i hope it is understandable smiley
« Last Edit: January 14, 2013, 02:40:08 pm by Mosc » Logged

Massachusetts, USA
Offline Offline
Tesla Member
***
Karma: 213
Posts: 9060
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Send Bitcoin tips to: 1L3CTDoTgrXNA5WyF77uWqt4gUdye9mezN
Send Litecoin tips to : LVtpaq6JgJAZwvnVq3ftVeHafWkcpmuR1e

Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

Massachusetts, USA
Offline Offline
Tesla Member
***
Karma: 213
Posts: 9060
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Try a Google search for: balancing robot PID tuning
Logged

Send Bitcoin tips to: 1L3CTDoTgrXNA5WyF77uWqt4gUdye9mezN
Send Litecoin tips to : LVtpaq6JgJAZwvnVq3ftVeHafWkcpmuR1e

the land of sun+snow
Offline Offline
Faraday Member
**
Karma: 159
Posts: 2932
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Try a Google search for: balancing robot PID tuning
Hehehe I'll read some stuff for sure!  smiley-grin

My principal worry is how understand where is the problem!
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/
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!
« Last Edit: January 16, 2013, 04:15:08 pm by Mosc » Logged

Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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...
Logged

Massachusetts, USA
Offline Offline
Tesla Member
***
Karma: 213
Posts: 9060
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Send Bitcoin tips to: 1L3CTDoTgrXNA5WyF77uWqt4gUdye9mezN
Send Litecoin tips to : LVtpaq6JgJAZwvnVq3ftVeHafWkcpmuR1e

Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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)
Logged

the land of sun+snow
Offline Offline
Faraday Member
**
Karma: 159
Posts: 2932
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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.

Logged

Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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...
« Last Edit: January 18, 2013, 12:01:36 pm by Mosc » Logged

the land of sun+snow
Offline Offline
Faraday Member
**
Karma: 159
Posts: 2932
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Offline Offline
Full Member
***
Karma: 0
Posts: 103
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Offline Offline
Newbie
*
Karma: 0
Posts: 1
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.
Logged

Pages: [1] 2   Go Up
Arduino Forum upgrade scheduled for Monday, October 20th, 11am-4pm (CEST). Sorry for the inconvenience!
Jump to: