Robot Velocista QTR8A (Parte III - Programacion del Robot)

Programación del Robot

Algoritmo PID

Este sera el encargado de regular el comportamienito del robot y corregir su marcha en el recorrido de la pista, recibe datos entrada (sensores) , las procesa, (PID) y envia la señal de correccion hacia los motores (salida de control) y nuevamente repite el proceso coparando la salida con la entrada (retroalimentacion) sin entrar en mas teoría se puede resumir de manera esquematica este controlador

• Entrada de mando: Vendría a ser el punto de referencia en donde queremos fijar al robot "Set point", nuestro objetivo es que el robot no se salga de la linea, por lo tanto lo deseado es que siempre se mantenga centrado en el medio, como se vio anteriormente el centro de los sensores arrojaba un valor de posición relativa de 3500, este sera nuestra referencia o "set point" (donde queremos que el robot se mantenga siempre.

• Controlador :  En este caso el controlador es el encargado de corregir y mantener en marcha al robot, su objetivo es que no se salga del punto de referencia o que almenos logre tenerlo dentro de los rangos permitidos. lo ideal es que mantenga al robot fijado en el centro, pero en la realidad ocurrirá que el mundo externo provoque muchas  perturbaciones haciendo que el robot oscile o se salga del punto de referencia por lo que tendrá que corregir continuamente la marcha. Si este controlador es el "PID"... Que para nuestro caso sera un algoritmo programado en arduino.

• Planta: La planta esta relacionada con las condiciones físicas del robot, peso, longitud, traccion de los motores, velocidad, rozamiento, inercia, etc.  Como se ve el diagrama de bloques, el controlador modifica el comportamiento de la planta, osea el comportamiento del robot.

• Salida o señal de control: Es la respuesta producida por el control aplicado en el robot, en este caso la salida  de control se envia al driver de motor y luego a los motores para que puedan corregir la marcha constantemente. la medición de esta salida es jutamente la señal es la posicion actual del robot medido .

• Realimentacion: Esta compuesta de la adquisición de datos, osea la de los sensores del robot, que va leer la posición relativa actual del robot la procesa y manda nuevamente a comparararse con la referencia que es el "set point"  , esta comparación traerá un resultado llamado error que nuevamente es entregado al controlador para que pueda procesarla y enviar una señal de correccion, la realimentacion es la comparación de la salida con respecto a la entrada, 

Todo esto en gran parte sera el algoritmo de control del robot: Se que es muy abstracto todo lo que concierne con PID, pero con la practica algunos de estos conceptos estaran claros.

En este post ya tramos acerca de cada una de las acciones del PID,  de las constantes y el procedimiento para sintonizar, por lo que ya no seré tan redundante explicando nuevamente. : http://aprendiendofacilelectronica.blogspot.pe/2014/12/robot-velocista-de-competencia_4.html

Circuito del Robot Velocista

Programa Completo

/////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////

#include <QTRSensors.h>

  //entradas
#define btn1      0
#define btn2      1
  //salidas
#define led1      13

#define led_on    9   //~
#define mi1       3
#define mi2       4
#define pwmi      6   //~
#define md1       8
#define md2       7
#define pwmd      5   //~
  

#define NUM_SENSORS             8  // number of sensors used
#define NUM_SAMPLES_PER_SENSOR  1 // average 4 analog samples per sensor reading
#define EMITTER_PIN             9   // emitter is controlled by digital pin 2

QTRSensorsAnalog qtra((unsigned char[]) {7, 6, 5, 4, 3, 2,1,0},NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
int proporcional=0;
int derivativo=0;
int integral=0;
int salida_pwm=0;
int proporcional_pasado=0;
int position=0;

////////////////////Parametros PID/////////////////////////////////
int velocidad=120;
//float KP=0.15, KD=2.2, KI=0.006; //reaccion rapida
float KP=0.01, KD=0.15 , KI=0.0001; //reaccion rapida
///////////////////////////////////////////////////////

/////////////////parametros de sensado/////////////////////////////
int linea=0; //0 linea negra, 1 para linea blanca
int flanco_color=  0 ;
int en_linea=  500 ;
int ruido= 30;
/////////////////////////////////////////////////////////
int boton1=7;
int boton2=7;

void setup()
{
    pinMode(led1,OUTPUT);
    pinMode(led_on,OUTPUT);
    pinMode(mi1,OUTPUT);
    pinMode(mi2,OUTPUT);
    pinMode(pwmi,OUTPUT);
    pinMode(md1,OUTPUT);
    pinMode(md2,OUTPUT);
    pinMode(pwmd,OUTPUT);

digitalWrite(led1,HIGH);
delay(200);
digitalWrite(led1,LOW);
delay(200);

digitalWrite(led1, HIGH);    // turn on Arduino's LED to indicate we are in calibration mode
  for (int i = 0; i < 200; i++)  // make the calibration take about 10 seconds
  {
    qtra.calibrate();       // reads all sensors 10 times at 2.5 ms per six sensors (i.e. ~25 ms per call)
  }
  digitalWrite(led1, LOW);     // turn off Arduino's LED to indicate we are through with calibration
 //Serial.begin(115200);
   //Serial.println();

   while(true)
   {
      botones();
      if(boton2==0) 
      {
        delay(20);
        digitalWrite(led1,HIGH);
        delay(100);
        digitalWrite(led1,LOW);
        delay(100);
        break;
      }
   }
  
Serial.begin(115200);
//bt.println("Hello, world?");

}

void loop()
{
  pid(1,velocidad,KP,KI,KD);
  frenos_contorno(600);

 Serial.println(position);
 delay(2);
}

void pid(int linea, int velocidad, float Kp, float Ki, float Kd)
{
   
   position = qtra.readLine(sensorValues,QTR_EMITTERS_ON, linea, flanco_color, en_linea, ruido);
  //0 linea negra, 1 para linea blanca
  
      //  Serial.println(position);
  proporcional = (position) - 3500; // set point es 3500, asi obtenemos el error
  integral=integral + proporcional_pasado; //obteniendo integral
  derivativo = (proporcional - proporcional_pasado); //obteniedo el derivativo
  int ITerm=integral*KI;
  if(ITerm>=255) ITerm=255;
  if(ITerm<=-255) ITerm=-255;
  
  salida_pwm =( proporcional * KP ) + ( derivativo * KD )+(ITerm);
  
  if (  salida_pwm >velocidad )  salida_pwm = velocidad; //limitamos la salida de pwm
  if ( salida_pwm  <-velocidad )  salida_pwm = -velocidad;
  
  if (salida_pwm < 0)
 {
    int der=velocidad-salida_pwm; //(+)
    int izq=velocidad+salida_pwm;  //(-)
    if(der>=255)der=255;
    if(izq<=0)izq=0;
    motores(izq, der);
 }
 if (salida_pwm >0)
 {
  int der=velocidad-salida_pwm; //(-)
  int izq=velocidad+salida_pwm; //(+)
  
  if(izq >= 255) izq=255;
  if(der <= 0) der=0;
  motores(izq ,der );
 }

 proporcional_pasado = proporcional;  
}


void frenos_contorno(int flanco_comparacion)
{
    if (position <=10) //si se salio por la parte derecha de la linea
    {
      while(true)
      { 
        digitalWrite(led1,HIGH);
        motores(-125,60);
        qtra.read(sensorValues); //lectura en bruto de sensor
        if ( sensorValues[0]<flanco_comparacion || sensorValues[1]<flanco_comparacion || sensorValues[2]<flanco_comparacion || sensorValues[3]<flanco_comparacion || sensorValues[4]<flanco_comparacion || sensorValues[5]<flanco_comparacion || sensorValues[6]<flanco_comparacion || sensorValues[7]<flanco_comparacion)
        {
          break;
        }
        
      }
    }

    if (position>=6990) //si se salio por la parte izquierda de la linea
    {
      while(true)
      {
        digitalWrite(led1,HIGH);
        motores(60,-125); 
        qtra.read(sensorValues);
        if (sensorValues[7]<flanco_comparacion || sensorValues[6]<flanco_comparacion|| sensorValues[5]<flanco_comparacion || sensorValues[4]<flanco_comparacion || sensorValues[3]<flanco_comparacion || sensorValues[2]<flanco_comparacion || sensorValues[1]<flanco_comparacion|| sensorValues[0]<flanco_comparacion)
        {
          break;
        }
      }
  }
  digitalWrite(led1,LOW);
}


void motores(int motor_izq, int motor_der)
{
  if ( motor_izq >= 0 )  
  {
    digitalWrite(mi1,LOW);
    digitalWrite(mi2,HIGH); 
    analogWrite(pwmi,motor_izq); 
  }
  else
  {
    digitalWrite(mi1,HIGH); 
    digitalWrite(mi2,LOW);
    motor_izq = motor_izq*(-1); 
    analogWrite(pwmi,motor_izq);
  }

  if ( motor_der >= 0 ) //motor derecho
  {
    digitalWrite(md1,LOW);
    digitalWrite(md2,HIGH);
    analogWrite(pwmd,motor_der);
  }
  else
  {
    digitalWrite(md1,HIGH);
    digitalWrite(md2,LOW);
    motor_der= motor_der*(-1);
    analogWrite(pwmd,motor_der);
  }
}

void botones()
{
    boton1=digitalRead(btn2);
    boton2=digitalRead(btn1); ///boton izquierdo, derecho
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

Nota: este programa esta utilizando la libreria modificada de este post Modificacion de libreria QTR, si estas usando la libreria sin modificar, que de hecho no funcionara con este codigo tendrás que descargarlo desde este link: https://mega.nz/#!CkMkiZhA!FMK4eJQVmOGZK9GZzfqSKRgEPOijTSkMmbNDUwbf5C0 en instalarlo nuevamente. 

Instalacion de Libreria QTR SENSORS:

https://www.youtube.com/watch?v=iGDi4sxwaxw&feature=youtu.be

Esquematicos de FK-BOT:

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

 He pasado muy buenos días diseñando este robot, que fue considerado en su momento como el más rapido, 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!!!  😊😎🙌

Driver Pololu :https://mega.nz/#F!GkckwITB!tBRwDXi3MO-D9U3xMqgBTw

Cifrado: !tBRwDXi3MO-D9U3xMqgBTw

Driver Sparkfun-Compatible(rojo): https://mega.nz/#F!Ok1jULzb!DFAwhpyvyMLxdrc3C_xedw

Cifrado: !DFAwhpyvyMLxdrc3C_xedw

PRUEBAS FINALES: 

Post Anterior  << Manejando sensores QTR8A y Driver