Кратко: хочу функцию, которая получает x,y,z,угол инструмента относительно оси OY, и будет выдавать мне углы для четырех серв. Формулу написал, в коде воплотил, но... Дуина не дружит с плавающей точкой и возвращает NAN NAN NANI Был бы очень благодарен, если кто то сможет подправить этот код и заставить его работать. Что Я пробовал: 1.Поменять типы данных на int, выдает белиберду 2.Поменять локальные переменные на глобальные и наоборот 3.Вынести вычисления из этой функции и считать напрямик 4.Танцевать с бубном и принести в жертву кота 5.Послать все и использовать готовую либу (отборный шлак) Сам код, библиотек не требует: Код (C++): const float a = 125; const float b = 125; const float L3 = 50; const float L0 = 80; float axis1; float axis2; float axis3; float axis4; void setup() { Serial.begin(9600); IK(0,125,155,90); Serial.print(axis2); Serial.print(' '); Serial.print(axis3); Serial.print(' '); Serial.println(axis4); } void loop() { } void IK(float x, float y, float z, float beta){ y = y-(cos(beta)*L3); z = z+L0+(sin(beta)*L3); float l = sqrt(x*x + y*y); float c = sqrt(l*l + z*z); axis1=acos(y/l); axis2=acos(b*b+c*c-a*a)/(2*b*c)+acos(l*l+c*c-y*y)/(2*x*c); axis3=180-axis2-acos(a*a+c*c-b*b)/(2*a*c); axis4=180-axis2-axis3+beta; }
Дружит вполне себе нормально. Вот только нужно следить, что бы аргумент у функции 'acos' не выходил за пределы -1 .. 1. Иначе ей (функции 'acos') немножечко плохо становится.
и еще не стоит забывать, что аргументы функций cos() sin() tan() - задаются в радианах. Судя по числам в коде ТС, я бы предположил, что у него там градусы.
Что не понял? Чем радианы отличаются от градусов? Так в 10-ом классе, дисциплина "тригонометрия" называется. Напомню - 180 градусов - это ПИ радиан. Полный оборот вектора - это 2ПИ радиан. Остальные градусы по аналогии.
Тригонометрию в школе прогулял? - на калькуляторе у тебя в косинусе градусы. а в ардуине - радианы. Разницу понимаешь? - твой код может в калькуляторе правильный ответ давать, а в ардуине нет.
У меня тригонометрии пока не было. Как я понял от parovoZZ, перевод градусов в радианы будет (Х*ПИ)/2, а наоборот 180Х/ПИ, все верно? Переделаю формулу под это и поделюсь результатом!
