problemi con robot autobilanciante

Salve a tutti!

Ho deciso di alzare il robot autobilanciante che avevo costruito per aiutarlo nell’ardua impresa… ora è così e si comporta decisamente meglio, anche se non sono riuscito a registrare uno dei suoi momenti migliori, a partire da 00:07 dovreste farvi un’idea. Cosa potete dirmi a proposito dei parametri del pid?

/* PID variables */
double Kp = 10 ;            // 10     
double Ki =  0;             // 0.000001     
double Kd = 5;             // 5

Ho postato il codice completo.

coast_mode.ino (13.7 KB)

Scusa ma sei sicuro che mettere la batteria in cima aiuti? Non è che col suo peso sbilancia troppo il corpo?

La batteria in cime facilita le cose. Prova ad immaginare un manico di scopa e tu che provi a bilanciarlo sul palmo della tua mano. Se avesse in peso in cima ti aiuterebbe nello scopo.

VIDEO e parametri del pid relativi:

/* PID variables */
double Kp = 9;            // 9    
double Ki = 0.2;          //0.2           
double Kd = 8;             // 8-10

Penso di aver fatto del mio meglio, ma vedo che :
-il backlash è un problema perché il robot tende ad inclinarsi un po’ ogni tot e poi va in overshoot

-ruote più pesanti sarebbero in grado di trasferire verso il basso una coppia maggiore delle mie ( ruote molto leggere )

-fixed time loop: sto pensando che … penso di dare un comando ogni 100 ms … ma la comunicazione seriale può rallentare sensibilmente il miol codice?

-Da aggiungere al programma che al primo tocco dello " 0 " dalla MPU il robot deve iniziare a lavorare

mi fa impazzire

codice ultimo negli allegati

coast_mode.ino (13.8 KB)

Anch'io penso che la posizione della batteria sia sbagliata: lo scopo deve essere quello di ridurre il più possibile la posizione verticale del baricentro, in modo da facilitare il bilanciamento.

La batteria non solo deve essere posta in alto, ma deve essere il più lontano possibile dal fulcro, il robot auto bilanciante è un pendolo inverso e come tale va trattato. Penso che vi ricordate tutti che il tempo di oscillazione del pendolo, per piccoli angoli, è una costante e dipende esclusivamente dalla distanza della massa dal centro di rotazione, nel caso di angoli di oscillazione elevati dipende anche dal coseno dell'angolo. Lo scopo della batteria sopra e lontana è quello di avvicinarsi quanto possibile al pendolo ideale, ovvero una massa puntiforme sospesa ad una certa distanza. Maggiore è la massa verso l'alto del robot e migliore diventa la similitudine, maggiore è la distanza della massa dal fulcro di rotazione e maggiore è il tempo di oscillazione, ovvero la costante di tempo del robot diventa maggiore e questo porta benefici per il controllo in quanto serve una velocità minore per il ciclo del pid.

NXTfreeDOmF: -il backlash è un problema perché il robot tende ad inclinarsi un po' ogni tot e poi va in overshoot

Non sei in overshoot, sei nella condizione opposta, controllo troppo lento, solo dopo vai in overshoot perché il controllo non è abbastanza veloce per compensare. Il backlash, a meno che non sia enorme, non è un reale problema se le cose funzionano come si deve.

-ruote più pesanti sarebbero in grado di trasferire verso il basso una coppia maggiore delle mie ( ruote molto leggere )

Assolutamente no, questa è fisica di base, quello che conta è la massa totale del mezzo e non delle ruote, l'attrito, e quindi la forza trasferibile, dipende esclusivamente dalla massa totale, la sua ripartizione sulle ruote, e dal coefficiente d'attrito tra i materiali.

-fixed time loop: sto pensando che .... penso di dare un comando ogni 100 ms .. ma la comunicazione seriale può rallentare sensibilmente il miol codice?

Questo è il tuo problema, troppo lento il ciclo di controllo, magari pure con troppo jitter, devi lavorare ad almeno 10 ms (100 hz), meglio ancora se lo fai a 1ms.

Grazie astro del tuo aiuto. Ho ritrovato la discussione (in cui aiutavi phanto) dove si parlava del fatto che il periodo del pid dovesse essere compreso tra 1:30 e 1:50 della costante di tempo del robot… ma non torna con quello che ho misurato…

int STD_LOOP_TIME = 9;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

void loop()  {


 
/* Drive motors */
  PID(targetAngle,targetOffset,turningOffset);   
  serialOut_timing();

/* Use a time fixed loop */
  lastLoopUsefulTime = micros() - loopStartTime;
  if (lastLoopUsefulTime < STD_LOOP_TIME)
  lastLoopTime = micros() - loopStartTime;
  loopStartTime = micros();  

}

void serialOut_timing() {

  static int skip=0;

  if(skip++==5) { // display every 500 ms
    skip = 0;
    Serial.print(lastLoopUsefulTime); 
    Serial.println();  
    Serial.print(lastLoopTime); 
    Serial.println();
  }
}

Ho allegato l’output della funzione e lastLoopUsefulTime è circa 5 millisecondi, lastLoopTime varia tra 9 e 11 inizialmente, per poi stabilizzarsi sul 3 per il primo e sul 10 per il secondo. Quindi sembra essere costante senza jitter…

Se fisso il tempo del loop, posso eliminare la variabile tempo dal mio PID?

Ho allegato anche un piccolo file con soltanto la parte relativa al timing, così è più rapida la comprensione.

code_timing.txt (1.57 KB)

timing_output.txt (1.59 KB)

Il pid digitale DEVE lavorare ad intervalli di tempo costanti, senza jitter, non puoi usare il tempo come variabile, deve essere una costante. Devi invocare il PID ad intervalli costanti, p.e. usando un timer, altrimenti non funziona o funziona male, nel computo usa solo numeri interi, lascia perdere i float, e il tempo consideralo sempre pari a un valore costante che imponi tu in base al ciclo di campionamento, p.e. 1-5-10, etc.

Indagando sul fatto che i motori non si muovessero, ho avviato il vecchio sketch di test dei motori e funzionano perfettamente… :o la motor driver funziona.
Allora ho pensato che il problema fosse dello sketch.
Seguendo le indicazioni di astro, ho provato a fissare il tempo del mio loop, in modo che il pid fosse valutato ad intervalli costanti.
Ma io ho semplicemente modificato il codice, aggiungendo un paio di righe così come spiega Brett nel suo tutorial:
(brett tutorial to improve beginner’s pid)
(int SampleTime = 10;)
Le ruote però, hanno smesso di muoversi…

void PID(double restAngle, double offset, double turning) {
   double PIDValue;
   unsigned long now = millis();
   int timeChange = (now - lastTime);
   if(timeChange>=SampleTime){

  double error = restAngle - pitch;
  double pTerm = Kp * error;
  iTerm += error;
  iTerm= Ki*iTerm;
  
  if(iTerm > 90) iTerm = 90;
  else if(iTerm < -90) iTerm = -90;
  
  double dTerm = Kd * (error - lastError);    
  PIDValue = pTerm + iTerm + dTerm;
  lastError = error;
  lastTime = now;
  
  if(PIDValue > 799) PIDValue = 799;
  else if(PIDValue < -799) PIDValue = -799;

   double PIDLeft;
   double PIDRight;
 
    PIDLeft = PIDValue;
    PIDRight = PIDValue;
   
    moveMotor(left, PIDLeft);  
    moveMotor(right, PIDRight);
   
   Serial.print(pTerm);Serial.print("\t"); Serial.print(iTerm);Serial.print("\t");     
   Serial.print(dTerm);Serial.print("\t"); Serial.print(PIDValue);
   Serial.println();
   }  
}

Ancora più insospettito, ho fatto qualche altro test COL ROBOT SDRAIATO SUL TAVOLO:
se provo a stampare kTerm, è tutto ok;
se provo a stampare iTerm, leggo solo dei nan;
se provo a stampare dTerm, è tutto ok;
se provo a stampare PIDValue, leggo solo dei nan;

/* PID variables */
double Kp = 8;
double Ki = 1; (ma anche se la pongo=0)
double Kd = 3;

E QUESTO RISULTA:

701.45	nan	-0.44	nan
697.96	nan	-1.31	nan
693.06	nan	-1.84	nan
687.41	nan	-2.12	nan
681.41	nan	-2.25	nan
675.18	nan	-2.34	nan
668.84	nan	-2.38	nan
662.47	nan	-2.39	nan
656.12	nan	-2.38	nan
649.72	nan	-2.40	nan
643.35	nan	-2.39	nan
637.08	nan	-2.35	nan

FINO A STABILIZZARSI SUL:
CIRCA 200    NAN     0.00    NAN

Il PIDValue=nan quindi le ruote non si muovono.
Ma per quale motivo leggo questi nan? io non lo so proprio.

hardware testato e funzionante
il robot andava, ho postato diversi video

coast_mode.ino (11.3 KB)

nan = Not a Number … stai, per caso, facendo delle divisioni per zero ? O stai passando a float() qualche cosa che NON è un numero ?

La descrizione di float() di processing dice :

Converts an int or String to its floating point representation. An int is easily converted to a float, but the contents of a String must resemble a number, or NaN (not a number) will be returned. For example, float(“1234.56”) evaluates to 1234.56, but float(“giraffe”) will return NaN.

Guglielmo

gpb01:
nan = Not a Number … stai, per caso, facendo delle divisioni per zero ? O stai passando a float() qualche cosa che NON è un numero ?

La descrizione di float() di processing dice :

Guglielmo

int SampleTime = 10; 
unsigned long lastTime;
double lastError; // Store last angle error
double iTerm; // Store integral Term

void PID(double restAngle, double offset, double turning) {
   double PIDValue;
   unsigned long now = millis();
   int timeChange = (now - lastTime);
   if(timeChange>=SampleTime){

  double error = restAngle - pitch;
  double pTerm = Kp * error;
  iTerm += error;
  iTerm= Ki*iTerm;
  
  if(iTerm > 90) iTerm = 90;
  else if(iTerm < -90) iTerm = -90;
  
  double dTerm = Kd * (error - lastError);    
  PIDValue = pTerm + iTerm - dTerm;
  lastError = error;
  lastTime = now;
  
  if(PIDValue > 799) PIDValue = 799;
  else if(PIDValue < -799) PIDValue = -799;

   double PIDLeft;
   double PIDRight;
 
    PIDLeft = PIDValue;
    PIDRight = PIDValue;
   
    moveMotor(left, PIDLeft);  
    moveMotor(right, PIDRight);
//    Serial.print(pTerm);Serial.print("\t"); Serial.print(iTerm);Serial.print("\t"); //Serial.print(dTerm);Serial.print("\t"); Serial.print(PIDValue);
//   Serial.println();
   }  
}

Non mi sembra che ci siano i presupposti per restituirmi un nan…
avete idea da dove possa nascere?

Ho allegato il codice.

C’è nessuno che possa darmi una mano?
Ho postato codice completo, output seriale e probabile funzione incriminata.

Avete qualche idea?

def.ino (10.2 KB)

Posso solo suggerirti di mettere delle Serial.print() prima e dopo ogni calcolo e verificare che i valori siano numerici e validi ...

Guglielmo

provo come dici tu. grazie guglielmo

Got my Balancing bot code Working Here it is:

altered one other file:

//in "MPU6050_6Axis_MotionApps20.h"
//I've altered line 305 to: 
 0x02,   0x16,   0x02,   0x00, 0x02                // D_0_22 inv_set_fifo_rate

to slow down the fifo buffer filling. corrected garbage values that appeared.

My motor drive will be different but this gives you an idea what i’m doing.

Attempting to test with your code now :slight_smile:

BalancingBotUno.ino (12.4 KB)

codice in allegato.
Ho semplificato il codice per capire da dove venisse fuori quel nan…
non passo nulla come String e non ho fato divisioni per zero, lo dimostra kTerm numerico e positivo…
lo dimostra dterm…
iTerm = nan e quindi Output=nan…
davvero frustrante

ho stampato a seriale le variabili prima e dopo qualsiasi operazione: nel pid che senso ha che la variabile “error” se moltiplicata per kp mi dia un numero, se moltiplicata per ki mi dia nan???
anche error è numerica e la stampa correttamente…

def.ino (10.2 KB)

Era un problema di variabili definite erroneamente da me fuori dal pid.
Allego il codice definitivo.
Il robot funziona, ha solo bisogno di un fine tuning delle variabili del pid.
Presto postero’ un video!

def.ino (10.1 KB)