Función Promedio Ponderado

► TEST de sensores IR MECHA 22.

Los sensores extremos están invertidos como muestran las lecturas.

☼ Después de realizar el test de sensores y comprobar la entrada analógica correspondiente a los sensores extremos izquierdo y derecho, realizamos los cambios en el código para que estos queden orientados correctamente de izquierda a derecha al igual que están en el robot y luego podemos realizar el encabezado para la función promedio ponderado.

► Encabezado de código promedio ponderado:

/*Inicio de encabezado para función promedio ponderado*/

/* PINES SENSORES */

#define S1 A7

#define S2 A6

#define S3 A5

#define S4 A4

#define S5 A3

#define S6 A2

#define S7 A1

#define S8 A0

/* VARIABLES PARA LECTURA DE SENSORES*/

int LS1 = 0;

int LS2 = 0;

int LS3 = 0;

int LS4 = 0;

int LS5 = 0;

int LS6 = 0;

int LS7 = 0;

int LS8 = 0;

/* fin de encabezada para función promedio ponderado*/

void setup() {

 /* en las entadas analógicas no son necesarios los modos de pin */

Serial.begin(9600);               // inicio monitor serial

}

void loop() {

promedio_ponderado();             // llamada de función en bucle infinito  

}

 void promedio_ponderado() {      // creamos la función vacía

// Lectura de sensores //

LS1 = analogRead(S1);             // sensor extremo izquierdo

LS2 = analogRead(S2);

LS3 = analogRead(S3);

LS4 = analogRead(S4);

LS5 = analogRead(S5);

LS6 = analogRead(S6);

LS7 = analogRead(S7);

LS8 = analogRead(S8);             // sensor extremo derecho

/* Mapeo de lecturas */

LS1 = map(LS1, 0,1023,1000,0);

LS2 = map(LS2, 0,1023,1000,0);

LS3 = map(LS3, 0,1023,1000,0);

LS4 = map(LS4, 0,1023,1000,0);

LS5 = map(LS5, 0,1023,1000,0);

LS6 = map(LS6, 0,1023,1000,0);

LS7 = map(LS7, 0,1023,1000,0);

LS8 = map(LS8, 0,1023,1000,0);

/* Muestra de valores en monitor serial */

Serial.print(" | ");

Serial.print(LS1); Serial.print(" | ");

Serial.print(LS2); Serial.print(" | ");

Serial.print(LS3); Serial.print(" | ");

Serial.print(LS4); Serial.print(" | ");

Serial.print(LS5); Serial.print(" | ");

Serial.print(LS6); Serial.print(" | ");

Serial.print(LS7); Serial.print(" | ");

Serial.print(LS8); Serial.println(" | ");

delay(500);                             // retardo de tiempo

}

☼ Después de la corrección en la posición de lectura realizamos un nuevo test y graficamos los valores.

► En el grafico se puede observar que la linea se encuentra entre el sensor 4 y el sensor 5.

☼ Calculo de promedio ponderado.

En el código aplicaremos la formula de promedio ponderado para sacar la posición relativa.

▼ Vamos a agregar al código anterior las líneas para la creación de las variables tipo float que utilizaremos en el calculo del promedio ponderado de la posición, las colocamos luego de la variables para los sensores.

/* Variable posición relativa*/

float ponderacion = 0;

float suma_lecturas = 0;

float pos_rel = 0;

▼ Las formulas para los cálculos las agregamos ya en la función promedio_ponderado luego del mapeo de las lecturas.

/*calculos de posicion relativa*/

ponderacion = ((LS1*0)+(LS2*1)+(LS3*2)+(LS4*3)+(LS5*4)+(LS6*5)+(LS7*6)+(LS8*7));

suma_lecturas = (LS1+LS2+LS3+LS4+LS5+LS6+LS7+LS8);

pos_rel = 1000*(ponderacion/suma_lecturas);

▼ Para el test vamos a imprimir la pos_rel calculada en el monitor serial, para ese agregamos las líneas correspondientes luego del delay.

/* Muestra de valores calculados posicion relativa*/

Serial.print("ponderacion = ");Serial.println(ponderacion);

Serial.print("suma = ");Serial.println(suma_lecturas);

Serial.print("pos_rel = ");Serial.println(pos_rel);

☼ Lectura de sensores y valores calculados robot MECHA 22.

☼ Se puede ver en la imagen anterior los cambios que se producen el los valores calculados cuando la linea cambia de posición a raíz del movimiento de la trompa donde están los sensores.

▼ No hay una sincronización perfecta en la imagen anterior, así que veremos los valores calculados en los extremos con imágenes estáticas para mejorar nuestro análisis.

☼ Podemos observar que cuando la trompa de nuestro robot:

• Esta a punto de salirse por la izquierda el valor es 1485

• Esta a punto de salirse por la derecha el valor es 5718

Esto nos indica que el valor aumenta cuando la linea se aleja del centro hacia el lado derecho del robot y disminuye cuando lo hace hacia el lado izquierdo del robot.

► Calculo de Error de posición para control PID.

Ahora nos falta en el código hacer el calculo del error, como ya vimos en promedio ponderado la posición relativa del robot cuando la linea esta en el centro, si utilizamos 8 sensores o la regelta QTR8A ese valor calculado es de 3500 (set point).

Por lo que el error es:

Error = posición relativa - set point

▼ En el código vamos a agregar la creación de la variable tipo entera (int) Error junto a las variables anteriores:

/* Variable poscicion relativa*/

float ponderacion = 0;

float suma_lecturas = 0;

float pos_rel = 0;

int Error = 0;

▼ Luego de calculo de pos_rel colocamos la formula de calculo para el error con el set point correspondiente (depende de la cantidad de sensores que utilicemos):

/*cálculos de posición relativa*/

ponderacion = ((LS1*0)+(LS2*1)+(LS3*2)+(LS4*3)+(LS5*4)+(LS6*5)+(LS7*6)+(LS8*7));

suma_lecturas = (LS1+LS2+LS3+LS4+LS5+LS6+LS7+LS8);

pos_rel = 1000*(ponderacion/suma_lecturas);

Error = pos_rel - 3500;    // formula de error para PID

▼ Para el test vamos a imprimir la variable Error en el monitor serie, agregamos la linea correspondiente:

Serial.print("Error = ");Serial.println(Error);

► Realizamos un nuevo test para comprobación y poder sacar las conclusiones finales.

☼ Conclusiones finales.

• Cuando la línea esta del medio hacia la izquierda de la trompa de nuestro robot, el valor es negativo.

• Cuando la línea esta del medio hacia la derecha de la trompa de nuestro robot, el valor es positivo.

Por lo tanto:

• Si el robot, esta saliéndose hacia la derecha el valor es negativo.

• Si el robot, esta saliéndose hacia la izquierda el valor es positivo.

Resultante:

• Si esta saliéndose hacia la izquierda, debemos aumentar la velocidad del motor derecho y disminuir la del izquierdo.

• Si esta saliéndose hacia la derecha, debemos aumentar la velocidad del motor izquierdo y disminuir la del derecho.

Con el error calculado vamos a desarrollar el control PID que controla nuestro robot.

▼ Código para calculo de promedio ponderado final:

/* PINES SENSORES */

#define S1 A7

#define S2 A6

#define S3 A5

#define S4 A4

#define S5 A3

#define S6 A2

#define S7 A1

#define S8 A0

/* VARIABLES PARA SENSORES*/

int LS1 = 0;

int LS2 = 0;

int LS3 = 0;

int LS4 = 0;

int LS5 = 0;

int LS6 = 0;

int LS7 = 0;

int LS8 = 0;

/* Variable poscicion relativa*/

float ponderacion = 0;

float suma_lecturas = 0;

float pos_rel = 0;

int Error = 0;

void setup() {

 /* en las entadas analogicas

no son nesesarios los modos de pin */

Serial.begin(9600);          // incio monitor serial

}

void loop() {

promedio_ponderado();       

}

 void promedio_ponderado() {   // creamos la funcion vacia

// Lectura de sesnores //

LS1 = analogRead(S1);    // sensor extremo izquierdo

LS2 = analogRead(S2);

LS3 = analogRead(S3);

LS4 = analogRead(S4);

LS5 = analogRead(S5);

LS6 = analogRead(S6);

LS7 = analogRead(S7);

LS8 = analogRead(S8);    // sensor extremo derecho

/* Mapeo de lecturas */

LS1 = map(LS1, 0,1023,1000,0);

LS2 = map(LS2, 0,1023,1000,0);

LS3 = map(LS3, 0,1023,1000,0);

LS4 = map(LS4, 0,1023,1000,0);

LS5 = map(LS5, 0,1023,1000,0);

LS6 = map(LS6, 0,1023,1000,0);

LS7 = map(LS7, 0,1023,1000,0);

LS8 = map(LS8, 0,1023,1000,0);

/*calculos de posicion relativa*/

ponderacion = ((LS1*0)+(LS2*1)+(LS3*2)+(LS4*3)+(LS5*4)+(LS6*5)+(LS7*6)+(LS8*7));

suma_lecturas = (LS1+LS2+LS3+LS4+LS5+LS6+LS7+LS8);

pos_rel = 1000*(ponderacion/suma_lecturas);

Error = pos_rel - 3500;

/* Muestra de valores en monitor serial */

Serial.print(" | ");

Serial.print(LS1); Serial.print(" | ");

Serial.print(LS2); Serial.print(" | ");

Serial.print(LS3); Serial.print(" | ");

Serial.print(LS4); Serial.print(" | ");

Serial.print(LS5); Serial.print(" | ");

Serial.print(LS6); Serial.print(" | ");

Serial.print(LS7); Serial.print(" | ");

Serial.print(LS8); Serial.println(" | ");

delay(500);  // retardo de tiempo

/* Muestra de valores calculados posicion relativa*/

Serial.print("ponderacion = ");Serial.println(ponderacion);

Serial.print("suma = ");Serial.println(suma_lecturas);

Serial.print("pos_rel = ");Serial.println(pos_rel);

Serial.print("Error = ");Serial.println(Error);

/*Para las pruebas del robot ya en pista se deben comentar las impresiones del monitor serial*/

}