Boa tarde,

o Robo em questão é um braço tipo escada com 2 graus de liberdade 

cada grau é controlado por um micro servo

Os valores são inseridos por um teclado matricial 4x4

E a comunicação com o programador é feita com LCD16x2

os valores digitados no teclado 4x4 são armazenados em variaveis tipo float,

O ERRO :

a função "acos()" É USADA PARA CONVERTER O PLANO CARTEZIANO EM ROTACIONAL mas, não esta respondendo com o valor que a equação responde quando testada no matlab

as equações são essas:(apartir da linha 121 do programa)

angulo_A1 = acos(((X^2)+(Y^2)-128)/128);
angulo_A1 = angulo_A1*57.29;  // para converter de rad para graus
angulo_A = angulo_A1;

Serial.println("A");
Serial.println(angulo_A);

angulo_B1 = acos(((2)*(X^2))/((2)*X*(((X^2)+(Y^2))^(1/2))));
angulo_B1 = angulo_B1*57.29; // para converter de rad para graus
angulo_B2 = (180-angulo_A1)/2;
angulo_B = angulo_B1 + angulo_B2;

Serial.println("B");
Serial.println(angulo_B);

antes de calcular o programa impromi os valores das variaveis e estão certos!

segue anexo o programa completo, vou fazer um video para vocer entender melhor!!