Le NoRobo ne balance toujours pas, mais j’avance !

Le titre de cet article était au départ « Le NoRobo balance [presque] ! ». Mais j’étais trop optimiste… Dans l’article précédent de cette série Un robot arduino, j’ai un sketch simple qui fait fonctionner le NoRobo sur trois roues et lit correctement son angle par rapport à la verticale. Il faut maintenant que le NoRobo balance ! 

Réglages

Le code à ce stade est toujours NoRobo-Gyro-2016-04-24A.ino. Attention de ne pas oublier de placer I2C.ino dans le même répertoire. On les trouve ici sur GitHub.

J’ai fait beaucoup de recherches sur la régulation PID et je ne parviens toujours pas à faire balancer mon robot…

Je viens de comprendre la raison d’être de la variable idl_motor_power , définie en ligne 81 par uint8_t idl_motor_power = 80;  ! Les moteurs DC ne fonctionnent pas dès 0. Les valeurs qu’ils doivent recevoir pour commencer à rouler sont en général bien plus élevées.

J’ai fait un petit sketch (Dc-motor-potentiometer.ino) incluant un potentiomètre (branché en A0) et j’ai regardé à partir de quelle valeur les deux moteurs démarrent. Dans mon cas c’est 75.

Dans les difficultés que j’ai, il y a deux choses différentes :

• trouver le bon réglage PID ;
• faire varier la vitesse de mes deux moteurs selon ce réglage dans tous les cas.

Ajuster la vitesse des moteurs selon l’angle

Pour l’instant, ce que je comprends c’est que la variable PID_out  donnera une valeur indiquant de combien ajuster la vitesse des moteurs pour que le robot reste vertical.

Si je fais tourner le sketch NoRobo-Gyro-2016-04-24A.ino, je vois que mon angle mesuré de départ est -2.98° environ. Si je penche le robot vers l’avant, l’angle devient positif. Vers l’arrière, et c’est normal, il reste négatif et prend des valeurs plus grandes.

Je vais considérer que  PID_out est positive lorsque le robot est trop en avant, négative s’il est trop en arrière.

Si MotorPower peut prendre la valeur -255 à +255 (le NoRobo avance lorsque MotorPower est positif) :

Si je définis que MotorPower = MotorPower + PID_out ;  avec le MotorPower initial défini par le joystick :

A l’arrêt, si le robot penche trop vers l’avant, MotorPower prend la valeur de PID_out, positive. Le NoRobo va donc avancer. Parfait !

A l’arrêt, si le robot penche trop vers l’arrière, MotorPower prend la valeur de PID_out, négative. Le NoRobo va donc reculer. Parfait !

Si le robot avance et penche trop vers l’avant, PID_out est positive et MotorPower augmente. Le NoRobo va donc accélérer. Parfait !

Si le robot avance et penche trop vers l’arrière, PID_out est négative et MotorPower diminue. Le NoRobo va donc ralentir. Parfait !

Si le robot recule et penche trop vers l’avant, PID_out est positive et MotorPower va dminuer en valeur absolue. Le NoRobo va donc ralentir son recul. Parfait !

Si le robot recule et penche trop vers l’arrière, PID_out est négative et MotorPower augmenter en valeur absolue. Le NoRobo va donc accélérerson recul. Parfait !

Il faudra donc :

• que MotorPower puisse prendre la valeur -255 à +255. Attention, ce n’est pas le cas car MotorPower est défini comme uint16_t, un entier de 16 bits Unsigned (donc toujours positif…)  ;
• que  PID_out soit positive lorsque le robot est trop en avant, positive sinon ;
• Définir MotorPower en fonction du joystick PUIS
•  MotorPower = MotorPower + PID_out ;
• Faire en sorte que lorsqu’on veut que les moteurs avance, on considère qu’ils ne peuvent pas bouger lorsqu’on leur envoie une commande analogWrite d’une valeur inférieure au minimum de mes moteurs….
• régler direction pour que vers l’avant (true) si .

Sketch et réglage PID

A force d’essayer, longuement, de comprendre la boucle de régulation PID d’un robot à balancier, j’ai compris qu’il y en a deux en cascade :

• 1 boucle qui ne tient compte que de l’angle
• 1 boucle qui tient compte de la position du robot

C’est d’ailleurs expliqué dans un des liens fournis dans le tutoriel qui me sert de base… Cet article explique qu’on régule sur deux éléments. Il explique aussi que le calcul nécessite une base temps constante. Il va sans doute falloir que je remettes les interruptions et que je régle les timers…

Je n’aurai jamais rien compris sans avoir lu cet article sur une boucle PID simple. Le pseudocode est très explicite.

Cet article, avec le code d’un robot à balancier est également intéressant, car le code est bien documenté.

Sur robotic.stackexchange, cet échange parle des boucles PID en cascade, c’est cet article qui m’a fait comprendre qu’il me fallait une cascade !

Self Balancing Robot explique bien comment régler les constantes du PID. Voir aussi le code sur github ici et instructables ici.

Enfin, l’article suivant est plutôt clair.

Je vais reprendre le code relatif au calcul de la régulation PID du tutoriel initial (avec la carte Gimbal). En attendant, il faut que je comprennes comment agir sur les moteurs.

Agir sur les moteurs en fonction de l’angle

Lorsqu’on veut agir sur un moteur à courant continu (DC), on envoie trois commandes :

analogWrite(motorD_enable, power);
digitalWrite(motorD_forward, dir);
digitalWrite(motorD_backward, !dir);

En mode digital, on envoie 0 (false) ou 1(true) aux broches correspondant à motorD-forward et  motorD-backward (broches 7 et 2 du L293D) pour indiquer la direction du mouvement. Dans mon cas, si dir = true, ces commandes font avancer le robot vers l’avant.

En mode analogique, on envoie une valeur comprise entre 0 et 255 à motorD_enable ((broche 1 du L293D, connectée à la broche 9 de l’arduino). On pourrait imaginer que le moteur commence tout doucement à avancer avec des valeurs basses et atteint sa vitesse maximale quand la valeur transmise est 255.

En fait, tous les moteurs ont une zone dans laquelle l’impulsion n’est pas suffisante pour les faire avancer. Dans mon cas, des valeurs comprises entre 0 et 73 n’ont aucun effet !

Déterminer la valeur à partir de laquelle les moteurs bougent

J’ai ajouté un potentiomètre à mon montage, avec les deux broches extérieures connectées au +5V et au GND de l’arduino, et la broche centrale à la broche analogique A0 de l’arduino. Celui ci reçoit alors des valeurs comprises entre 0 et 1023.

Dans le sketch Dc-motor-potentiometer.ino, je convertis ces valeurs reçues en valeurs comprises entre 0 et 255. Il me suffit ensuite de regarder à partir de quelle valeur les moteurs démarrent. C’est comme ça que j’ai déterminé que la valeur la plus faible à envoyer aux moteurs est 73.

Et maintenant, définir la valeur à envoyer au moteur selon la correction PID…

C’était très compliqué…. J’ai créé un sketch utilisant le potentiomètre installé précédemment pour simuler la variation de PID.

J’ai testé ce sketch (NoRobo-proto-potentiometer-PID-B.ino) en définissant MotorPower = 0 (robot à l’arrêt), MotorPower = -100 (le robot recule) et MotorPower = 100 (le robot avance.

Petit à petit, et grâce à l’aide des collègues du FabLab (cf cet article de Fabrikerné) , j’ai réussi à poser le pseudocode correspondant aux calculs à faire selon les cas. J’explique :

schéma de principe pour le calcul de la vitesse (ici 75 correspond à idl_motor_power – en fait c’est 73)

MotorPower peut prendre une valeur comprise entre -255 et +255, selon la position du joystick qui commande le NoRobo. Dans mon sketch prototype, je définis la valeur comme une constante au début du sketch (-100, 0 ou 100 testés).

Dans le cas général, il suffit d’additionner la valeur de PID_out, fournie par le potentiomètre. Mais il y a quelques cas particuliers, et c’est là que ça se corse :

• cas ou speed est supérieur à 255, là c’est simple, on décide alors qu’il est égal à 255 ;
• cas où MotorPower est plutôt faible, mais supérieur à idl_motor_power (75 dans le schéma, 73 en réalité) : si PID_out est très grand, speed va changer de signe et devenir petit, plus faible que idl_motor_power…
• cas oû speed est plus petit que idl_motor_power mais du même signe que MotorPower, il faut que la correction de PID se fasse quand même…

Le pseudocode qui fonctionne correctement :

// définir les variables correctement
int16_t MotorPower = 100	// valeurs -255 à 255
uint8_t idl_motor_power = 73;
int8_t PID_out				// valeurs -127 à 127
bool direction = false ;	// direction du mouvement

int16_t R_Speed = 0;    	// valeurs pour la roue droite - devrait être uint16 (toujours positif)
int16_t L_Speed = 0;

int pot = 0  ; // valeur lue par le potentiomètre


// setup
définir les moteurs comme des sorties
définir le potentiomètre comme une entrée

// boucle

lire la valeur du potentiomètre
La transformer en valeur de PID_out, limitée entre -100 et +100

définir une variable entière temporaire speed (int speed ;)

CAS où MotorPower est dans [-255,-73] ou [+73, +255] 
c'est à dire (MotorPower <= -idl_motor_power) || (MotorPower >= idl_motor_power)
	
	SI MotorPower et PID_out sont de signe opposé (( MotorPower * PID_out ) < 0 ))
		
		SI la valeur absolue de MotorPower est inférieure à celle de PID_out
		c'est à dire abs(MotorPower) < abs(PID_out)
		
			si PID_out est négatif
				speed = MotorPower + PID_out - idl_motor_power  ;
			sinon (PID_out positif)
				speed = MotorPower + PID_out + idl_motor_power  ;
		
		SINON (abs(MotorPower) >= abs(PID_out))
		
			speed = MotorPower + PID_out ;
			
	SINON MotorPower et PID_out sont de même signe
		
		speed = MotorPower + PID_out ;
		
CAS opposé où MotorPower est dans [-73,+73]

	si PID_out est négatif 
		speed = -idl_motor_power + PID_out ;
	sinon (PID_out positif)
		speed = +idl_motor_power + PID_out ; ;
		
A ce stade, on a une valeur juste pour speed mais il faut déterminer le sens de rotation

SI ( speed < - idl_motor_power ) 
    // si speed < 0 le robot va vers l'AR
    direction = false ; 
    speed = abs( speed ) ;  // speed redevient positif
      
SINON SI ( speed > idl_motor_power ) 
      
    // si speed > 0 le robot va vers l'AV
    direction = true ;
      
SINON SI( ( speed >= - idl_motor_power ) && ( speed < 0 )  ) 
    
    speed = 2*idl_motor_power + speed ; 
    direction = true ;    
    
SINON SI ( ( speed <= idl_motor_power ) && ( speed >= 0 )  ) 

    speed = - (speed - 2*idl_motor_power)  ;
    direction = false ;
 

Et enfin, speed ne doit pas excéder 255

SI  speed > 255 
    speed = 255 ;

	
Puis définir la vitesse qui va être envoyée à chacun des deux moteurs
  
R_Speed = speed;
L_Speed = speed;

Et ça fonctionne correctement avec un potentiomètre qui simule les valeurs du PID. Cette phase a été bien plus difficile que ce que je prévoyais…

Et maintenant…

Je vais devoir reprendre le code initial du balancing robot avec gimbal (dans ce tutoriel), qui gère convenablement les timers. Puis, en m’aidant de NoRobo-Gyro-2016-04-24A.ino  :

1. dans le code du balancing robot, éliminer ce qui est spécifique à des moteurs brushless et remplacer par le code pour mes moteurs DC ;
2.  dans le code du balancing robot, lui faire « comprendre » que mon gyroscope n’est pas positionné de la même manière et que le calibrage est réalisé robot « debout ».
3. ajuster la formule du PID pour qu’elle prenne en compte l’angle en Y et pas en Z.
4. Indiquer aux moteurs quelles valeurs ils doivent prendre.
5. tester, ça fonctionnera !

Je ne suis pas sortie de l’auberge… La suite dans le prochain article de cette série Un robot arduino !