Implementación del Código

El código se implementó en lenguaje de programación C, utilizando el programa CodeVision AVR. En este programa se creó un proyecto con la siguiente configuración:

Característica	Asignación
CHIP	AT90USB1286
Reloj Interno	2 MHz
‘scanf()’	long, width
‘printf()’	float, width, precision

El chip es el utilizado por el Teensy++, que trabaja con un reloj de 2MHz. Para poder utilizar las ecuaciones cinemáticas de manera precisa se cambiaron las propiedades de ‘scanf()’ y ‘printf()’ para poder utilizar números flotantes. Las librerías y las funciones que utiliza el programa se muestran a continuación.

//Librerías utilizadas #include <90USB1286.h> #include <delay.h> #include <stdio.h> #include <stdlib.h> #include <string.h> #include <math.h> #include <stdbool.h> //Funciones void frecuencia() void angular(int fi, int fd, float t) void direccion(int fi, int fd, float t) void ajustaAng(int y) bool distMin() void main(void)

La función frecuencia() utiliza un Timer para contar los flancos de subida de la señal cuadrada proveniente de los encoders durante un tiempo determinado  así calcular la frecuencia, angular() y direccion() hace los cálculos de cinemática para obtener los grados que ha girado el robot y la distancia que ha recorrido dependiendo del tiempo que haya pasado, ajustaAng() hace un desplazamiento de 90° al ángulo obtenido por trigonometría y así considerar que en un inicio el robot está orientado hacia el eje ‘y’ y por último distMin() regresa ‘verdadero’ si la distancia que mide el sensor ultrasónico es menor a 20cm. para detenerlo antes de provocar una colisión.

El diagrama de flujo del código que se presenta a continuación explica el procedimiento que sigue el Teensy para ejecutar la programación. Dependiendo del modo seleccionado, el robot tomará las variables como un set de coordenadas y un componente de velocidad o como una instrucción para el control manual, en el que se indica la dirección de movimiento y la velocidad de los motores.