Stap 5: Código
El código whinose es bastante sencillo. Lo primero es que cargamos la librería math.h. Esta bevat varias funciones trigonométricas, nieuwe acos y atan, que vamos een utilizar. Luego nl 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:
- Conexión seriële
- funciones de movimiento y ley de cosenos
LECTURA del puerto seriële
Nl la líneas 29 een 30 del código:
if (Serial.available() > 0){ posX = Serial.parseInt(); posY = Serial.parseInt();} Por el puerto serieel, vamos een enviar al Arduino las coordenadas como uitsluitend x, y en por coma. La función parseInt(), permite leer del puerto seriële 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í. Nl este caso leemos una primera vez y almacenamos nl la variabele global posX, y la segunda nl posY, así entonces utilizamos luego nl la línea 38, la función para calcular los ángulos y verhuizer los servo's.
Funciones
Creamos tres funciones importantes (whinose hay un instructable sobre funciones). La primera que llamamos getAngle():
float getAngle(float a, float b, float c) { return acos((square(b) + square(c) - square(a)) / (2 * b * c));} 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:
void armTo(float x, float y) { float h = sqrt(square(x) + square(y)); //calculates angles based on desired point float a1 = (getAngle(ARMLENGTH, ARMLENGTH, h) + atan2(y, x)); float a2 = (getAngle(h, ARMLENGTH, ARMLENGTH)); moveServo(a1, a2);} La cual es la que calcula, los dos ángulos que ocupamos usando la ley de cosenos. Los ángulos los almacenamos nl a1 y a2. Noten que een a1, le sumamos atan2(x,y), ese es el ángulo que hay desde la linea del horizonte hasta el punto, que hay que sumarle al ángulo del servo1.
Simplemente mueve los servo motores, La función moveServo, convirtiendo los ángulos de radianes een grados que es lo que ocupa la función write() de los servo's. esta función se puede mejorar muchísimo, para hacer verhuizer los motores de apoco is el ángulo deseado... eso se los dejo de tarea.
