Robot Velocista de competencia - Parte III : Algoritmo Pid - Programacion Robot velocista

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 proporcionalmente con la mismo velocidad que se produce, de esta manera evita que el error se incremente, en otra palabra, anticipara la accion evitando asi las oscilaciones excesivas.
                                    Derivativo=proporcional-proporcional_pasado

->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

#include <QTRSensors.h>
/////////////////////////////////////////////////////////////////////////////////////
//*************  Programa realizado por MARCO A. CABALLERO MORENO***************   //
// Solo para fines educativos   
// robot velocista mini FK BOT  V 2.0                                            //
// micro motores pololu MP 10:1, sensores qtr 8rc, driver drv8833, arduino nano    //
//                                     05/12/2014                  
// ACTUALIZADO 29/3/2015            
/////////////////////////////////////////////////////////////////////////////////////

#define NUM_SENSORS   8     //numero de sensores usados
#define TIMEOUT       2000  // tiempo de espera para dar resultado en uS
#define EMITTER_PIN   6     //pin led on
///////////////pines arduino a utilizar/////////////////////
#define led1          13 
#define led2          4  
#define mot_i         7
#define mot_d         8
#define sensores      6
#define boton_1       2  //pin para boton
#define pin_pwm_i     9
#define pin_pwm_d     10

QTRSensorsRC qtrrc((unsigned char[]) {19, 18, 17, 16,15,14,11,12}
,NUM_SENSORS, TIMEOUT, EMITTER_PIN);

//variables para almacenar valores de sensores y posicion
unsigned int sensorValues[NUM_SENSORS];
unsigned int position=0;

/// variables para el pid
int  derivativo=0, proporcional=0, integral=0; //parametros
int  salida_pwm=0, proporcional_pasado=0;

///////////////AQUI CAMBIEREMOS LOS PARAMETROS DE NUESTRO ROBOT!!!!!!!!!!!!!!!
int velocidad=120;              //variable para la velocidad, el maximo es 255
float Kp=0.18, Kd=4, Ki=0.001;  //constantes
//variable para escoger el tipo de linea
int linea=0;                    //  0 para lineas negra, 1 para lineas blancas

void setup()
{
 delay(800);
 pinMode(mot_i, OUTPUT);//pin de direccion motor izquierdo
 pinMode(mot_d, OUTPUT);//pin de direccion motor derecho
 pinMode(led1, OUTPUT); //led1
 pinMode(led2, OUTPUT); //led2
 pinMode(boton_1, INPUT); //boton 1 como pull up
        
 for (int i = 0; i <40; i++)  //calibracion durante 2.5 segundos,
 {                                 //para calibrar es necesario colocar los sensores sobre la superficie negra y luego 
  digitalWrite(led1, HIGH);  //la blanca
  delay(20);
  qtrrc.calibrate();    //funcion para calibrar sensores   
  digitalWrite(led1, LOW);  
  delay(20);
 }
digitalWrite(led1, LOW); //apagar sensores para indicar fin de calibracion 
delay(400); 
digitalWrite(led2,HIGH); //encender led 2 para indicar la
while(true)
{
    int x=digitalRead(boton_1); //leemos y guardamos el valor
                                      // del boton en variable x
    delay(100);
    if(x==0) //si se presiona boton 
    {
        digitalWrite(led2,LOW); //indicamos que se presiono boton
        digitalWrite(led1,HIGH); //encendiendo led 1
        delay(100);
        break; //saltamos hacia el bucle principal
    }
}
}       


void loop()
{

 //pid(0, 120, 0.18, 0.001, 4 );
  pid(linea,velocidad,Kp,Ki,Kd); //funcion para algoritmo pid 
                                 //primer parametro: 0 para lineas negras, tipo 1 para lineas blancas
                                 //segundo parametro: velocidad pwm de 0 a 255
                                 //tercer parametro: constante para accion proporcional
                                 //cuarto parametro: constante para accion integral
                                 //quinto parametro: constante para accion derivativa
  //frenos_contorno(0,700);
  frenos_contorno(linea,700); //funcion para frenado en curvas tipo 
                              //primer parametro :0 para lineas negras, tipo 1 para lineas blancas
                              //segundo parametro:flanco de comparación va desde 0 hasta 1000 , esto para ver 
}

////////funciones para el control del robot////
 void pid(int linea, int velocidad, float Kp, float Ki, float Kd)
{
  position = qtrrc.readLine(sensorValues, QTR_EMITTERS_ON, linea); //0 para linea negra, 1 para linea blanca
  proporcional = (position) - 3500; // set point es 3500, asi obtenemos el error
  integral=integral + proporcional_pasado; //obteniendo integral
  derivativo = (proporcional - proporcional_pasado); //obteniedo el derivativo
  if (integral>1000) integral=1000; //limitamos la integral para no causar problemas
  if (integral<-1000) integral=-1000;
  salida_pwm =( proporcional * Kp ) + ( derivativo * Kd )+(integral*Ki);
  
  if (  salida_pwm > velocidad )  salida_pwm = velocidad; //limitamos la salida de pwm
  if ( salida_pwm < -velocidad )  salida_pwm = -velocidad;
  
  if (salida_pwm < 0)
 {
  motores(velocidad+salida_pwm, velocidad);
 }
 if (salida_pwm >0)
 {
  motores(velocidad, velocidad-salida_pwm);
 }

 proporcional_pasado = proporcional;  
}

//funcion para control de motores
void motores(int motor_izq, int motor_der)
{
  if ( motor_izq >= 0 )  //motor izquierdo
 {
  digitalWrite(mot_i,HIGH); // con high avanza
  analogWrite(pin_pwm_i,255-motor_izq); //se controla de manera
                                        //inversa para mayor control
 }
 else
 {
  digitalWrite(mot_i,LOW); //con low retrocede
  motor_izq = motor_izq*(-1); //cambio de signo
  analogWrite(pin_pwm_i,motor_izq); 
 }

  if ( motor_der >= 0 ) //motor derecho
 {
  digitalWrite(mot_d,HIGH);
  analogWrite(pin_pwm_d,255-motor_der);
 }
 else
 {
  digitalWrite(mot_d,LOW);
  motor_der= motor_der*(-1);
  analogWrite(pin_pwm_d,motor_der);
 }

  
}

void frenos_contorno(int tipo,int flanco_comparacion)
{
  
if(tipo==0)
{
  if(position<=50) //si se salio por la parte derecha de la linea
 {
  motores(-80,90); //debido a la inercia, el motor 
                                  //tendera a seguri girando
                                  //por eso le damos para atras , para que frene
                                 // lo mas rapido posible 
  while(true)  
  {
   qtrrc.read(sensorValues); //lectura en bruto de sensor   
   if( sensorValues[0]>flanco_comparacion || sensorValues[1]>flanco_comparacion ) 
   //asegurar que esta en linea
   {
    break;
   } 
  }
 }

 if (position>=6550) //si se salio por la parte izquierda de la linea
 { 
  motores(90,-80);
  while(true)
  {
   qtrrc.read(sensorValues);
   if(sensorValues[7]>flanco_comparacion || sensorValues[6]>flanco_comparacion )
   {
    break;
   }  
  }
 }
}

if(tipo==1) //para linea blanca con fondo negro
{
 if(position<=50) //si se salio por la parte derecha de la linea
 {
  motores(-80,90); //debido a la inercia el motor 
                   //tendera a seguri girando
                   //por eso le damos para atras  
                   //para que frene lo mas rapido posible 
  while(true)  
  {
   qtrrc.read(sensorValues); //lectura en bruto de sensor
   if(sensorValues[0]<flanco_comparacion || sensorValues[1]<flanco_comparacion )   //asegurar que esta en linea
   {
    break;
   }
  }
 }

 if(position>=6550) //si se salio por la parte izquierda de la linea
 { 
  motores(90,-80);
  while(true)
  {
   qtrrc.read(sensorValues);
   if(sensorValues[7]<flanco_comparacion || sensorValues[6]<flanco_comparacion)
   {
    break;
   }  
  }
 }
}
}

ESQUEMA ROBOT VELOCISTA:

Esquemáticos PCB Mini FK-BOT 1.0:

Ya que me estuvieron pidiendo esquemáticos por mucho tiempo, pues aqui las libero.

He pasado muy buenos días diseñando este robot (la primera versión Mini FK-BOT 1.0), que fue considerado en su momento como el más rápido y que con el he ganado los concursos mas importantes del pais, ya ha pasado los años y cumplio su mision ...

La mejor forma de agradecerme es que le des clic a los anuncios de la parte superior del blog (cosa que no te costara nada hacerlo) , asi me ayudaras mucho! Saludos amigos robotero!!!  😊😎🙌

Enlace: https://mega.nz/#F!Lk0D0ABA!ErZkc0Pn38XY4eUiP7X7Dw

Clave Cifrado: !ErZkc0Pn38XY4eUiP7X7Dw

Video por ADKITS-PERU

Siguiente Post>> MODIFICACION LIBRERIA "QTR SENSORS" PARA MEJOR SENSADO - I Parte

Anterior post<< Robot Velocista de competencia: Utilizando la libreria QTR SENSORS