Algorithme Pid - Programmation Robot

velociste
->PID

Le PID (contrôle proportionnel, intégral et dérivatif) est un mécanisme de contrôle par rétroaction qui calcule
la déviation ou l'erreur entre une valeur mesurée et la valeur que l'on souhaite obtenir (point de consigne, position cible ou point
de consigne), pour appliquer une action corrective qui ajuste le processus.

Dans le cas du robot sprinteur, le contrôleur PID, (qui est une routine basée mathématiquement), traitera les
données du capteur, et il les utilise pour contrôler la direction (vitesse de chaque moteur), pour cela
façon de le maintenir en cours.

- Erreur - Nous appelons la différence entre la position cible et la position mesurée de l'erreur. (à quelle distance de
Le point de consigne se trouve le capteur, dans notre cas l'objectif est d'avoir les capteurs centrés)
-Point de consigne ou Position cible- Lorsque l'erreur est 0 (zéro). Dans le cas du robot sprinteur, c'est 3500, l'idée est
toujours le garder en ligne, ou dans le cas des capteurs, le garder centré et ainsi ne pas se
j'ai réussi à sortir de la ligne.

->PARAMETROS:

-Proportionnel : C'est la réponse à l'erreur qui doit être fournie immédiatement, c'est-à-dire, si nous
nous trouvons au centre de la ligne, les moteurs, auront en réponse une vitesse de même valeur, si nous
En nous éloignant du centre, l'un des moteurs réduira sa vitesse et l'autre augmentera.
Proportionnel=(position) -point_de_consigne

L'intégrale est la somme des erreurs accumulées, son objectif est de diminuer et d'éliminer.
el error en estado estacionario provocado por el modo proporcional, en otras palabras, si el robot velocista se
trouve beaucoup de temps éloigné du centre (cela arrive souvent lorsqu'il se trouve dans des virages), l'action
 l'intégral va s'accumulant et va diminuant l'erreur jusqu'à atteindre le point de consigne,
Intégral = Intégral + proportional_passé

Dérivé : C'est la dérivée de l'erreur, sa fonction est de maintenir l'erreur au minimum, en la corrigeant.
proportionnellement à la même vitesse qu'il se produit, de cette manière il évite que l'erreur s'accroisse,
En d'autres termes, cela anticipera l'action évitant ainsi les oscillations excessives.
Derivativo=proporcional-proporcional_pasado

->CONSTANTES:

Facteur (Kp) - C'est une valeur constante utilisée pour augmenter ou réduire l'impact Proportionnel. Si la valeur
c'est excessif, le robot répondra de manière instable, oscillant de manière excessive. Si la valeur est très
petit, le robot répondra très lentement, ayant tendance à sortir des virages

Facteur (Ki) - C'est une valeur constante utilisée pour augmenter ou réduire l'impact de l'Intégrale, La valeur
Un excès de cela provoquera des oscillations excessives, une valeur trop basse ne causera aucun impact.

Facteur (Kd) - C'est une valeur constante utilisée pour augmenter ou diminuer l'impact de la dérivée. Une valeur
Un excès provoquera une sur-amortissement, entraînant une instabilité.

Salida_pwm = ( proporcional * Kp ) + ( derivativo * Kd ) + (integral*Ki);

SYNCHRONISATION PID

Voici le défi, le réglage PID, c'est ici qu'il faudra rechercher les constantes qui correspondent à
les caractéristiques physiques du robot, la façon la plus simple de le faire est par essais et erreurs, jusqu'à obtenir la valeur
désiré. Voici quelques étapes qui aideront beaucoup à rechercher ces constantes :

1. Commencez par Kp, Ki et Kd égalant 0 et travaillez d'abord avec Kp. Essayez de fixer Kp à une valeur de 1 et
observer le robot. L'objectif est de faire en sorte que le robot suive la ligne, même si elle est très instable. Si le robot
va au-delà et perd la ligne, réduisez la valeur de Kp. Si le robot ne peut pas naviguer une fois, ou semble
lenta, augmente la valeur Kp.
2. Una vez que el robot es capaz de seguir un poco la línea, asignar un valor de 1 a Kd .Intente aumentar
ce valeur jusqu'à ce que je voie moins d'oscillations.
3. Une fois que le robot est assez stable sur la ligne suivante, attribuez une valeur de 0,5 à 1,0 à Ki. Si la valeur
Si la Ki est trop élevée, le robot se secouera rapidement à gauche et à droite. Si elle est trop basse, il ne le fera pas.
Il n'y aura aucune différence perceptible. L'intégrale est cumulative donc la valeur Ki a un impact.
significatif. il peut se terminer par des ajustements de 0,01.
4. Une fois que le robot suit la ligne avec une bonne précision, on peut augmenter la vitesse et
voir s'il est encore capable de suivre la ligne. La vitesse affecte le contrôleur PID et nécessitera une nouvelle harmonisation
comme les changements de vitesse.

ALGORITHME EN ARDUINO

?
1 #include <QTRSensors.h>
2 ///////////////////////////////////////////////////////////////////////////////////
//************* Programa realizado por MARCO A. CABALLERO MORENO***************
3 // Seulement à des fins éducatives
4 // robot velocista mini FK BOT V 2.0 //
5 // micro moteurs pololu MP 10:1, capteurs qtr 8rc, pilote drv8833, arduino nano
6 // 05/12/2014
7 // MIS À JOUR 29/3/2015
///////////////////////////////////////////////////////////////////////////////////
8
9 #define NUM_SENSORS 8 //numéro de capteurs utilisés
10 #define TEMPS_ATTENTE 2000 // temps d'attente pour donner le résultat aux États-Unis
11 #define EMITTER_PIN 6 //allumer la LED
12 ///////////////pines arduino à utiliser/////////////////////
13 #define led1 13
#define led2 4
14 #define mot_i 7
15 #define mot_d 8
16 #définir capteurs 6
17 #define bouton_1 2 //pin pour bouton
#define pin_pwm_i 9
18 #define pin_pwm_d 10
19
20 QTRSensorsRC qtrrc((unsigned char[]) {19, 18, 17, 16, 15, 14, 11, 12}
21 ,NUM_SENSORS, TIMEOUT, EMITTER_PIN;
22
23 //variables pour stocker les valeurs des capteurs et la position
24 unsigned int sensorValues[NUM_SENSORS];
unsigned int position=0;
25
26 /// variables pour le pid
27 int derivatif=0, proportionnel=0, intégral=0; //paramètres
28 int sortie_pwm=0, proportionnel_passé=0;
29
30 ///////////////ICI NOUS CHANGERONS LES PARAMÈTRES DE NOTRE ROBOT!!!!!!!!!!!!!!!
31 intvelocidad=120; //variable pour la vitesse, le maximum est 255
32 floatKp=0.18, Kd=4, Ki=0.001; //constantes
33 //variable pour choisir le type de ligne
intlinea=0; // 0 pour lignes noires, 1 pour lignes blanches
34
35 voidsetup()
36 {
37 retard(800);
38 pinMode(mot_i, SORTIE);//pin de direction moteur gauche
pinMode(mot_d, OUTPUT);//pin de direction moteur droit
39 pinMode(led1, SORTIE); //led1
40 pinMode(led2, SORTIE); //led2
41 pinMode(bouton_1, INPUT); //bouton 1 comme pull up
42
43 pour(int i = 0; i < 40; i++) //calibrage pendant 2,5 secondes,
44 { //pour calibrer, il est nécessaire de placer les capteurs
superficie noire et ensuite
45 digitalWrite(led1, HIGH); //la blanche
46 retard(20);
47 [Link](); // fonction pour calibrer les capteurs
48 digitalWrite(led1, FAIBLE);
49 retard(20);
}
50 digitalWrite(led1, LOW); //éteindre les capteurs pour indiquer la fin de la calibration
51 retard(400);
52 digitalWrite(led2,HIGH); //allumer led 2 pour indiquer le
53 tant que(vrai)
{
54 intx=digitalRead(boton_1); //nous lisons et enregistrons la valeur
55 // del bouton en variable x
56 retard(100);
57 si(x==0) //si le bouton est pressé
58 {
59 digitalWrite(led2,LOW); // nous indiquons que le bouton a été pressé
60 digitalWrite(led1,HIGH); //allumer led 1
retard(100);
61 break; //nous sautons vers la boucle principale
62 }
63 }
64 }
65
66
bouclevide()
67 {
68
69 //pid(0, 120, 0.18, 0.001, 4 );
70 pid(ligne,vitesse,Kp,Ki,Kd); //fonction pour algorithme pid
71 //premier paramètre : 0 pour les lignes noires, type 1 p
72 //segundo parametro: velocidad pwm de 0 a 255
//tercer parametro: constante para accion proporci
73 //cuarto parametro: constante para accion integral
74 //cinquième paramètre : constante pour action dérivée
75 //frenos_contorno(0,700);
frenos_contorno(ligne, 700); // fonction pour le freinage dans les courbes de type
76
//premier paramètre :0 pour lignes noires, type 1 pour
77 //deuxième paramètre : flanc de comparaison va de 0
78 pour voir
79 }
80
81 ////////fonctions pour le contrôle du robot////
82 voidpid(intligne, intvitesse, floatKp, floatKi, floatKd)
{
83 position = [Link](sensorValues, QTR_EMITTERS_ON, linea); //0 pour ligne n
84 blanche
85proporcional = (position) - 3500; // set point es 3500, asi obtenemos el error
86 intégral=intégral + proporcional_pasado; //obtenant l'intégrale
87 derivativo = (proporcional - proporcional_pasado); //obteniedo el derivativo
if(integral>1000) integral=1000; //nous limitons l'intégrale pour éviter des problèmes
88 si(integral<-1000) intégral=-1000;
89 salida_pwm =( proporcional * Kp ) + ( derivativo * Kd )+(integral*Ki);
90
91 si(salida_pwm > velocidad ) salida_pwm = velocidad; //limitons la sortie de p
92 si(salida_pwm < -vitesse ) salida_pwm = -vitesse;
93
si(salida_pwm < 0)
94
{
95 moteurs(velocité+sortie_pwm, velocité);
96 }
97 si(salida_pwm > 0)
98 {
moteurs(vitesse, vitesse-sortie_pwm);
99 }
100
101 proporcional_pasado = proporcional;
102 }
103
104 //fonction pour le contrôle des moteurs
105 voidmoteurs(int moteur_gauche, int moteur_droit)
106 {
107 if(moteur_izq >= 0) //moteur gauche
{
108 digitalWrite(mot_i,HIGH); // con haut avance
109 analogWrite(pin_pwm_i,255-motor_izq); //se contrôle de manière
110 inversa pour un meilleur contrôle
111 }
sinon
112 {
113 digitalWrite(mot_i,LOW); // avec low, reculer
114 motor_izq = motor_izq * (-1); //changement de signe
115 analogWrite(pin_pwm_i,moteur_izq);
116 }
117
si(moteur_droit >= 0 ) //moteur droit
118 {
119 digitalWrite(mot_d,HIGH);
120 analogWrite(pin_pwm_d,255-moteur_der);
121 }
sinon
122 {
123 digitalWrite(mot_d,LOW);
124 moteur_der = moteur_der * (-1);
125 analogWrite(pin_pwm_d,motor_der);
126 }
127
128
}
129
130 videfreins_contour(inttype, intflanc_comparaison)
131 {
132
133 si(tipo==0)
134 {
135 si(position<=50) //si s'est sorti par la partie droite de la ligne
{
136 moteurs(-80,90); //en raison de l'inertie, le moteur
137 //tendera à continuer à tourner
138 //c'est pourquoi nous reculons, pour qu'il freine
139 // le plus rapidement possible
tant que(true)
140
{
141 [Link](sensorValues); //lecture brute du capteur
142 si (valeursCapteur[0] > flanco_comparacion || valeursCapteur[1] > flanco_comparacion)
143 //s'assurer qu'il est en ligne
144 {
pause;
145 }
146 }
147 }
148
149 if(position>=6550) //si on sort par le côté gauche de la ligne
150 {
moteurs(90,-80);
151 tandis que (vrai)
152 {
153 [Link](valeursCapteur);
154 si(sensorValues[7]>flanco_comparacion || sensorValues[6]>flanco_comparacion)
{
155 pause;
156 }
157 }
158 }
}
159
160 si(tipo==1) //pour ligne blanche avec fond noir
161 {
162 si(position<=50) //si s'est sorti par la partie droite de la ligne
163 {
moteurs(-80,90); //en raison de l'inertie, le moteur
164
//tendra à suivre en tournant
165 //c'est pourquoi nous revenons en arrière
166 pour qu'il freine le plus rapidement possible
167 tant que(vrai)
168 {
[Link](sensorValues); //lecture brute du capteur
169 si(sensorValues[0]<flanco_comparacion || sensorValues[1]<flanco_comparacion )
170 est en ligne
171 {
172 pause;
173 }
}
174 }
175
176 si(position>=6550) //si s'est échappé par le côté gauche de la ligne
177{
178 moteurs(90,-80);
179 tant que(vrai)
{
180 [Link](valeursCapteur);
181 si(sensorValues[7]<flanco_comparacion || sensorValues[6]<flanco_comparacion)
182 {
183 pause;
}
184 }
185 }
186 }
187 }
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
SCHÉMA ROBOT VELOCISTE :

[Link]
[Link]
ALGORITHME EN ARDUINO

?
#include <QTRSensors.h>
1 ///////////////////////////////////////////////////////////////////////////////////
2 //************* Programa realizado por MARCO A. CABALLERO MORENO***************
3// Uniquement à des fins éducatives
4 // robot velocista mini FK BOT V 2.0 //
5 // micro moteurs pololu MP 10:1, capteurs qtr 8rc, driver drv8833, arduino nano
// 05/12/2014
6 // MIS À JOUR 29/3/2015
7 ///////////////////////////////////////////////////////////////////////////////////
8
9 #define NUM_SENSORS 8 //numéro de capteurs utilisés
10 #define DÉLAI 2000 // tiempo de espera para dar resultado en uS
#define EMITTER_PIN 6 // allumer la LED
11 ///////////////pines arduino à utiliser/////////////////////
12 #définir led1 13
13 #define led2 4
14 #define mot_i 7
15 #define mot_d 8
#define capteurs 6
16 #define bouton_1 2 //pin pour bouton
17 #define pin_pwm_i 9
18 #define pin_pwm_d 10
19
20 QTRSensorsRC qtrrc((unsigned char[]) {19, 18, 17, 16, 15, 14, 11, 12}
,NOMBRE_DE_CAPTEURS, TEMPS_D’ATTENTE, BROCHE_EMETTEUR);
21
22 //variables pour stocker les valeurs des capteurs et la position
23 unsigned int sensorValues[NUM_SENSORS];
24 unsigned int position = 0;
25
26 /// variables pour le pid
27 int derivatif=0, proportionnel=0, intégral=0; //paramètres
28 int salida_pwm=0, proporcional_pasado=0;
29
30 ///////////////ICI NOUS CHANGERONS LES PARAMÈTRES DE NOTRE ROBOT!!!!!!!!!!!!!!!
intvelocidad=120; //variable pour la vitesse, le maximum est 255
31 floatKp=0.18, Kd=4, Ki=0.001; //constantes
32 //variable para escoger el tipo de linea
33 intlinea=0; // 0 pour les lignes noires, 1 pour les lignes blanches
34
35 voidsetup()
36 {
retard(800);
37 pinMode(mot_i, OUTPUT);//pin de direction moteur gauche
38 pinMode(mot_d, OUTPUT);//broche de direction moteur droit
39pinMode(led1, SORTIE); //led1
40 pinMode(led2, SORTIE); //led2
pinMode(boton_1, INPUT); //bouton 1 en pull up
41
42
pour(int i = 0; i < 40; i++) //calibrage pendant 2,5 secondes,
43 { //pour calibrer, il est nécessaire de placer les capteurs
44 superficie noire et ensuite
45 digitalWrite(led1, HIGH); //la blanche
retard(20);
46 [Link](); // fonction pour calibrer les capteurs
47 digitalWrite(led1, BAS);
48 retard(20);
49 }
digitalWrite(led1, LOW); //éteindre les capteurs pour indiquer la fin de la calibration
50 retard(400);
51 digitalWrite(led2,HIGH); //allumer led 2 pour indiquer la
52 tant que(vrai)
53 {
54 intx=digitalRead(boton_1); //nous lisons et stockons la valeur
// del bouton en variable x
55 retard(100);
56 si(x==0) //si le bouton est pressé
57 {
58 digitalWrite(led2,LOW); //nous indiquons que le bouton a été pressé
digitalWrite(led1,HIGH); //allumer led 1
59 retard(100);
60 break; //nous sautons vers la boucle principale
61 }
62 }
63 }
64
65
bouclevide()
66 {
67
68 //pid(0, 120, 0.18, 0.001, 4 );
69pid(ligne,vitesse,Kp,Ki,Kd); //fonction pour algorithme pid
70 //premier paramètre : 0 pour lignes noires, type 1 p
71 //deuxième paramètre : vitesse pwm de 0 à 255
//troisième paramètre : constante pour action proportionnelle
72 //quatrième paramètre : constante pour action intégrale
73 //quinto paramètre : constante pour action dérivée
74 //frenos_contorno(0,700);
frenos_contorno(linea,700); //fonction pour le freinage dans les courbes de type
75
//premier paramètre :0 pour lignes noires, type 1 pour
76 //deuxième paramètre : flanc de comparaison va de 0
77 pour voir
78 }
79
80 ////////fonctions pour le contrôle du robot////
81 voidpid(int ligne, int vitesse, float Kp, float Ki, float Kd)
{
82 position = [Link](sensorValues, QTR_EMITTERS_ON, linea); //0 para linea n
83 blanc
84 proporcional = (position) - 3500; // set point es 3500, asi obtenemos el error
85 integral=integral + proporcional_pasado; //obtenant l'intégrale
derivativo = (proporcional - proporcional_pasado); //obteniedo el derivativo
86 if(integral>1000) integral=1000; //nous limitons l'intégrale pour éviter des problèmes
87 si(integral<-1000) integral=-1000;
88 salida_pwm =( proporcional * Kp ) + ( derivativo * Kd )+(integral*Ki);
89
90 if(salida_pwm > velocidad ) salida_pwm = velocidad; //nous limitons la sortie de p
91 si(salida_pwm < -vitesse) salida_pwm = -vitesse;
92
93 si(salida_pwm < 0)
{
94 moteurs(vitesse+sortie_pwm, vitesse);
95 }
96 si(salida_pwm >0)
97 {
motores(velocidad, velocidad-salida_pwm);
98 }
99
100 proporcional_pasado = proporcional;
101 }
102
103 //fonction pour le contrôle des moteurs
104 voidmoteurs(int moteur_izq, int moteur_der)
{
105 si(moteur_izq >= 0 ) //moteur gauche
106 {
107digitalWrite(mot_i,HIGH); // en haut avance
108 analogWrite(pin_pwm_i, 255 - motor_izq); // on contrôle de manière
//inverser pour un meilleur contrôle
109
}
110 sinon
111 {
112 digitalWrite(mot_i,LOW); //avec low, recule
113 motor_izq = motor_izq * (-1); //changement de signe
analogWrite(pin_pwm_i,moteur_izq);
114 }
115
116 si(moteur_droite >= 0 ) //moteur droit
117 {
118 digitalWrite(mot_d,HIGH);
119 analogWrite(pin_pwm_d,255-motor_der);
}
120 sinon
121 {
122 digitalWrite(mot_d,BAS);
123 moteur_der= moteur_der*(-1);
analogWrite(pin_pwm_d,moteur_der);
124 }
125
126
127 }
128
129 videfreins_contour(int type, int flanc_comparaison)
130 {
131
132 si(tipo==0)
133 { si(position<=50) //si s'est échappé par le côté droit de la ligne
134 {
135 moteurs(-80,90); //en raison de l'inertie, le moteur
136 //tendra à continuer à tourner
137 //c'est pourquoi nous reculi, pour qu'il freine
138 le plus rapidement possible
139 tant que (vrai)
{
140 [Link](valeursDuCapteur); // lecture brute du capteur
141 si( sensorValues[0]>flanco_comparacion || sensorValues[1]>flanco_comparacion )
142 //s'assurer qu'il est en ligne
143 {
pause;
144 }
145 }
146 }
147
148 si (position >= 6550) // si elle est sortie par le côté gauche de la ligne
149 {
moteurs(90,-80);
150 tandis que(true)
151 {
152 [Link](valeursCapteurs);
153 si(sensorValues[7]>flanco_comparacion || sensorValues[6]>flanco_comparacion )
{
154 pause;
155 }
156 }
157 }
158 }
159
si(tipo==1) //pour ligne blanche sur fond noir
160 {
161 si(position<=50) //si elle est sortie par le côté droit de la ligne
162{
163 moteurs(-80,90); // en raison de l'inertie, le moteur
164 tendra à continuer à tourner
//c'est pourquoi nous faisons marche arrière
165 //pour qu'il freine le plus rapidement possible
166 tant que(vrai)
167 {
168 [Link](sensorValues); // lecture brute du capteur
si(sensorValues[0]<flanco_comparacion || sensorValues[1]<flanco_comparacion )
169 est en ligne
170 {
171 pause;
172 }
173 }
}
174
175 si(position>=6550) //si s'est échappé par la partie gauche de la ligne
176 {
177 moteurs(90,-80);
178 while(true)
{
179 [Link](valeursCapteurs);
180 si(sensorValues[7]<flanco_comparacion || sensorValues[6]<flanco_comparacion)
181 {
182 faire une pause;
183 }
}
184
185 }
186 }
}
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217