Первый вариант функции не был рассчитан на отрицательные значения Х. Прежде чем управлять сервами, надо посмотреть что функция считает. Проверить правильно ли. Я не проверял. Надо дуинку прикупить )))
Добавлено спустя 41 минуту 34 секунды:При сборке манипулятора нужно сначала сервы выставить упр. Сигналом на 90 гр. А потом прикрутить штанги соотв. Иначе управлять расчетным методом будет сложно. Надо какие то смещения вводить.
Добавлено спустя 1 час 2 минуты 57 секунд:Попробуй так для начала:
Код:
#define L1 100 //длина первого плеча манипулятора в мм.
#define L2 100 //длина второго плеча манипулятора в мм.
#define Xmax 150
#define Ymax 150
//Функция выставляет servo1 servo2, на точку с координатами X,Y
//X<0 для левого квадранта, X>0 для правого
void SetServosToPoint(float X, float Y)
{
if(abs(X)>Xmax || Y>Ymax || Y<0 || X==0) return;
float L=sqrt(X*X+Y*Y);
float a1=acos(abs(X)/L);
float a2=acos((L*L+L1*L1-L2*L2)/2*L*L1);
float A=a1+a2;
if(X>0)A=PI-A;
float b1=PI/2-a1;
float b2=acos((L2*L2+L*L-L1*L1)/2*L2*L);
float B=PI/2-(b1+b2);
if(X>0)B+=PI/2;
//servo1.writeMicroseconds((int) map(A, 0, PI, 500, 2500));
//servo2.writeMicroseconds((int) map(B, 0, PI, 500, 2500));
Serial.print("A= ");
Serial.println(map(A, 0, PI*2, 0, 360));
Serial.print("B= ");
Serial.println(map(B, 0, PI*2, 0, 360));
}
Если будет выдавать туфту, то надо найти ошибку. Вступаем в увлекательный период отладки/пуско-наладки!
Добавлено спустя 9 минут 33 секунды:Судя по фото, серва S1 повернута на 180°.В этом случае все углы для нее будут зеркальные, что надо учитывать в расчете.