Acos и сложные математические вычисления на ардуино

Здравствуйте! Возникла задача - сделать манипулятор с двумя суставами. Управлять планируется только последней точкой манипулятора. Схема: Отрезки c и a - "кости" между суставами, имеют постоянное значение, точка B - первый сустав, точка A - второй сустав, задается расположение точки С (конец манипулятора) с помощью значения l и значения h. Манипулятором является только BAC, остальное лишь вспомогательные отрезки. Нужно вычислить угол A и угол ABD.
Нашел в интернете формулу расчета углов по сторонам треугольника. Получилось, что угол A равен арккосинусу из , а угол ABD равен сумме арккосинуса и арккосинуса . При этом . Написав код, несколько раз проверил, хотя до сих пор не выдает правильные значение. В чем проблема? Выдает значение либо nan или ovf на запрос о значении арккосинуса, либо просто неправильные значения еще до арккосинуса( выходит за рамки от -1 до 1). Заранее благодарен за помощь.

Код (C++):

#include <Servo.h>
#include <Math.h>
Servo serv1;
Servo serv2;
Servo serv3;
Servo serv4;
int Jox = A1;
int Joy = A0;
float c = 15.2;
float a = 13.8;
float b;
float h;
float l;
void setup() {
   serv1.attach(11);
   serv2.attach(10);
   serv3.attach(9);
   serv4.attach(8);
pinMode(Jox,INPUT);
pinMode(Joy,INPUT);
Serial.begin(9600);
}
void loop() {
  l = analogRead(Jox);
  h = analogRead(Joy);
  l = map(l,0,1023,1,180);
  h = map(h,0,1023,1,180);
  l = sq(l);
  h = sq(h);
  b = sqrt(l+h);
  float f = sq(a)+sq(c)-sq(b);
  float m = 2*c*a;
  float A =f/m;
  A = acos(A);
  float k = sq(c)+sq(b)-sq(a);
  float u = 2*c*b;
  float o = sq(b)+sq(l)-sq(h);
  float p = 2*b*l;
  float q = k/u;
  q = acos(q);
  float z = o/p;
  z = acos(z);
float B = q+z;
B = round(B);
  serv3.write(A);
  delay(50);
  serv2.write(B);
  delay(50);
 
}

 

Код не проверяю - занят. А вот на вскидку: исключите любую возможность деления на ноль.
Одним словом делитель не ноль!
У вас там есть ряд мест... я при возможности такого условия к переменным с такой возможностью вероятности прибавляю очень маленькое значение(вроде и ноль, но не ноль вовсе). В ряде случаев и отрицательные значения не допускаются.
Сами подсчитайте. Желаю успехов!

 

Проверил как считается у вас угол А.
Для этого задал такие параметры. Обозначения согласно вашего чертежа
а = 5
c = 10
угол А = 60 градусов. Для простоты, вель cos 60 = 0.5
По теореме косинусов вычислил b. Получил 8,66.
В прямоугольном треугольнике BCD принял I = 4. Тогда h будет 7,68.
Подставил это напрямую в ваш скетч

Спойлер: код

Код (C++):

//#include <Servo.h>
#include <Math.h>
/*Servo serv1;
Servo serv2;
Servo serv3;
Servo serv4;
*/
int Jox = A1;
int Joy = A0;
float c = 10;
float a = 5;
float b;
float h;
float l;
void setup() {
   /*
   serv1.attach(11);
   serv2.attach(10);
   serv3.attach(9);
   serv4.attach(8);
pinMode(Jox,INPUT);
pinMode(Joy,INPUT);
*/
Serial.begin(9600);
}
void loop() {
  /*
  l = analogRead(Jox);
  h = analogRead(Joy);
  l = map(l,0,1023,1,180);
  h = map(h,0,1023,1,180);
  */
  l = 4;
  h = 7.68;
  l = sq(l);
  h = sq(h);
  b = sqrt(l+h);
  Serial.println(b);
  //b = 8.66;
  float f = sq(a)+sq(c)-sq(b);
  float m = 2*c*a;
  float A =f/m;
  A = acos(A);

Serial.println(A);
  delay(2000);
  /*
  float k = sq(c)+sq(b)-sq(a);
  float u = 2*c*b;
  float o = sq(b)+sq(l)-sq(h);
  float p = 2*b*l;
  float q = k/u;
  q = acos(q);
  float z = o/p;
  z = acos(z);
float B = q+z;
B = round(B);
  serv3.write(A);
  delay(50);
  serv2.write(B);
  delay(50);
  */
}

В результате b = 8.66, угол А = 1,05 что соответствует 60 градусам в радианах.
Математика у вас правильная. Смотрите, что вы получаете с аналоговых входов. Скорее всего проблема там.

 

Спасибо большое за помощь! Обязательно проверю!

 

Сначала не понял, как получить ненулевое значение, потом понял. Спасибо большое!

 

У меня были подозрения по поводу самой функции acos(), сейчас опробовал ее отдельно и выводится не то, что я ожидал: при любых значениях выводится nan при следующем коде:

Код (C++):

void setup() {
Serial.begin(9600);
}
void loop() {
float n = Serial.read();
n = acos(n);
Serial.println(n);
delay(1000);
}

Решил проверить, может проблема еще раньше и узнал, что в строке ввода монитора порта уже стоит значение -1. Сам арккосинус высчитывает все правильно ( подставил значения в коде : acos(-1) или acos(0.7) и т.д.), но что-то не так при вводе значений с внешних устройств, потому что всегда, когда пытаюсь посчитать арккосинус с заранее неизвестными, вводимыми числами, ардуина выставляет мне nan. Короче говоря, acos() выдает nan при вводимых числах.
Пример 1:

Код (C++):

void setup() {
Serial.begin(9600);
}
void loop() {
float n = acos(0.7);
Serial.println(n);
delay(1000);
}

Выдает 0.80, тут все в порядке.
Пример 2:

Код (C++):

void setup() {
Serial.begin(9600);
}
void loop() {
  float a = Serial.read();
float n = acos(a);
Serial.println(n);
delay(1000);
}

При вводе любых значений кидает nan.
Теперь возникает вопрос, что происходит с функцией и как это исправить? Заранее благодарен за ответ,

 

Я на вскидку не помню, а искать сейчас некогда, но там передаются цифры в кодах ascii. 0 это 48, 1 - 49 и т.д. Дробные числа вообще передаются в виде строки.
Ну и так как вы читаете, данные из Serial не принимают. Надо проверять наличие пришедших данных с помощью Serial.available(), и если там что-то есть, тогда читать.

 

Спасибо большое, напишу что получилось сразу как опробую

 

Получилось считать только нужные значения, а теперь вопрос - как их преобразовать в читаемые? То есть преобразовать ASCII в float? У меня выводится ( когда ввожу 1 ) 49 13 10. Я даже не знаю как эти числа из монитора порта выцепить. Заранее благодарен за ответ!

 

https://ru.wikipedia.org/wiki/Atof_(Си)

 

Огонь. Спасибо! Сейчас опробую.

 

Спасибо за помощь, но мне это не подходит, мне нужно именно перевод типа String в тип int для того, чтобы перевести числа из ASCII в число float.

 

функция atof занимается переводом строки char* в float. переделать string в строку char* очень просто. при чем тут int непонятно

 

Мне не нужно переводить строку сразу в float. Я пока не понимаю в целом путь значений от ввода до выход, поэтому экспериментирую и разбираюсь.

 

Я хотел сделать следующее: у меня по цифрам выходят значения в ASCII ( например 49 13 10), преобразовать их в отдельные char, а затем склеить их в String, а уже данный String перевести в float.

 

По крайней мере сейчас хочу

 

Вам советуют как это делать правильно.
А хотите изврата - делайте как хотите, но тогда зачем спрашивать?

 

Думаю, мне решать что для меня изврат, а что нет. То, что мне посоветовали, переводить цифру в тип float, а мне сначала нужно расшифровать то, что приходит с ASCII, а потом уже переводить в нужный тип данных.

 

оно переводит во float не цифру, а строку ASCII. если у вас приходит строка, в которой вместо например "12.34" написано что-то вроде "двенадцать целых тридцать четыре сотых", то таки придется поизвращаться.

вернее не во float а в double, но это уже тонкости

 

я так понял что основная проблема в том чтобы в принципе собрать строку из того что приходит в сериал?
для этого проще всего использовать что-то вроде этого http://wikihandbk.com/wiki/Arduino:Справочник_языка_Arduino/Библиотеки/Serial/readStringUntil()