Hi all!
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!
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.
p.s. Sorry for my english, i hope it is understandable