Controllo PID

Ragazzi sono vicino al vedere stare in piedi il mio mini segway, ma ho ancora qualche dubbio:
credete che il mini segway [con motori lego + arduino uno + imu mpu6050] possa stare in equilibrio con un controllore PID sulla velocità?
Che rapporto può avere questo con il controllare i motori in velocità e posizione ( passare al motore un comando del tipo motorA.setSpeedUntil(speed, encoder_pos) rispetto a controllarli solo in velocità ( motorA.setSpeed(255) ) ?

Come ArduWay di marco triverio (metto in allegato il codice che lo bilancia, Ardu.ino)
http://forum.arduino.cc/index.php/topic,8739.0.html ne parlano qui
http://sourceforge.net/projects/a2l/ codice preso da qui
-Vorrei come lui poter usare un encoder solo e controllare i motori in velocità: cosa diavolo ha fatto con questa force e un vettore di stati(??) a bilanciare il robottino? Chi + esperto di me riesce a darmi qualche informazione preziosa su questo codice?

   /* 
     *  CONTROLLER ACTION
     */
    force = 0;
    for(int i = 0; i < 4; i++) { //controller action
      force =  force + matr_K[i]*state[i];
    }
    force = (-1)*force*30; //TODO check motor gain, 27=ok
    //if(force < 170 && force > -170) l.on();
    //else l.off();
    m.setSpeed((int) (force));

ardu.ino (2.15 KB)

ciao, allora, il PID è sufficiente, ho visto tantissimi progettini come il tuo usarlo.

I first used the rectangle method for integration but now I've preferred the trapezoid method to minimize the error: what I've found out is that most of the error comes from (1) the sensor and (2) ADC approximation.

Anyone going to replicate this robot should use an angle sensor (maybe I2C based) instead of a gyroscope; integration is bringing avoidable error.

a quanto piare lui usa Regola del trapezio - Wikipedia , ma visto che tu hai una IMU non ti serve.

edit: come non detto
Lui sta mettendo in stato un pò di valori; posiszione stimata? velocità lineare, e un paio di altre cose, poi le moltoplica con il fattore K (quindi è la P di un pid) e le somma per ottenere la forza finale.. non ho idea da che csa sia partito per fare ciò, che io sappia basta un pid sull'angolazione, ma magari lui ha voluto aggiungere robe per poter controllare la posizione, e la cosa mi sfugge

lesto:
ciao, allora, il PID è sufficiente, ho visto tantissimi progettini come il tuo usarlo.

I first used the rectangle method for integration but now I've preferred the trapezoid method to minimize the error: what I've found out is that most of the error comes from (1) the sensor and (2) ADC approximation.

Anyone going to replicate this robot should use an angle sensor (maybe I2C based) instead of a gyroscope; integration is bringing avoidable error.




a quanto piare lui usa http://it.wikipedia.org/wiki/Regola_del_trapezio , ma visto che tu hai una IMU non ti serve.

edit: come non detto
Lui sta mettendo in stato un pò di valori; posiszione stimata? velocità lineare, e un paio di altre cose, poi le moltoplica con il fattore K (quindi è la P di un pid) e le somma per ottenere la forza finale.. non ho idea da che csa sia partito per fare ciò, che io sappia basta un pid sull'angolazione, ma magari lui ha voluto aggiungere robe per poter controllare la posizione, e la cosa mi sfugge

Posto il codice del PID che sputa fuori una velocità (da passare ai due motori) e valuta l’errore come differenza tra il setpoint e il valore rilevato dalla IMU.

La procedura standard è questa?
Quali migliorie potete consigliarmi?
Mi sono sfuggiti errori?
ben accetto qualsiasi consiglio!

//DEFINIZIONE VARIABILI                            
double output;
double input;

double setpoint = 0; //da correggere

double P = 0;
double I = 0;
double D = 0;
//Coefficienti del regolatore
double kP = 10;
double kI = 0;
double kD = 0;
//
double error;
double ITerm;
double DTerm; 
double Ki, Kp, Kd;
float currentValue=0;
float lastInput=0;
int vel=0;
int equilibrio=0;

float x=0;
float rpy_rol=0;
float rpy_pit=0;
float rpy_yaw=0;
float ADegX=0;
float ADegY=0;

int updatePid(int setpoint, float x) { // compute PWM value
 float time = millis();
 float previousTime = time;                                            // Salva il tempo al ciclo precedente
 float interval = time - previousTime;                                 // Calcola il tempo impiegato per eseguire un ciclo
                   
         error =(double)(abs(setpoint) - abs(x));
	  
         P = error * kP;                                                                                                    // Calcola la variabile proporzionale 
         I =(double) I + interval * kI * P;                                                                    // Calcola la variabile integrativa
         D =(double) kD * (abs(currentValue - lastInput) / interval);            // Calcola la variabile derivativa	
         lastInput=currentValue;
        
         vel =(int) P + I + D;
        
        //elimino le micro correzioni      
        if(vel <= 1 && vel > 0) vel = 0;                                
        if(vel >= -1 && vel < 0) vel = 0;                               
        
        //if( vel >= 255/2 )  vel = 255/2;                                // da usare se ancora in fase di test  per evitare la vel max e distruggere tutto
                
        constrain(vel, 0, 255);
        return vel;
}

// dentro al LOOP
 vel = updatePid(setpoint, x);
        
             if(setpoint == x)  {equilibrio = 0;}-->motori fermi
             if(ADegX > 246)    {equilibrio = 1;}---> senso orario ruote
             if(ADegX < 246)    {equilibrio = 2;}---> senso antiorario ruote

ps: ADegX è uno dei valori rilevati dalla mia IMU: lo uso per semplicità (perchè non mi sn venute in mente altre soluzioni per andicare l’inclinazione e quindi in quale delle due casistiche ricadere). empiricamente ho visto che il valore della stabilità è 246.

guarda che il PID serve proprio per evitare valori così hardcodati..

setpoint è l'inclinazione desisderata, x è la tua angolazione rilevata, e vel è la velocità a cui muovere il motore. Tu devi giocare con i valori di P I e D (in teoria solo P, ed eentualmente un pizzico di I, bastano)

lesto:
guarda che il PID serve proprio per evitare valori così hardcodati…

setpoint è l’inclinazione desisderata, x è la tua angolazione rilevata, e vel è la velocità a cui muovere il motore. Tu devi giocare con i valori di P I e D (in teoria solo P, ed eentualmente un pizzico di I, bastano)

cosa vuoi dire? che devo rendere tutto + semplice?
Ad esempio così?

int sampleTime =9;
unsigned long lastTime = 0;
double output;
double input;
double setpoint = 0;
double error;
double iTerm, lastValue,pTerm,dTerm;

int updatePid(double targetValue, double currentValue) {
	
	/*How long since we last calculated*/
	unsigned long now = millis();
	float pidTerm;
	int timeChange = (now - lastTime);

	if (timeChange >= sampleTime) {
		/*Compute all the working error variables*/
		error = targetValue - currentValue;

		iTerm += Ki * error;
		iTerm = constrain(iTerm, -50, 50);

		pTerm = Kp * error;

		dTerm = Kd * (currentValue - lastValue);

		/*Compute PID Output*/
		pidTerm = (pTerm - dTerm + iTerm) * K;

		lastValue = currentValue;
		lastTime = now;
	}
	return constrain(round(pidTerm), -255, 255);
}


void loop(){

	input = VALORE DALLA IMU;
	output = updatePid(setpoint,input);

	AmotorNXT.setMotor(output);
	BmotorNXT.setMotor(output);



}

sì, poi devi cercare iTerm, pTerm,dTerm;

Non sn stato a riportare tutte le variabili, ma solo la funzione che calcola il PID. stampa a video solo il valore 100 e un altro valore che tende ad aumentare fino a che raggiunge il limite 255 e dopo questa cosa si alternano infiniti 100 con qualche 255 in mezzo. Che cosa sto combinando?

unsigned long lastTime;
int SampleTime=100; //0.1 sec

int updatePid(float mySetpoint, float x){

   unsigned long now = millis();
   
   unsigned long timeChange = (now - lastTime);
   if(timeChange>=SampleTime)
   {
      /*Compute all the working error variables*/
      float error = mySetpoint - x;
      ITerm+= (ki * error);
      float dInput = (x - lastInput);
 
      /*Compute PID Output*/
      int output =kp * error + ITerm- kd * dInput;
      	  
      /*Remember some variables for next time*/
      lastInput = x;
      lastTime = now;

                
                //if( vel >= 255/2 )  vel = 255/2;        // in fase di test  
                if(output>=245){output=255;}        
                if(output>=255){output=255;}
                if(output<=0)  {output=0;}
                if(output <= 5 && output > 0) {vel = 0;}
                return output;
   }}
100
100
100
100
100
100
100
100
44
100
100
100
100
100
100
100
100
125
100
100
100
100
100
100
100
100
129
100
100
100
100
100
100
100
100
132
100
100
100
100
100
100
100
100
135
100
100
100
100
100
100
100
100
138

che cosa sono quella serie di if finaly? dov'è il resto del codice?

messa così la risposta è "magia nera" :grin:

E’ il pid che serve per controllare in pwm la velocità dei motori e bilanciare il robottino
[motori lego nxt–arduino uno–imu mpu6050]

/////////DEFINIZIONE VARIABILI
 float x=0; //valore che arriva dalla imu

float mySetpoint = 81.9;  //valore imu dal robot tenuto in equilibrio con la mano
    
    double kp=10;                  // * (P)roportional Tuning Parameter
    double ki=5;                  // * (I)ntegral Tuning Parameter
    double kd=1;                  // * (D)erivative Tuning Parameter
   
double ITerm, lastInput;
unsigned long lastTime;
int SampleTime=100; //0.1 sec
lastTime = millis()-SampleTime;

int updatePid(float mySetpoint, float x){

   unsigned long now = millis();
   
   int timeChange = (now - lastTime);
   if(timeChange>=SampleTime)
   {
      /*Compute all the working error variables*/
      float error = mySetpoint - x;
      ITerm += ki*(error * (SampleTime/1000));
      float dInput = (x - lastInput);
 
      /*Compute PID Output*/
      int output = (kp * error) + ITerm + (kd * dInput);
      	  
      /*Remember some variables for next time*/
      lastInput = x;
      lastTime = now;
      
                if(output>=255){output=255;}
                if(output<=0)  {output=0;}
                
                return output;
   }}

lastTime = millis()-SampleTime;
questa riga mi da errore:

copia:286: error: expected constructor, destructor, or type conversion before '=' token

int lastTime = millis()-SampleTime; ho corretto così ma solo a volte non mi da errore!! cosa sto facendo?
questa parte viene da PID_v1.
dove sbaglio!?

(riguardo l’errore sui valori sputati fuori sarà colpa dell windup?)

che c'è tutta una parte di codice che NON è all'interno di una funzione.?

cioè se questo è il tuo sketch manca un bel pò di roba, il setup(), il loop(), gli eventuali #include, la tua funziona updatePid fa return solo se l'if è vero

(riguardo l'errore sui valori sputati fuori sarà colpa dell windup?)

mi si è rotta la sfera di cristallo, e di sicuro non è questo il codice che usi visto che non compila. In particolare bisogna capire anche QUALI valori passi al pid e QUANDO

Allego il file così evitiamo fraintendimenti
lascio le cose come stanno così puoi vedere tu stesso l’errore che mi da.
Non ho postato tutto il codice per l’eccessiva lunghezza.
Non capisco perchè mi dia errore sulla riga del millis() :frowning:

se metto tutto dentro la funzione che calcola il pid i due millis() non avranno sempre lo stesso valore?
cosa deve ritornare la funzione si l’if è falso??

copia.ino (63.1 KB)

all'esterno di funzioni puoi solo dichiarare
quindi NON puoi fare una istruzione

lastTime=millis() - SampleTime;

ma puoi fare una DICHIARAZIONE + inizializzazione

unsignd long lastTime=millis() - SampleTime;

per il PID mi sembra possibile se la X corrisponde al valore di setpoint...

    double kp=10;                  // * (P)roportional Tuning Parameter
    double ki=1;                  // * (I)ntegral Tuning Parameter
    double kd=10;                  // * (D)erivative Tuning Parameter
  
    double ITerm, lastInput;
    unsigned long lastTime;
    unsigned long SampleTime;							

void iniz(){
      SampleTime = 100;							
      lastTime = millis()-SampleTime;	
  
}


int updatePid(float mySetpoint, float x){
   init();
   
   unsigned long now = millis();
   unsigned long timeChange = (now - lastTime);
   if(timeChange>=SampleTime)
   {
      /*Compute all the working error variables*/
      float error = mySetpoint - x;
      ITerm += ki*(error * (SampleTime/1000));
      float dInput = (x - lastInput);
 
      /*Compute PID Output*/
      int output = (kp * error) + ITerm - (kd * dInput);
      	  
      /*Remember some variables for next time*/
      lastInput = x;
      lastTime = now;

                
                if(output>=255){output=255;}
                if(output<=0)  {output=0;}
               
                return output;
   }
   else return -1;
}
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
Enabling DMP... done.
Enabling interrupt detection... done.
DMP ready! Waiting for first data from MPU-6000...
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
255
100
100
100
100
100
100
100
100
255
100
100
100
100
100
100
100
100
255

perchè ricade soltanto su questi due valori?
dovrebbe darmi sempre 0 (è fermo in equilibrio in verticale)
e 255 ai due estremi… :frowning:

assolutamente no, stai usando sia la parte integrale che la parte derivativa. Il comportamento che dici te lo avresti usando SOLO la parte proporzionale. per questo metti KI e KD a 0, e vedi se solo parte KP funziona; in otler è facile da calcolare il valore atteso, dato che l'algorimto diventa

risultato= (valoreNuovo-valoreVecchio)*kp

Adesso il pid funziona e anche se non ho ancora settato kd e ki il robottino prova con delle “botte” a bilanciarsi… che carino XD

    double kp=35; //100 botta forte
    double ki=0;                  
    double kd=0;                  
  
    double ITerm, lastInput;
    unsigned long lastTime;
    unsigned long SampleTime;							

void iniz(){
      SampleTime = 100;							
      lastTime = millis()-SampleTime;	
  }


int updatePid(float mySetpoint, float x){
   iniz();
   
   unsigned long now = millis();
   unsigned long timeChange = (now - lastTime);
   if(timeChange>=SampleTime)
   {
      /*Compute all the working error variables*/
      float error = mySetpoint - x;
      ITerm += ki*(error * (SampleTime/1000));
      float dInput = (x - lastInput);
 
      /*Compute PID Output*/
      int output = (kp * error) + ITerm - (kd * dInput);
      	  
      /*Remember some variables for next time*/
      lastInput = x;
      lastTime = now;

                                        
                if(output>=255){output=255;}
                if(output<=0)  {output=0;}
                if(output <= 2 && output > 0) {vel = 0;}
                return output;
   }
   else return -1;
}

il problema che ho è che il robot comincia a correggere a circa 70-80(troppo tardi) su 255 e quindi cade.
FINO A 70-80 I DUE MOTORI LEGO NON SI MUOVONO ED EMETTONO UNO STRANO BEEP NEVROTICO!
qualcuno avete mai riscontrato di questi problemi?
ho cercato su google ma niente… perchè un motore con encoder(che x ora nn uso) deve fare quel suono… troppi comandi che gli arrivano?? escluderei falsi contatti…
ps smacchinando e spulciando in giro con il kit lego nxt ho letto che hanno previsto una funzione tipo NXTmotor.WaitFor();
che attende che il mattoncino esegua l’istruzione. Ora che arduino ha preso il posto del brick non vorrei che mancasse qualcosa di hardware/software… :disappointed_relieved:

HO TROVATO QUALCOSA:
http://www.mindstorms.rwth-aachen.de/trac/wiki/FAQ

    I'm controlling two motors, and when their movement has stopped, the motors make a strange high-pitched noise…
        This is a feature of the NXT firmware. The sound occurs when working with two synchronized motors, e.g. for driving robots. Once the movement has stopped (using old-fashioned SendMotorSettings command from toolbox version 2.*, or using ActionAtTachoLimit = 'Coast' with NXTMotor in toolbox version 4.*, or DirectMotorCommand), the motors are still synced. If you move one motor, the other will move, too. This by design, and this is when you hear the high-pitched sound from the motors. To disable this noise, just send a .Stop() command from NXTMotor or use StopMotor(). The next motor command should clear / override the synchronization anyway.

ragazzi come potete vedere dalle foto io uso arduino uno quindi non c’è traccia di nxt ne firmware… sono allibito
mi spieghi chi + di me sa, per favore…

copia.ino (63 KB)

scusate faccio reply x mettere un paio di foto!

per i motori on so cosa dire.

per il pid dovresti drovare il valore limitie per cui i colpi NON avvengono, poi a quel punto inizi ad aumentare la I, oppure trovare l valore limite per cui avvengono ed aumentre la D