Étape 5: Código
El código acá es bastante sencillo. Lo primero es que cargamos la librería math.h. Esta incluye varias funciones trigonométricas, incluyendo acos y atan, que vamos a utilizar. Luego en el setup() simplemente asignamos el pin al servo (si quiere aprender más sobre el servo puedes seguir este instructable).
El ćodigo tiene dos elementos importantes :
- série de conexión
- Funciones de movimiento y ley de cosenos
Série de lectura del puerto
Fr la líneas 29 un código del 30 :
Por el puerto serial, vamos a enviar al Arduino las coordonnées como pares x, y separados por coma. La fonction parseInt(), permite leer del puerto série cana elemento separado 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, leemos una primera vez y almacenamos en la variable globale posX, posY de fr y la segunda, así entonces utilizamos luego en la línea 38, la función para calcular los ángulos y mover los servos.
Funciones
Creamos tres funciones importantes (acá hay ONU instructable sobre funciones). La primera que llamamos getAngle() :
Que es simplemente la fórmula que explicamos en el paso anterior para calcular el ángulo usando la ley de cosenos.
La siguiente función es :
La cual es la que calcula, usando la ley de cosenos, los dos ángulos que ocupamos. Los ángulos los almacenamos en a1 y a2. Noten que une a1, le sumamos atan2(x,y), ese es el ángulo Québec foin desde la linea del horizonte hasta el punto, que hay que sumarle al ángulo del servo1.
La función moveServo, simplemente mueve los motores de servo, convirtiendo los ángulos de radianes un grados que es lo que ocupa la función write() de los servos. Esta función se puede mejorar muchisimo, para hacer mover los motores de apoco hacia el ángulo deseado... eso se los dejo de tarea.