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:

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


--

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

Vi tengo aggiornati

Ho così modificato il programma:


Main program:

Code:

// Definizione dei pin utilizzati dai sensori
#define sensore1 A0                           // Definisce il pin del sensore1
#define sensore2 A1                           // Definisce il pin del sensore2

// Definizione dei pin utilizzati dai motori
#define motore1 3                             // Definisce il pin PWM a cui il motore1 è collegato
#define motore2 5                             // Definisce il pin PWM a cui il motore2 è collegato

// Variabili per il calcolo dell'orientamento
float orientamento = 0;                         // Inclinazione dell'AutoBalance, in funzione delle distanze rilevate
float previousorientamento = 0;

// Variabili temporali, per il calcolo del PID
long int time = 0;                               // Tempo auttuale, dall'accensione del dispositivo
long int previousTime = 0;                       // Tempo del precedente ciclo
int 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 = 5;                                // Variabile PID PROPORZIONALE
float kI = 18;                         // Variabile PID INTEGRATIVA
float kD = 25;                                // Variabile PID DERIVATRIVA

// Gestione PWM
float pwm = 0;

void setup()
{
//Serial.begin( 9600 );                     // Avvia la comunicazzione seriale
    pinMode( motore1, OUTPUT );               // Imposta il pin del motore1 come uscita
    pinMode( motore2, OUTPUT );               // Imposta il pin del motore2 come uscita
}

void loop()                                   //
{  
  controllomotori( getPID() );
/*
  Serial.print( "  Orientamento =  " );    
  Serial.print( orientamento );
  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( pwm );
*/
  previousorientamento = orientamento;         // Orientamento al ciclo precedente  
}

Acquisizione dati:

Code:

float getdistanza( int sensore )
{
  int n = 5;                                     // Numero di volte che si deve effettuare la misura, per il calcolo della media
  int arrays1[n];                                // Array di appoggio per la rilevazione della distanza1
  int somma = 0;                                 // Variabile di appoggio per il calcolo della media
  int minimo = 9999;
  int massimo = 0;
  int C;                                        

  for ( C = 0; C < n; C++ )
  {  
    arrays1[C] = analogRead( sensore );
  
    if ( arrays1[C] < minimo )
        minimo = arrays1[C];
      
    if ( arrays1[C] > massimo )
        massimo = arrays1[C];
 
    somma = somma + arrays1[C];
  }
          
  return (somma - minimo - massimo) / ( n - 2 );
}

Calcolo PID:

Code:

float getPID()                                                      // Algoritmo per il calcolo del PID
{
  getorientation();                                               // Richiama la funzione per ottenere l'orientamento
  
  previousTime = time;                                            // Salva il tempo al ciclo precedente
  time = millis();
  interval = time - previousTime;                                 // Calcola il tempo impiegato per eseguire un ciclo
  
  P = orientamento * kP;                                          // Calcola la variabile proporzionale
  I = I + P * interval * kI / 10000 ;                                      // Calcola la variabile integrativa
  D = kD * ( orientamento - previousorientamento ) / interval;    // Calcola la variabile derivativa
  
 // if (I > 100) I=100;
 // if (I < 100) I=100;

  PID = P + I + D;                                                // Calcola il PID
  
  if( PID > 127 ) PID = 127;                                 // Se il valore supera 255/2, il valore è impostato a 255/2  
  if( PID < -127 ) PID = -127;                               // Se il valore è minore di -255/2, il valore è impostato a -255/2  
  
  //if(PID <= 1 && PID > 0) PID = 0;                                // Approssima il PID a 0
  //if(PID >= -1 && PID < 0) PID = 0;                               // Approssima il PID a 0
  
  return( PID );                                                  // Ritorna il valore del PID
}

void getorientation()                                             // Algoritmo per il calcolo dell'orientamento
{
  orientamento = getdistanza( sensore1 ) - getdistanza( sensore2 );               // Differenza fra le distanze rilevate dai due sensori
}

Controllo motori:

Code:

void controllomotori ( float PID )                      // Algoritmo per il controllo dei due motori DC
{
  pwm = 127 + PID;
 /*
  if ( pwm > 127 && pwm < 180 )
     pwm = 180;
    
  if ( pwm < 127 && pwm > 74 )
    pwm = 74;
 */
  analogWrite( motore1, pwm );          // Controlla il motore1 in base al PID
  analogWrite( motore2, pwm );          // Controlla il motore2 in base al PID
}

Non sono ancora riuscito a bilanciare perfettamente il PID, ma ora riesce a tenersi in equlibrio per 15/20 secondi.
Il problema principale erano i print a schermo, che prendevano troppo tempo, tolti quelli e ottimizzato un pò il programma la situazione è migliorata molto  

Inoltre ho notato che distanziando maggiormente i sensori dal corpo del robot funziona meglio (anche se non ho ben capito perchè).

Inoltre ho provato a togliere la "zona morta" in cui il valore del PWM è così basso che i motori non si muovono, forzando questi valori al minimo valore del PWM per cui le ruote muovono (molto lentamente).
Devo ancora capire se serve questa parte o riesco a farla tramite PID  

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!

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

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.

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)

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

Try a Google search for: balancing robot PID tuning

Hehehe I'll read some stuff for sure!  

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!

Grazie mille per la risposta!  

Se ti posso chiedere una cosa, le formule che hai usato per il calcolo del PID, sono uguali a quelle che ho usato io?
E come invii il valore del PID ai motori? sempre nello stesso modo?

Ok!
Come rimonto il tutto farò un video!  

E grazie per i consigli su come regolare i coefficienti!
Non sapevo proprio come fare, come posso rifaccio qualche prova, e provo a regolarli imitando il tuo metodo  


edit. Mi interessa particolarmente il calcolo della I (del pid)  
La I assume valori troppo grandi, penso che ci sia qualche errore nel suo calcolo..

Per fargli assumere un valore nell'ordine delle centinaia ho dovuto mettere un kI = 0.00005;  o.o'

edit.2 Pensandoci, una cosa che mi darebbe un grande aiuto sarebbe conoscere quale ordine di grandezza assumo P, I e D durante il funzionamento (P suppongo 10 o 10², gli altri due?)
Ed un'ultima cosa, il valore di I và limitato in qualche modo? O deve essere libero di salire o scendere di qualsiasi valore? (in alcuni esempi trovati online ho visto che veniva limitato)

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?

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:

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!

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.


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

Vi allego lo Sketch: (L'ho diviso in schede per comodità)

Main Program:

Code:

// Definizione dei pin utilizzati dai sensori
#define sensore1 A0                           // Definisce il pin del sensore1
#define sensore2 A1                           // Definisce il pin del sensore2

// Definizione dei pin utilizzati dai motori
#define motore1 3                             // Definisce il pin PWM a cui il motore1 è collegato
#define motore2 5                             // Definisce il pin PWM a cui il motore2 è collegato

// Variabile per le distanze anteriore e posteriore
int distanzas1 = 0;                           // Valore letto dal sensore1
int distanzas2 = 0;                           // Valore letto dal sensore2

// Contatori d'appoggio
int C;                                        // Contatore d'appoggio
int J;                                        // Contatore d'appoggio

// Variabili per il filtraggio delle misure
int arrays1[10];                              // Array di appoggio per la rilevazione della distanza1
int arrays2[10];                              // Array di appoggio per la rilevazione della distanza2
int n = 20;                                   // Numero di volte che si deve effettuare la misura, per il calcolo della media
int somma = 0;                                // Variabile di appoggio per il calcolo della media
int appoggio;                                 // Variabie di appoggio per l'ordinamento degli array

// Variabili per il calcolo dell'orientamento
int orientamento = 0;                         // Inclinazione dell'AutoBalance, in funzione delle distanze rilevate
int previousorientamento = 0;

// 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 = 12;                                // Variabile PID PROPORZIONALE
float kI = 430;                               // Variabile PID INTEGRATIVA
float kD = 20;                                // Variabile PID DERIVATRIVA

void setup()
{
    Serial.begin( 9600 );                     // Avvia la comunicazzione seriale
    pinMode( motore1, OUTPUT );               // Imposta il pin del motore1 come uscita
    pinMode( motore2, OUTPUT );               // Imposta il pin del motore2 come uscita
}

void loop()                                   //  In Debug
{  
  controllomotori( getPID() );
  
  Serial.print( "Distanza S1 =  " );    
  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;         // Orientamento al ciclo precedente  
}

Acquisizione Dati:

Code:

int getdistanzas1()                              // Algoritmo per l'acquisizione della distanza s1
{
  for ( C = 1; C <= n; C++ )                     // Legge 20 valori dal sensore1, e li trascrive in un array
  {
    arrays1[C] = analogRead( sensore1 );
  }  
    
  for ( C = 1; C <= n; C++ )                     // Ordina l'array in modo crescente
  {
    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++ )             // Non considera gli estremi
  {
    somma = somma + arrays1[C];
  }
  
  distanzas1 = somma / ( n - 10 );               // Effettua la media dei valori
  
  return( distanzas1 );                          // Ritorna il valore distanzas2 oppurtunamente filtrato
}  




int getdistanzas2()                              // Algoritmo per l'acquisizione della distanza s2
{
  for ( C = 1; C <= n; C++ )                     // Legge 20 valori dal sensore2, e li trascrive in un array
  {
    arrays2[C] = analogRead( sensore2 );
  }  
    
  for ( C = 1; C <= n; C++ )                     // Ordina l'array in modo crescente
  {
    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++ )             // Non considera gli estremi
  {
    somma = somma + arrays2[C];
  }
  
  distanzas2 = somma / ( n - 10 );               // Effettua la media dei valori
  
  return( distanzas2 );                          // Ritorna il valore distanzas2 oppurtunamente filtrato
}

Calcolo PID:

Code:

int getPID()                                                      // Algoritmo per il calcolo del PID
{
  getorientation();                                               // Richiama la funzione per ottenere l'orientamento
  
  previousTime = time;                                            // Salva il tempo al ciclo precedente
  time = millis();
  interval = time - previousTime;                           // Calcola il tempo impiegato per eseguire un ciclo
  
  P = orientamento * kP;                                          // Calcola la variabile proporzionale
  I = I + interval * kI * P;                                      // Calcola la variabile integrativa
  D = kD * ( orientamento - previousorientamento ) / interval;    // Calcola la variabile derivativa
  
  PID = P + I + D;                                                // Calcola il PID
  
  if( PID >= 255/2 ) PID = 255/2;                                 // Se il valore supera 255/2, il valore è impostato a 255/2  
  if( PID <= -255/2 ) PID = -255/2;                               // Se il valore è minore di -255/2, il valore è impostato a -255/2  
  
  if(PID <= 1 && PID > 0) PID = 0;                                // Approssima il PID a 0
  if(PID >= -1 && PID < 0) PID = 0;                               // Approssima il PID a 0
  
  return( PID );                                                  // Ritorna il valore del PID
}

void getorientation()                                             // Algoritmo per il calcolo dell'orientamento
{
  orientamento = getdistanzas1() - getdistanzas2();               // Differenza fra le distanze rilevate dai due sensori
}

Controllo motori:

Code:

void controllomotori ( int PID )                      // Algoritmo per il controlla i due motori DC
{
  analogWrite( motore1, ( 255 / 2 ) + PID );          // Controlla il motore1 in base al PID
  analogWrite( motore2, ( 255 / 2 ) + PID );          // Controlla il motore2 in base al PID
}

Quello di cui sono abbastanza sicuro è che la parte di acquisizione dati funziona "bene".
Non avevo idea di come farla, cosi ho deciso di prendere n valori, metterli in un tabella, ordinarla, togliere gli estremi e fare la media con i restanti.
Non ho idea se questo sia un metodo efficace oppure no, ma sembra fare il suo dovere...

Il problema è il resto  

Togliendo la parte integrativa dal PID da accenni di funzionamento, ma non riesce assolutamente a tenersi in equilibrio...

Qualcuno ha qualche idea / consiglio su quello che posso fare?  

L'obiettivo è portarlo come progetto d'esame  
Grazie a tutti in anticipo.

Salve a tutti
Mi sto cimentando nella costruzione di un AutoBalance, uno di quei sistemi con due ruote che riescono a tenersi in equilibrio da soli.

Per ora, il mio unico obbiettivo è mantenere l'equilibrio su una superficie piana.
Quindi niente salite, discese, forze esterne, o controllo del movimento.

La parte meccanica, in pratica è finita.
Purtroppo non ho una foto a portata di mano, quindi vi metto un rendering per darvi un'idea di come è fatto:

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

Il "robot" è composto da un Arduino UNO, due sensori SHARP analogici ( GP2Y0A21YK0F ), due motoriduttori ( SBGM9) ed un driver motore fatto da me ( se volete poi allego anche lo schema elettrico di questo).
Il primo piano per ora è vuoto, in seguito ci andranno le batterie. (Per ora faccio le prove con dei fili volanti)

--

Allora, il problema è, ovviamente, che non si tiene in equilibrio  

La parte della gestione del PID l'ho copiata da un esempio trovato online (per la precisione quì: http://schianorobotics.altervista.org/SISTEMI_DI_CONTROLLO_MULTIVARIABILE.pdf), il resto l'ho scritto io..

Non sò se il problema sia dovuto ad un problema software, al fatto che non sono capace di gestire un PID oppure (la cosa che temo di più) che i due motoriduttori non sono adatti, e che dovrò cambiarli con dei servo 360° (cosa che vorrei, ovviamente, evitare...)

Continuo nel post successivo che non fà postare più 9500 caratteri 

Grazie mille!

Allora penso che userò i +5V, usando appunto il piedino +5V 

Salve, ho un dubbio riguardo l'alimentazione di arduino...

Allora, tramite il Jack va alimentato 7-12V
Ma tramite il piedino Vin?

C'è un modo per alimentarlo con 5 v?