Ahora comenzaremos con la robótica pura y dura, los conceptos son básicos, pero la relación entre mecánica, electrónica, informática y diseño, es un arte.

Antes de comenzar a programar el robot, es necesario ajustar algunos componentes. Por supuesto, ya habrá notado que entre los materiales para construir a Open Lamborghino no hemos nombrado (aun) al pivote; y no los hemos hecho porque es para poder poner un pivote, debemos tener ensamblado todo nuestro robot.

Observe este gráfico del sensor de reflectancia QRE1113

Como puede ver, el sensor tiene su mejor rango de respuesta entre 0.5 y 3mm de distancia de la superficie medida. Por esta razón, el mejor lugar en donde podamos poner el pivote, es donde logremos la mejor respuesta. (Pruebe la respuesta del sensor, con lecturas analógicas de cada sensor; mientras más alta la diferencia entre la reflectancia medida entre el blanco y el negro, mejor.)

Bien, ya estamos leyendo la línea, pero necesitamos calibrar la señal, para hacer más fidedignas las mediciones.

Ver código


#define PINBUZZER  10
#define PINBOTON  2


int v_s_min[6] = {1023, 1023, 1023, 1023, 1023, 1023};
int v_s_max[6] = {0, 0, 0, 0, 0, 0};
volatile int s_p[6];
boolean online;
int pos;
int l_pos;



void setup() {
  Serial.begin(115200);
  Serial.println("hola");
  Peripherals_init();

  WaitBoton();

  delay(1000);

  calibracion();

  digitalWrite(13, LOW);
  tone(PINBUZZER, 1500, 50);
  delay(70);
  tone(PINBUZZER, 1500, 50);
  delay(70);

  WaitBoton();
  delay(1000);
  digitalWrite(13, HIGH);

}



void loop() {

  int line_position = GetPos();
  Serial.println(line_position);
  delay(1);



}


void calibracion() {

  int v_s[6];

  for (int j = 0; j < 100; j++) {
    delay(10);
    v_s[0] = analogRead(A6);
    v_s[1] = analogRead(A5);
    v_s[2] = analogRead(A4);
    v_s[3] = analogRead(A3);
    v_s[4] = analogRead(A2);
    v_s[5] = analogRead(A1);

    for (int i = 0; i < 6; i++) {

      Serial.print(v_s[i]);
      Serial.print("\t");

    }
    Serial.println();

    for (int i = 0; i < 6; i++) {
      if (v_s[i] < v_s_min[i]) {
        v_s_min[i] = v_s[i];
      }
    }


    for (int i = 0; i < 6; i++) {
      if (v_s[i] > v_s_max[i]) {
        v_s_max[i] = v_s[i];
      }
    }
  }

  beep();
  beep();

  Serial.println();
  Serial.print("Mínimos ");
  Serial.print("\t");

  for (int i = 0; i < 6; i++) {

    Serial.print(v_s_min[i]);
    Serial.print("\t");

  }
  Serial.println();
  Serial.print("Máximos ");
  Serial.print("\t");

  for (int i = 0; i < 6; i++) {

    Serial.print(v_s_max[i]);
    Serial.print("\t");

  }
  Serial.println();
  Serial.println();
  Serial.println();






}

void readSensors() {

  volatile int s[6];



  s[0] = analogRead(A6);
  s[1] = analogRead(A5);
  s[2] = analogRead(A4);
  s[3] = analogRead(A3);
  s[4] = analogRead(A2);
  s[5] = analogRead(A1);

  for (int i = 0; i < 6; i++) {
    if (s[i] < v_s_min[i]) {
      s[i] = v_s_min[i];
    }

    if (s[i] > v_s_max[i]) {
      s[i] = v_s_max[i];
    }
    s_p[i] = map(s[i], v_s_min[i], v_s_max[i], 100, 0);
  }


  volatile int sum = s_p[0] + s_p[1] + s_p[2] + s_p[3] + s_p[4] + s_p[5];
  if (sum > 100) {
    online = 1;

  } else {
    online = 0;
    sum = 100;
  }


  if (online) {
    for (int i = 0; i < 6; i++) {
      Serial.print(s_p[i]);
      Serial.print("\t");
    }
    //Serial.println();
  }



}


int GetPos() {
  readSensors();
  int prom = -2.5 * s_p[0] - 1.5 * s_p[1] - 0.5 * s_p[2] + 0.5 * s_p[3] + 1.5 * s_p[4] + 2.5 * s_p[5];
  int sum = s_p[0] + s_p[1] + s_p[2] + s_p[3] + s_p[4] + s_p[5];

  if (online) {
    pos = int(100.0 * prom / sum);
  } else {
    if (l_pos < 0) {
      pos = -255;
    }
    if (l_pos >= 0) {
      pos = 255;
    }
  }
  l_pos = pos;
  return pos;
}



void Peripherals_init() {
  pinMode(PINBOTON, INPUT);
  pinMode(PINBUZZER, OUTPUT);
}


void WaitBoton() {   // Entra en un bucle infinito de espera.
  while (!digitalRead(PINBOTON));  // Se sale del bucle cuando se aprieta el botón
  tone(PINBUZZER, 2000, 100);      // Cuando sale del bucle, suena el buzzer
}


void beep() {
  tone(PINBUZZER, 2000, 100);
  delay(200);
}



Procedimiento de calibración:

  • Cargamos el programa.
  • Ponemos el robot sobre una superficie negra opaca, con una línea blanca.
  • Abrimos el puerto serial.
  • Apretamos el botón, y comenzamos a girar el robot de manera que todos los sensores pasen por el blanco y por el negro, hasta que suenen 2 beeps.
  • Finalmente, puedes apretar el botón para que se imprima por puerto serial la lectura calibrada y la posición del robot.

La última columna nos mostrará la posición relativa del robot, con un rango entre -255 y +255. Donde -255 significa que el la trompa se está saliendo por la derecha, 255 significa que la trompa se está saliendo por la izquierda, y 0 significa que el sensor está completamente alineado respecto a la línea.

Entonces nuestro objetivo será mantener ese valor lo más cercano a cero. Para eso, utilizaremos los motores convenientemente para corregir la posición relativa del robot sobre la línea.

Antes de eso, debemos asegurarnos que la polaridad de los motores es la correcta.

Para eso debemos asegurarnos que cuando ejecutes este código, ambas ruedas del robot se muevan para adelante (si alguna rueda va en el sentido contrario, debes cambiar la polaridad de la rueda, desconectando el motor y conectándolo al revés).

Siguiendo la línea

Esta parte es un mar de conocimientos que prometemos abordar en otros tutoriales. Lo importante ahora es concentrarnos en seguir la línea. El siguiente código mantiene al robot en la línea con la ayuda de un sistema de control PD.

Ver código

#define PINBUZZER  10
#define PINBOTON  2

# define AIN1 8    // pin 1 de dirección del Motor Izquierdo
# define AIN2 9    // pin 2 de dirección del Motor Izquierdo
# define PWMA 5    // pin PWM del Motor Izquierdo


# define BIN1 4    // pin 1 de dirección del Motor Derecho
# define BIN2 7    // pin 2 de dirección del Motor Derecho
# define PWMB 6    // pin PWM del Motor Derecho





int base = 0;
float Kprop = 1.2;
float Kderiv = 7.5;
float Kinte = 0.0;
int pos;



int setpoint = 0;
int last_error = 0;
int pot_limite = 250;


int v_s_min[6] = {1023, 1023, 1023, 1023, 1023, 1023};
int v_s_max[6] = {0, 0, 0, 0, 0, 0};
volatile int s_p[6];
boolean online;

int l_pos;




void setup() {
  Serial.begin(115200);
  pinMode(13, OUTPUT);


  digitalWrite(13, LOW);
  Serial.println("hola");
  Motores(0, 0);
  WaitBoton();
  digitalWrite(13, HIGH);
  Peripherals_init();

  beep();
  delay(1000);
  calibracion();

  digitalWrite(13, LOW);
  tone(PINBUZZER, 1500, 50);
  delay(70);
  tone(PINBUZZER, 1500, 50);
  delay(70);

  WaitBoton();
  delay(1000);
  digitalWrite(13, HIGH);

}


void loop() {

  int line_position = GetPos();
  int Correction_power = PIDLambo(line_position, Kprop, Kderiv, Kinte);
  Motores(base + Correction_power, base + -Correction_power);
  Serial.print(line_position);
  Serial.print("\t");
  Serial.println(Correction_power);


}


void Peripherals_init() {
  pinMode(PINBOTON, INPUT);
  pinMode(PINBUZZER, OUTPUT);
}


void WaitBoton() {   // Entra en un bucle infinito de espera.
  while (!digitalRead(PINBOTON));  // Se sale del bucle cuando se aprieta el botón
  tone(PINBUZZER, 2000, 100);      // Cuando sale del bucle, suena el buzzer
}

void beep() {
  tone(PINBUZZER, 2000, 100);
  delay(200);
}

int PIDLambo(int POS, float Kp, float Kd, float Ki) {

  int error = pos - setpoint;
  int derivative = error - last_error;
  last_error = error;
  int pot_giro = (error * Kp + derivative * Kd);



  if (pot_giro > pot_limite)
    pot_giro = pot_limite;
  else if (pot_giro < -pot_limite)
    pot_giro = -pot_limite;
  return pot_giro;


}


void calibracion() {

  int v_s[6];

  for (int j = 0; j < 100; j++) {
    delay(10);
    v_s[0] = analogRead(A6);
    v_s[1] = analogRead(A5);
    v_s[2] = analogRead(A4);
    v_s[3] = analogRead(A3);
    v_s[4] = analogRead(A2);
    v_s[5] = analogRead(A1);

    for (int i = 0; i < 6; i++) {

      Serial.print(v_s[i]);
      Serial.print("\t");

    }
    Serial.println();

    for (int i = 0; i < 6; i++) {
      if (v_s[i] < v_s_min[i]) {
        v_s_min[i] = v_s[i];
      }
    }


    for (int i = 0; i < 6; i++) {
      if (v_s[i] > v_s_max[i]) {
        v_s_max[i] = v_s[i];
      }
    }
  }

  beep();
  beep();

  Serial.println();
  Serial.print("Mínimos ");
  Serial.print("\t");

  for (int i = 0; i < 6; i++) {

    Serial.print(v_s_min[i]);
    Serial.print("\t");

  }
  Serial.println();
  Serial.print("Máximos ");
  Serial.print("\t");

  for (int i = 0; i < 6; i++) {

    Serial.print(v_s_max[i]);
    Serial.print("\t");

  }
  Serial.println();
  Serial.println();
  Serial.println();






}

void readSensors() {

  volatile int s[6];



  s[0] = analogRead(A6);
  s[1] = analogRead(A5);
  s[2] = analogRead(A4);
  s[3] = analogRead(A3);
  s[4] = analogRead(A2);
  s[5] = analogRead(A1);

  for (int i = 0; i < 6; i++) {
    if (s[i] < v_s_min[i]) {
      s[i] = v_s_min[i];
    }

    if (s[i] > v_s_max[i]) {
      s[i] = v_s_max[i];
    }
    s_p[i] = map(s[i], v_s_min[i], v_s_max[i], 100, 0);
  }


  volatile int sum = s_p[0] + s_p[1] + s_p[2] + s_p[3] + s_p[4] + s_p[5];
  if (sum > 100) {
    online = 1;

  } else {
    online = 0;
    sum = 100;
  }


  if (online) {
    for (int i = 0; i < 6; i++) {
      Serial.print(s_p[i]);
      Serial.print("\t");
    }
    //Serial.println();
  }



}




int GetPos() {
  readSensors();
  int prom = -2.5 * s_p[0] - 1.5 * s_p[1] - 0.5 * s_p[2] + 0.5 * s_p[3] + 1.5 * s_p[4] + 2.5 * s_p[5];
  int sum = s_p[0] + s_p[1] + s_p[2] + s_p[3] + s_p[4] + s_p[5];

  if (online) {
    pos = int(100.0 * prom / sum);
  } else {
    if (l_pos < 0) {
      pos = -255;
    }
    if (l_pos >= 0) {
      pos = 255;
    }
  }
  l_pos = pos;
  return pos;
}






void TB6612FNG_init() {

  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

}

void MotorIz(int value) {
  if (value >= 0) {
    // si valor positivo vamos hacia adelante

    digitalWrite(AIN1, HIGH);
    digitalWrite(AIN2, LOW);
  } else {
    // si valor negativo vamos hacia atras

    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, HIGH);
    value *= -1;
  }

  // Setea Velocidad

  analogWrite(PWMA, value);
}


void MotorDe(int value) {
  if (value >= 0) {
    // si valor positivo vamos hacia adelante

    digitalWrite(BIN1, HIGH);
    digitalWrite(BIN2, LOW);
  } else {
    // si valor negativo vamos hacia atras

    digitalWrite(BIN1, LOW);
    digitalWrite(BIN2, HIGH);
    value *= -1;
  }

  // Setea Velocidad

  analogWrite(PWMB, value);
}


void Motores(int left, int righ) {
  MotorIz(left);
  MotorDe(righ);
}


Instrucciones

  • Carga el programa.
  • Haz el procedimiento de calibración.
  • Deja el robot alineado con la línea blanca.
  • Presiona el botón.
  • Observa como la trompa del robot tiende a mantenerse alineada. (intenta moverlo, para ver la tendencia).
  • Ahora que la trompa está alineada, es hora de subir la velocidad base, para que comienze a seguir la línea

Esta velocidad base debe ser un número entero de 0 a 255. Comienza a aumentar el valor de base, de a poco, hasta encontrar el valor base estable y rápido para el robot.

Ahora que ya logramos seguir la línea, es hora de pasar al siguiente tutorial.