roboforum.ru

-10   11,4   9,85   8,4
-9   12,21   10,75   9,44
-8   12,88   11,5   10,29
-7   13,45   12,13   10,98
-6   13,93   12,65   11,55
-5   14,32   13,07   12,01
-4   14,63   13,4   12,37
-3   14,87   13,66   12,65
-2   15,03   13,84   12,84
-1   15,13   13,94   12,95
0   15,17   13,98   12,99
1   15,13   13,94   12,95
2   15,03   13,84   12,84
3   14,87   13,66   12,65
4   14,63   13,4   12,37
5   14,32   13,07   12,01
6   13,93   12,65   11,55
7   13,45   12,13   10,98
8   12,88   11,5   10,29
9   12,21   10,75   9,44
10   11,4   9,85   8,4

-10   11,40175425099138   20,04987562112089   8,402325267042627
-9   12,206555615733702   10,754451150103323   9,443650760992956
-8   12,884098726725126   11,504699910719626   10,288088481701516
-7   13,45362404707371   12,128828005937953   10,98033988749895

#include <Servo.h>

Servo servo1;
Servo servo2;
String inputString = "";

void setup() {
Serial.begin(19200);
servo1.attach(5);
servo2.attach(6);
}

void loop() {
  CheckSerial();
  delay(100);
}

#define L1 65 //длина первого плеча манипулятора в мм.
#define L2 65 //длина второго плеча манипулятора в мм.
#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;// вариант, если серва1 повернута на 180°
  if(X>0)A=PI-A; // вариант, если серва1 НЕ повернута на 180°
  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-B;
  Serial.print("A= ");
  Serial.println(180*A/PI);
  Serial.print("B= ");
  Serial.println(180*B/PI);
  servo1.writeMicroseconds(RadianToMcs(A));
  servo2.writeMicroseconds(RadianToMcs(B));
  Serial.print("servo1= ");
  Serial.println(RadianToMcs(A));
  Serial.print("servo2= ");
  Serial.println(RadianToMcs(B));
}

int RadianToMcs(float rad)
{
  int grad=180*rad/PI;
  grad=map(grad, 0, 180, 500, 2500);
  return grad;
}

//Задавать строку координат в виде X,Y (Например: -40,60)
void CheckSerial()
{

  while (Serial.available())
  {
    char inChar = (char)Serial.read();
  if (inChar == '\n')
  {
   MakeCmd();
   inputString="";
   break;
  }
  else inputString += inChar;
  }
}
void MakeCmd()
{
  if(inputString.length()<3)return;
  int k=inputString.indexOf(',');
  if(k<0)return;
  float x=inputString.substring(0,k).toFloat();
  float y=inputString.substring(++k).toFloat();
  Serial.print("X= ");
  Serial.println(x);
  Serial.print("Y= ");
  Serial.println(y);
  SetServosToPoint(x,y);
}