Paso 5: Código
El código acá es bastante sencillo. Lo primero es cargamos la librería math.h. Esta incluye varias funciones trigonométricas, incluyendo acos y atan, vamos a utilizar. Luego en el setup() simplemente asignamos el pin al servo (si quiere aprender más sobre el servo puedes seguir este instructivo).
El ćodigo tiene dos elementos importantes:
- conexión serial
- funciones de movimiento y ley de cosenos
Lectura del puerto serial
En la líneas 29 un código del 30:
Por el puerto serie, vamos a enviar al Arduino las coordenadas como pares x, y separados por coma. La función parseInt(), permite leer del puerto serial cana elemento frente por coma. Al ejecutarlo por primera vez, lee el primero, hasta la coma, al ejecutarlo la segunda, lee inmediatamente después de la coma el siguiente y así. En este caso, vive una primera vez y almacenamos en la variable global posX, posY de at y la segunda, así entonces cuántos luego en la línea 38, los la función para calcular los ángulos y mover los servos.
Funciones
Creamos tres funciones importantes (acá hay un instructable sobre funciones). La primera llamamos getAngle(se):
Es simplemente la fórmula explicamos en el paso anterior para calcular el ángulo usando la ley de cosenos.
La siguiente función es:
La cual es la calcula, usando la ley de cosenos, los dos ángulos ocupamos. Los ángulos los almacenamos en a1 y la a2. Noten que un a1, le sumamos atan2(x,y), ese es el ángulo que hay desde la linea del horizonte hasta el punto, hay ubicandose al ángulo del servo1.
La función moveServo, simplemente mueve los servo motores, convirtiendo los ángulos de radianes a grados es lo ocupa la función write() de los servos. Esta función se puede mejorar muchísimo, para hacer mover los motores de apoco hacia el ángulo deseado... eso se los dejo de tarea.
