//Programa para calcular la cinemática inversa de un manipulador planar
//de dos grados de libertad

//Librerias
#include <Servo.h>
Servo motor1;
Servo motor2;
//variables
float  nv=0;
float L1= 0;
float L2=0;
float angle1 ;      
float angle2 ;
float x;
float y;
float rad_angle1;  
float rad_angle2;
volatile float pi = 3.14159265359;

//configuación del Arduino
void setup() {
  Serial.begin(115200);
  motor1.attach(10);  //Selección del GPIO 10
  motor2.attach(9);  //Selección del GPIO 9
 
 // envia a los servos a su posición inicial
  motor1.write(0);
  motor2.write(0);
  Serial.println("Introduzca datos");
}
//Rutina para evitar el valor cero que envia la función parseFloat
void no_valor()
{
 while (Serial.available() == 0){}           
 nv = Serial.parseFloat();   
}

//Programa principal
void loop() {
 //captura de datos 
      Serial.println ("longitud del primer enlace");
     while (Serial.available() == 0){}           
          L1 = Serial.parseFloat();
          no_valor(); //función para evitar el cero de la captura de teclado
     Serial.println ("longitud del segundo enlace");   
     while (Serial.available() == 0){}         
           L2 = Serial.parseFloat();
           no_valor(); 
     Serial.println("Proporciona el valor de X ");
      while(Serial.available()==0){}
           x=Serial.parseFloat();
           no_valor();
      Serial.println("Proporciona el valor de Y ");
      while(Serial.available()==0){}
           y=Serial.parseFloat();
           no_valor();
     //cálculo de los ángulos
      rad_angle2 = acos((sq(x)+ sq(y) - sq(L1) - sq(L2)) / (2*L1*L2));
      rad_angle1= atan(y / x) - atan((L2*sin(rad_angle2)) / (L1+ L2*cos(rad_angle2)));
      delay(1000);
     //conversión de radianes a grados
      angle1= (rad_angle1*180)/pi;
      angle2= (rad_angle2*180)/pi;
      
 //despliega los datos de entrada y los resultados    
 Serial.print("x is "); 
 Serial.println(x);
 Serial.print("y is "); 
 Serial.println(y);
 Serial.println("Los resultados de angulos son  ");
 Serial.print("angle1 is  "); 
 Serial.println(angle1);
 Serial.print("angle2 is "); 
 Serial.println(angle2);
 motor1.write(angle1); 
 motor2.write(angle2);
 delay(10000);  
 // envia a los servos a su posición inicial
 motor1.write(0);
 motor2.write(0);
 delay(5000);   
}