roboforum.ru

Код: Выделить всё • Развернуть

///////////// МАНИПУЛЯТОР ДЛЯ ГРАФИЧЕСКИХ ИЗОБРАЖЕНИЙ скетч СЕТКА КООРДИНАТНАЯ ДЕКАРТОВА ПРОТО //////////////////////////////
#include <Servo.h> 
#define L1 65 //длина первого плеча манипулятора в мм.
#define L2 65 //длина второго плеча манипулятора в мм.
#define Xmax 120
#define Ymax 120

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

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

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


//Функция выставляет 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-(b1+b2);
  if(X>0)B=PI-B+PI/2;
  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);
}

Madf писал(а):Блиин, парни, вы ему ща готовую штуку сделаете, а он побежит сразу в журнал печататься и детишек за бабки учить. Хотя да, согласен, хоть что-то правильное и полезное издаст.

Код: Выделить всё • Развернуть

///////////// МАНИПУЛЯТОР ДЛЯ ГРАФИЧЕСКИХ ИЗОБРАЖЕНИЙ скетч СЕТКА КООРДИНАТНАЯ ДЕКАРТОВА ПРОТО //////////////////////////////
#include <Servo.h> 
#define L1 65  //длина первого плеча манипулятора в мм.
#define L2 65  //длина второго плеча манипулятора в мм.
#define Xmax 120
#define Ymax 120
#define Acor1 352
#define Acor2 220
int Ymin=70;


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

void setup() {
 Serial.begin(9600);
 servo1.attach(5);
 servo2.attach(6);
 servo3.attach(7);
 servo1.write(32);
 servo2.write(110);
 servo3.write(90);
}

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


//Функция выставляет servo1 servo2, на точку с координатами X,Y
//X<0 для левого квадранта, X>0 для правого
void SetServosToPoint(int X, int Y)
{

  if(abs(X)>Xmax || Y>Ymax || Y<Ymin || X==0) return;
  X=-X;
  float L=sqrt(X*X+Y*Y);
  float a1=acos(X/L);
  float a2=acos((L*L+L1*L1-L2*L2)/(2*L*L1));
  float A=a1+a2;
  A=PI-A;// вариант, если серва1 повернута на 180°
  float b1=PI/2-a1;
  float B=acos((L2*L2 + L1*L1 - L*L) / (2*L2 * L1));
  Serial.print("A= ");
  Serial.println(180*A/PI);
  Serial.print("B= ");
  Serial.println(180*B/PI);
  servo1.writeMicroseconds(RadianToMcs(A)+Acor1);
  servo2.writeMicroseconds(RadianToMcs(B)+Acor2);
  float CurX=X;float CurY=Y;
//  Serial.print("servo1= ");
//  Serial.println(RadianToMcs(A));
//  Serial.print("servo2= ");
//  Serial.println(RadianToMcs(B));
  Serial.print("Current point: X= ");
  Serial.print(X);
  Serial.print(", Y= ");
  Serial.println(Y);
}

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);
}