Algoritmo Pid - Programacion Robot

velocista
->PID

El PID (control proporcional, integral y derivativo) es un mecanismo de control por realimentacion que calcula
la desviacion o error entre un valor medido y el valor que se quiere obtener (set point, target position o punto
de consigna), para aplicar una acción correctora que ajuste el proceso.

En el caso del robot velocista, el controlador PID ,(que es una rutina basada matematicamente), procesara los
datos del sensor, y lo utiliza para controlar la dirección (velocidad de cada motor), para de esta
forma mantenerlo en curso.

- Error - Llamamos a la diferencia entre la posición objetivo y la posición medida del error. (que tan lejos del
punto de consigna se encuentra el sensor, en nuestro caso el objetivo es tener los sensores centrados)
-Set point o Target Position - Cuando el error es 0 (cero). En el caso del robot velocista es 3500, la idea es
siempre mantenerlo en la la linea, o lo que es el caso de los sensores, mantenerlo centrado y asi no se
llegue a salir de la linea.

->PARAMETROS:

-Proporcional: Es la respuesta al error que se tiene que entregar de manera inmediata, es decir, si nos
encontramos en el centro de la linea, los motores , tendran en respuesta una velocidad de igual valor, si nos
alejamos del centro, uno de los motores reducira su velocidad y el otro aumentara.
Proporcional=(posición) -punto_consigna

-Integral: La integral es la sumatoria de los errores acumulados, tiene como propósito el disminuir y eliminar
el error en estado estacionario provocado por el modo proporcional, en otras palabras, si el robot velocista se
encuentra mucho tiempo alejado del centro (ocurre muchas veces cuando se encuentra en curvas), la accion
 integral se ira acumulando e ira disminuyendo el error hasta llegar al punto de consigna,
Integral=Integral + proporcional_pasado

-Derivativo: Es la derivada del error, su funcion es mantener el error al minimo, corrigiendolo

->CONSTANTES:

Factor (Kp) - Es un valor constante utilizado para aumentar o reducir el impacto de Proporcional. Si el valor
es excesivo, el robot tendera responder inestablemente, oscilando excesivamente. Si el valor es muy
pequeño, el robot respondera muy lentamente, tendiendo a salirse de las curvas

Factor (Ki) - Es un valor constante utilizado para aumentar o reducir el impacto de la Integral, El valor
excesivo de este provocara oscilaciones excesivas, Un valor demasiado bajo no causara impacto alguno.

Factor (Kd) - Es un valor constante utilizado para aumentar o reducir el impacto de la Derivada. Un valor
excesivo provocara una sobre amortiguacion. provocando inestabilidad.

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

->SINTONIZACION PID

Aqui viene el reto, la sintonizacion pid, es aqui donde se tendra que buscar las constantes que correspondan a
las caracteristicas fisicas del robot, la forma mas facil de hacerlo es por ensayo y error, hasta obtener el valor
deseado. Aqui hay unos pasos que ayudaran mucho a buscar esas constantes:

1. Comience con Kp, Ki y Kd igualando 0 y trabajar con Kp primero. Pruebe establecer Kp a un valor de 1 y
observar el robot. El objetivo es conseguir que el robot siga la línea, incluso si es muy inestable. Si el robot
llega más allá y pierde la línea, reducir el valor de Kp. Si el robot no puede navegar por una vez, o parece
lenta, aumente el valor 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
este valor hasta que vea menos oscilaciones.
3. Una vez que el robot es bastante estable en la línea siguiente, asigne un valor de 0,5 a 1,0 a Ki. Si el valor
de Ki es demasiado alta, el robot se sacudirá izquierda y derecha rápidamente. Si es demasiado baja, no
se vera ninguna diferencia perceptible. El Integral es acumulativo por lo tanto el valor Ki tiene un impacto
significativo. puede terminar ajustando por 0,01 incrementos.
4. Una vez que el robot está siguiendo la línea con una buena precisión, se puede aumentar la velocidad y
ver si todavía es capaz de seguir la línea. La velocidad afecta el controlador PID y requerirá resintonizar
como los cambios de velocidad.

->ALGORITMO EN ARDUINO

?
1 #include <QTRSensors.h>
2 ///////////////////////////////////////////////////////////////////////////////////
//************* Programa realizado por MARCO A. CABALLERO MORENO***************
3 // Solo para fines educativos
4 // robot velocista mini FK BOT V 2.0 //
5 // micro motores pololu MP 10:1, sensores qtr 8rc, driver drv8833, arduino nano
6 // 05/12/2014
7 // ACTUALIZADO 29/3/2015
///////////////////////////////////////////////////////////////////////////////////
8
9 #define NUM_SENSORS 8 //numero de sensores usados
10 #define TIMEOUT 2000 // tiempo de espera para dar resultado en uS
11 #define EMITTER_PIN 6 //pin led on
12 ///////////////pines arduino a utilizar/////////////////////
13 #define led1 13
#define led2 4
14 #define mot_i 7
15 #define mot_d 8
16 #define sensores 6
17 #define boton_1 2 //pin para boton
#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 para almacenar valores de sensores y posicion
24 unsigned int sensorValues[NUM_SENSORS];
unsigned int position=0;
25
26 /// variables para el pid
27 int derivativo=0, proporcional=0, integral=0; //parametros
28 int salida_pwm=0, proporcional_pasado=0;
29
30 ///////////////AQUI CAMBIEREMOS LOS PARAMETROS DE NUESTRO ROBOT!!!!!!!!!!!!!!!
31 int velocidad=120; //variable para la velocidad, el maximo es 255
32 float Kp=0.18, Kd=4, Ki=0.001; //constantes
33 //variable para escoger el tipo de linea
int linea=0; // 0 para lineas negra, 1 para lineas blancas
34
35 void setup()
36 {
37 delay(800);
38 pinMode(mot_i, OUTPUT);//pin de direccion motor izquierdo
pinMode(mot_d, OUTPUT);//pin de direccion motor derecho
39 pinMode(led1, OUTPUT); //led1
40 pinMode(led2, OUTPUT); //led2
41 pinMode(boton_1, INPUT); //boton 1 como pull up
42
43 for (int i = 0; i <40; i++) //calibracion durante 2.5 segundos,
44 { //para calibrar es necesario colocar los sensore
superficie negra y luego
45 digitalWrite(led1, HIGH); //la blanca
46 delay(20);
47 qtrrc.calibrate(); //funcion para calibrar sensores
48 digitalWrite(led1, LOW);
49 delay(20);
}
50 digitalWrite(led1, LOW); //apagar sensores para indicar fin de calibracion
51 delay(400);
52 digitalWrite(led2,HIGH); //encender led 2 para indicar la
53 while(true)
{
54 int x=digitalRead(boton_1); //leemos y guardamos el valor
55 // del boton en variable x
56 delay(100);
57 if(x==0) //si se presiona boton
58 {
59 digitalWrite(led2,LOW); //indicamos que se presiono boton
60 digitalWrite(led1,HIGH); //encendiendo led 1
delay(100);
61 break; //saltamos hacia el bucle principal
62 }
63 }
64 }
65
66
void loop()
67 {
68
69 //pid(0, 120, 0.18, 0.001, 4 );
70 pid(linea,velocidad,Kp,Ki,Kd); //funcion para algoritmo pid
71 //primer parametro: 0 para lineas negras, tipo 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 //quinto parametro: constante para accion derivati
75 //frenos_contorno(0,700);
76 frenos_contorno(linea,700); //funcion para frenado en curvas tipo
//primer parametro :0 para lineas negras, tipo 1 para
77 //segundo parametro:flanco de comparación va desde 0
78 para ver
79 }
80
81 ////////funciones para el control del robot////
82 void pid(int linea, int velocidad, float Kp, float Ki, float Kd)
{
83 position = qtrrc.readLine(sensorValues, QTR_EMITTERS_ON, linea); //0 para linea n
84 blanca
85 proporcional = (position) - 3500; // set point es 3500, asi obtenemos el error
86 integral=integral + proporcional_pasado; //obteniendo integral
87 derivativo = (proporcional - proporcional_pasado); //obteniedo el derivativo
if (integral>1000) integral=1000; //limitamos la integral para no causar problemas
88 if (integral<-1000) integral=-1000;
89 salida_pwm =( proporcional * Kp ) + ( derivativo * Kd )+(integral*Ki);
90
91 if ( salida_pwm > velocidad ) salida_pwm = velocidad; //limitamos la salida de p
92 if ( salida_pwm < -velocidad ) salida_pwm = -velocidad;
93
if (salida_pwm < 0)
94
{
95 motores(velocidad+salida_pwm, velocidad);
96 }
97 if (salida_pwm >0)
98 {
motores(velocidad, velocidad-salida_pwm);
99 }
100
101 proporcional_pasado = proporcional;
102 }
103
104 //funcion para control de motores
105 void motores(int motor_izq, int motor_der)
106 {
107 if ( motor_izq >= 0 ) //motor izquierdo
{
108 digitalWrite(mot_i,HIGH); // con high avanza
109 analogWrite(pin_pwm_i,255-motor_izq); //se controla de manera
110 //inversa para mayor control
111 }
else
112 {
113 digitalWrite(mot_i,LOW); //con low retrocede
114 motor_izq = motor_izq*(-1); //cambio de signo
115 analogWrite(pin_pwm_i,motor_izq);
116 }
117
if ( motor_der >= 0 ) //motor derecho
118 {
119 digitalWrite(mot_d,HIGH);
120 analogWrite(pin_pwm_d,255-motor_der);
121 }
else
122 {
123 digitalWrite(mot_d,LOW);
124 motor_der= motor_der*(-1);
125 analogWrite(pin_pwm_d,motor_der);
126 }
127
128
}
129
130 void frenos_contorno(int tipo,int flanco_comparacion)
131 {
132
133 if(tipo==0)
134 {
135 if(position<=50) //si se salio por la parte derecha de la linea
{
136 motores(-80,90); //debido a la inercia, el motor
137 //tendera a seguri girando
138 //por eso le damos para atras , para que frene
139 // lo mas rapido posible
while(true)
140
{
141 qtrrc.read(sensorValues); //lectura en bruto de sensor
142 if( sensorValues[0]>flanco_comparacion || sensorValues[1]>flanco_comparacion )
143 //asegurar que esta en linea
144 {
break;
145 }
146 }
147 }
148
149 if (position>=6550) //si se salio por la parte izquierda de la linea
150 {
motores(90,-80);
151 while(true)
152 {
153 qtrrc.read(sensorValues);
154 if(sensorValues[7]>flanco_comparacion || sensorValues[6]>flanco_comparacion )
{
155 break;
156 }
157 }
158 }
}
159
160 if(tipo==1) //para linea blanca con fondo negro
161 {
162 if(position<=50) //si se salio por la parte derecha de la linea
163 {
164 motores(-80,90); //debido a la inercia el motor
//tendera a seguri girando
165 //por eso le damos para atras
166 //para que frene lo mas rapido posible
167 while(true)
168 {
qtrrc.read(sensorValues); //lectura en bruto de sensor
169 if(sensorValues[0]<flanco_comparacion || sensorValues[1]<flanco_comparacion )
170 esta en linea
171 {
172 break;
173 }
}
174 }
175
176 if(position>=6550) //si se salio por la parte izquierda de la linea
177 {
178 motores(90,-80);
179 while(true)
{
180 qtrrc.read(sensorValues);
181 if(sensorValues[7]<flanco_comparacion || sensorValues[6]<flanco_comparacion)
182 {
183 break;
}
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
ESQUEMA ROBOT VELOCISTA:

http://aprendiendofacilelectronica.blogspot.pe/2014/12/robot-velocista-de-competencia-
parte.html
ALGORITMO EN ARDUINO

?
#include <QTRSensors.h>
1 ///////////////////////////////////////////////////////////////////////////////////
2 //************* Programa realizado por MARCO A. CABALLERO MORENO***************
3 // Solo para fines educativos
4 // robot velocista mini FK BOT V 2.0 //
5 // micro motores pololu MP 10:1, sensores qtr 8rc, driver drv8833, arduino nano
// 05/12/2014
6 // ACTUALIZADO 29/3/2015
7 ///////////////////////////////////////////////////////////////////////////////////
8
9 #define NUM_SENSORS 8 //numero de sensores usados
10 #define TIMEOUT 2000 // tiempo de espera para dar resultado en uS
#define EMITTER_PIN 6 //pin led on
11 ///////////////pines arduino a utilizar/////////////////////
12 #define led1 13
13 #define led2 4
14 #define mot_i 7
15 #define mot_d 8
#define sensores 6
16 #define boton_1 2 //pin para boton
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}
,NUM_SENSORS, TIMEOUT, EMITTER_PIN);
21
22 //variables para almacenar valores de sensores y posicion
23 unsigned int sensorValues[NUM_SENSORS];
24 unsigned int position=0;
25
26 /// variables para el pid
27 int derivativo=0, proporcional=0, integral=0; //parametros
28 int salida_pwm=0, proporcional_pasado=0;
29
30 ///////////////AQUI CAMBIEREMOS LOS PARAMETROS DE NUESTRO ROBOT!!!!!!!!!!!!!!!
int velocidad=120; //variable para la velocidad, el maximo es 255
31 float Kp=0.18, Kd=4, Ki=0.001; //constantes
32 //variable para escoger el tipo de linea
33 int linea=0; // 0 para lineas negra, 1 para lineas blancas
34
35 void setup()
36 {
delay(800);
37 pinMode(mot_i, OUTPUT);//pin de direccion motor izquierdo
38 pinMode(mot_d, OUTPUT);//pin de direccion motor derecho
39 pinMode(led1, OUTPUT); //led1
40 pinMode(led2, OUTPUT); //led2
pinMode(boton_1, INPUT); //boton 1 como pull up
41
42
for (int i = 0; i <40; i++) //calibracion durante 2.5 segundos,
43 { //para calibrar es necesario colocar los sensore
44 superficie negra y luego
45 digitalWrite(led1, HIGH); //la blanca
delay(20);
46 qtrrc.calibrate(); //funcion para calibrar sensores
47 digitalWrite(led1, LOW);
48 delay(20);
49 }
digitalWrite(led1, LOW); //apagar sensores para indicar fin de calibracion
50 delay(400);
51 digitalWrite(led2,HIGH); //encender led 2 para indicar la
52 while(true)
53 {
54 int x=digitalRead(boton_1); //leemos y guardamos el valor
// del boton en variable x
55 delay(100);
56 if(x==0) //si se presiona boton
57 {
58 digitalWrite(led2,LOW); //indicamos que se presiono boton
digitalWrite(led1,HIGH); //encendiendo led 1
59 delay(100);
60 break; //saltamos hacia el bucle principal
61 }
62 }
63 }
64
65
void loop()
66 {
67
68 //pid(0, 120, 0.18, 0.001, 4 );
69 pid(linea,velocidad,Kp,Ki,Kd); //funcion para algoritmo pid
70 //primer parametro: 0 para lineas negras, tipo 1 p
71 //segundo parametro: velocidad pwm de 0 a 255
//tercer parametro: constante para accion proporci
72 //cuarto parametro: constante para accion integral
73 //quinto parametro: constante para accion derivati
74 //frenos_contorno(0,700);
75 frenos_contorno(linea,700); //funcion para frenado en curvas tipo
//primer parametro :0 para lineas negras, tipo 1 para
76 //segundo parametro:flanco de comparación va desde 0
77 para ver
78 }
79
80 ////////funciones para el control del robot////
81 void pid(int linea, int velocidad, float Kp, float Ki, float Kd)
{
82 position = qtrrc.readLine(sensorValues, QTR_EMITTERS_ON, linea); //0 para linea n
83 blanca
84 proporcional = (position) - 3500; // set point es 3500, asi obtenemos el error
85 integral=integral + proporcional_pasado; //obteniendo integral
derivativo = (proporcional - proporcional_pasado); //obteniedo el derivativo
86 if (integral>1000) integral=1000; //limitamos la integral para no causar problemas
87 if (integral<-1000) integral=-1000;
88 salida_pwm =( proporcional * Kp ) + ( derivativo * Kd )+(integral*Ki);
89
90 if ( salida_pwm > velocidad ) salida_pwm = velocidad; //limitamos la salida de p
91 if ( salida_pwm < -velocidad ) salida_pwm = -velocidad;
92
93 if (salida_pwm < 0)
{
94 motores(velocidad+salida_pwm, velocidad);
95 }
96 if (salida_pwm >0)
97 {
motores(velocidad, velocidad-salida_pwm);
98 }
99
100 proporcional_pasado = proporcional;
101 }
102
103 //funcion para control de motores
104 void motores(int motor_izq, int motor_der)
{
105 if ( motor_izq >= 0 ) //motor izquierdo
106 {
107 digitalWrite(mot_i,HIGH); // con high avanza
108 analogWrite(pin_pwm_i,255-motor_izq); //se controla de manera
109 //inversa para mayor control
}
110 else
111 {
112 digitalWrite(mot_i,LOW); //con low retrocede
113 motor_izq = motor_izq*(-1); //cambio de signo
analogWrite(pin_pwm_i,motor_izq);
114 }
115
116 if ( motor_der >= 0 ) //motor derecho
117 {
118 digitalWrite(mot_d,HIGH);
119 analogWrite(pin_pwm_d,255-motor_der);
}
120 else
121 {
122 digitalWrite(mot_d,LOW);
123 motor_der= motor_der*(-1);
analogWrite(pin_pwm_d,motor_der);
124 }
125
126
127 }
128
129 void frenos_contorno(int tipo,int flanco_comparacion)
130 {
131
132 if(tipo==0)
{
133 if(position<=50) //si se salio por la parte derecha de la linea
134 {
135 motores(-80,90); //debido a la inercia, el motor
136 //tendera a seguri girando
137 //por eso le damos para atras , para que frene
138 // lo mas rapido posible
139 while(true)
{
140 qtrrc.read(sensorValues); //lectura en bruto de sensor
141 if( sensorValues[0]>flanco_comparacion || sensorValues[1]>flanco_comparacion )
142 //asegurar que esta en linea
143 {
break;
144 }
145 }
146 }
147
148 if (position>=6550) //si se salio por la parte izquierda de la linea
149 {
motores(90,-80);
150 while(true)
151 {
152 qtrrc.read(sensorValues);
153 if(sensorValues[7]>flanco_comparacion || sensorValues[6]>flanco_comparacion )
{
154 break;
155 }
156 }
157 }
158 }
159
if(tipo==1) //para linea blanca con fondo negro
160 {
161 if(position<=50) //si se salio por la parte derecha de la linea
162 {
163 motores(-80,90); //debido a la inercia el motor
164 //tendera a seguri girando
//por eso le damos para atras
165 //para que frene lo mas rapido posible
166 while(true)
167 {
168 qtrrc.read(sensorValues); //lectura en bruto de sensor
if(sensorValues[0]<flanco_comparacion || sensorValues[1]<flanco_comparacion )
169 esta en linea
170 {
171 break;
172 }
173 }
}
174
175 if(position>=6550) //si se salio por la parte izquierda de la linea
176 {
177 motores(90,-80);
178 while(true)
{
179 qtrrc.read(sensorValues);
180 if(sensorValues[7]<flanco_comparacion || sensorValues[6]<flanco_comparacion)
181 {
182 break;
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