Vi tengo aggiornati 
Ho così modificato il programma:
Main program:
// 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:
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:
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:
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 