roboforum.ru

По 10мм с шагом 45 градусов, по линейке 20мм. Где то 2 потерялась лишняя.

Кривовастенько. Сейчас шаг Dstep 3 мм стоит или увеличивал?

Не, ничего не трогал, даже не догадываюсь почему в два раза длиннее рисует.
Пытаюсь понять чего моя функция с отрицательными Х врёт. Скоро "скину свой крест".

Добавлено спустя 27 минут 18 секунд:
Такой.

Ничем не хуже. Но функция у тебя больно сложная получилась.

Но понять почему не так рисует в отрицательных Х не могу. Придётся переделывать. Шаг нужен 1мм всё равно.

dimamichev писал(а):даже не догадываюсь почему в два раза длиннее рисует.

А я догадался. Вот исправленная функция:

Код: Выделить всё

void DrawVector(int d, float a)
{
  if (d > Dmax || a < 0 || a > 359)return; // не корректные параметры
  if (CurX == 1000 || CurY == 1000)return; // не задана начальная точка
  if (d < 0) //задана перестановка пера
  {
    //up_down_pen(false);//поднимем перо
    d = abs(d);
    GoToFinLine(d, a);
  } else //задано рисование линии
  {
    up_down_pen(true);//опустим перо
    int n = d / Dstep; //разобьем линию на n отрезков.
    for (int i = 1; i <= n; i++)GoToFinLine(Dstep, a);//нарисуем отрезки по очереди
    d -= n * Dstep;
    if (d > 0)GoToFinLine(d, a); //последнюю точку нарисуем отдельно для точности.
  }
}

Добавлено спустя 5 минут 36 секунд:

Добавил координатную сетку с шагом 10мм.
Теперь каждый сможет виртуально поиграться с твоей рисовалкой

Добавлено спустя 5 минут 8 секунд:
Следующий шаг - кружочки и дуги.

Предлагаю задавать дуги в виде того же вектора, вокруг которого будет описываться правильная дуга.
Т.о. сохранится масштабируемость и легкое изменение направления надписи под любым углом.
Если отрицательное значение длины вектора говорит о том, что надо переместиться сразу к концу вектора с поднятым пером, то отрицательное значение угла будет сообщать, что вокруг вектора надо описать дугу. Окружность, соответственно, опишется 2мя встречно направленными векторами.
Буква Р в примере будет описана 4мя парами целых чисел.

Это победа (номер сообщения 1945)!
Попробую разобраться со всеми функциями в скетче (а ведь это ещё только отрезки). И поиграться с эмулятором.

Добавлено спустя 3 часа 47 минут 24 секунды:

Код: Выделить всё

void loop() {
// SetServosToPoint(h,g);//перемещает стержень-перо в точку с координатами h,g
//DrawVector(h,g);//рисует отрезок длиной h,в направлении (под углом) g
// GoToFinLine (h,g);//перемещает перо в заданных параметрах длины и угла h,g
//up_down_pen(false);//поднять перо-стержень
// up_down_pen(true);//опустить перо-стержень

while(t<5)
{
//РИСУЕМ ДОМИК много раз потому что карандас
SetServosToPoint(0,70);
DrawVector(20,90);DrawVector(20,180);DrawVector(20,270);DrawVector(20,0); up_down_pen(false);
GoToFinLine (7,120);

DrawVector(10,90);DrawVector(10,180);DrawVector(10,270);DrawVector(10,0); up_down_pen(false);
GoToFinLine (35,135);

DrawVector(30,0);DrawVector(10,345);DrawVector(30,180); DrawVector(10,90); up_down_pen(false);


t++;}
}


Подправил-разкомментировал строку X=-X; - а то ставились точки симметрично оси ОУ (положительные х вместо отрицательных и наоборот)
И установил #define Dstep 1. В итоге перестали ставиться правильно углы в векторах. DrawVector(10,90); рисует правильно, а DrawVector(10,45); или DrawVector(10,89); как DrawVector(10,0); Кстати в таких изменениях углы стали отсчитываться "нормально" - от положительной полуоси ОХ, против часовой стрелки.

Против ЧС разве нормально? Я наоборот добивался по ЧС. Так мозгам привычнее: положительные Х справа, угол от гор. оси по ЧС. Я пока все оставил как есть. Почему-то, когда расчет идет через дуню, то эмулятор выставляет "сервы" слегка менее точно, чем, когда считает самостоятельно. Хотя алгоритмы у них одинаковые. Дуина дает небольшую погрешность не понятно почему.
Ладно, займемся дугами.

Добавлено спустя 7 минут 27 секунд:

dimamichev писал(а):Это победа (номер сообщения 1945)!

Во как! Надо отметить!

dimamichev писал(а):И установил #define Dstep 1

С отрезком длиной в 1мм посчитать ничего не получится. Единица она и есть единица. Меньше 3х думаю не стоит.

Ну вот, полчасика разминки мозгов и пошли дуги:

Осталось переделать в дунькин код и опробовать. Ну это, когда найду еще кусочек времени.

Добавлено спустя 3 минуты 38 секунд:

Scorpio писал(а):...то отрицательное значение угла будет сообщать, что вокруг вектора надо описать дугу

Так, кстати, не получается, т.к. направление 0° остается неопределенным. Чтобы указать программе, что надо описывать дугу вокруг вектора, будем добавлять к значению угла 1000: "30:45"-вектор, "30:1045"-дуга.

Дуги рисуются множеством векторов?

Точно так. Попробовал на дуне. Почему то получается кривее и короче, хотя функция та же

Добавлено спустя 1 час 33 минуты 41 секунду:
Исправил типы переменных - стало точнее. Вот такая стала функция

Код: Выделить всё

//Рисует отрезок длины d в направлении a(в градусах)
//либо описывает правильную дугу, с диаметром d в направлении a(значения угла задаются как 1000+а)
//либо переставляет перо на конец вектора, при отрицательных d
void DrawVector(int d, float a)
{
  bool IsArc = false;
  if (a > 999) {
    IsArc = true;  //Чтобы обозначить рисование дуги, к значению угла добавляется 1000
    a -= 1000;
  }
  if (d > Dmax || a < 0 || a > 359)return; // не корректные параметры
  if (CurX == 1000 || CurY == 1000)return; // не задана начальная точка
  if (d < 0) //задана перестановка пера
  {
    up_down_pen(false);//поднимем перо
    d = abs(d);
    GoToFinLine(d, a);
  } else //задано рисование
  {
    up_down_pen(true);//опустим перо
    if (!IsArc) //рисование отрезка
    {
      int n = d / Dstep; //разобьем линию на n отрезков.
      for (int i = 1; i <= n; i++)GoToFinLine(Dstep, a);//нарисуем отрезки по очереди
      d -= n * Dstep;
      if (d > 0)GoToFinLine(d, a); //последнюю точку нарисуем отдельно для точности.
    }
    else //рисование дуги
    {
      //посчитаем координаты центра окружности
      float x1, y1;
      float x0 = CurX + d / 2 * cos(GradToRad(a));
      float y0 = CurY + d / 2 * sin(GradToRad(a));
      int nn = d * PI/2 / Dstep; //разобьем дугу на n отрезков.
      float da = PI / nn;// вычислим угловой шаг
      float aa = GradToRad(a);
     
      for (int i = 1; i <= nn; i++)
      {
        //будем вычислять координаты точек, принадлежащих дуге
        aa += da;
        if(aa>PI*2)aa-=PI*2;
        //Будем считать из центра окружности в обратном направлении заданному
        x1 = x0 - d / 2 * cos(aa);
        y1 = y0 - d / 2 * sin(aa);
        //и рисовать линию апроксимации
        SetServosToPoint(x1, y1);
        delay(pause*3);
      }
    }

  }
}


Надо еще отключить приведение угла //a = constrain(a, 0, 360); в функции MakeCmd(). Иначе тысячи не пропустит.
Еще заменил byte pause = 30; // единица временного интервала, т.к. n в качестве глобальной переменной меня раздражала
Собственно, мы вплотную подошли к буковкам.....

Добавлено спустя 3 часа 15 минут 13 секунд:

Код: Выделить всё

#include <Servo.h>

Servo servo1;//указываем сервопривод основного поворота (при увел. угла - поворот рычага ВЛЕВО)
Servo servo2;//указываем сервопривод дополнительного поворота (при увел. угла - поворот рычага ВПРАВО)
Servo servo3;//указываем сервопривод подъёмника (при увел. угла - поворот рычага ВВЕРХ)

byte n=30;// единица временного интервала

#define S1_0 110 //начальный угол 1 сервопривода
#define S2_0 114 //начальный угол 2 сервопривода
#define S3_UP 110 //начальный угол 3 сервопривода (угол подъёма фломастера над листом)
#define S3_DW 85  //угол 3 сервопривода (угол опускания фломастера на лист)


String inputString = "";

//текущее положение пера
int CurX=1000;
int CurY=1000;
// Текущее состояние пера опущено/поднято
boolean PenIsDown=false;
//параметры манипулятора
#define L1 65  //длина первого плеча манипулятора в мм.
#define L2 65  //длина второго плеча манипулятора в мм.
#define Xmax 120//значение граничных координат
#define Ymax 115//значение граничных координат
#define Acor1 352//поправка на посадку качалки сервы 1
#define Acor2 220//поправка на посадку качалки сервы 2
#define Ymin 50//значение граничных координат
// ограничение длины вектора в мм
#define Dmax 50
// разбивать линию на точки через каждые Dstep в мм
#define Dstep 3

//Для аварийной остановки в процессе рисования
boolean StopFlag=false;
//Для работы с эмулятором заменить на true
boolean EmulatorFlag=false;
//boolean EmulatorFlag=true;

void setup() {
Serial.begin(19200);
if(!EmulatorFlag)
   {
     servo1.attach(5);// подключаем переменную servo к соответствующему выводу платы Ардуино
     servo2.attach(6);
     servo3.attach(7);
   }
Parking();
Serial.println("RBT");
delay(1000);
}
//////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////
void loop() {
//CheckSerial();
  Parking();
  SetServosToPoint(10,90);DrawVector(20,1000);up_down_pen(false);
 
 
  delay(100);
me:goto me;
}
//////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////
//выдержка времени, возвращающая true, если пришел символ в COM (ПОКА НЕ ЗАДЕЙСТВОВАНА)
//boolean DelayWithStop(int ms)
//{
//  while (!Serial.available())
//  {
//    delay(5);
//    ms-=5;
//    if(ms<0)break;
//  }
//  StopFlag=Serial.available();
//  return Stopflag;
//}

///////////////////////////////////////////////////////////////////////////
// устанавливает механизм в начальное положение
void Parking()
{
servo3.write(S3_UP);
delay(1000);
servo1.write(S1_0);//начальная установка положения
servo2.write(S2_0);
CurX=1000;CurY=1000;//Начальная точка для рисования не определена.
}
/////////////////////////////////////////////////////////////////////////
void  up_down_pen(boolean  down)
{
  if(PenIsDown^down)
  {
    byte a3;
    if(down)
      {
        if(!EmulatorFlag)
        {
          a3=S3_UP;
          while (a3>S3_DW){servo3.write(a3--);delay(n);}
        }
        else Serial.println("DWN");
      }
      else 
      {
        if(!EmulatorFlag)
        {
          a3=S3_DW;
          while (a3<S3_UP){servo3.write(a3++);delay(n);}
        }
        else Serial.println("UP"); 
      }
    PenIsDown=down;
  }
   Serial.print("Pen is ");
   Serial.println(PenIsDown);
}
////////////////////////////////////////////////////////////////////////////////
//Рисует отрезок длины d в направлении a(в градусах)
//либо описывает правильную дугу, с диаметром d в направлении a(значения угла задаются как 1000+а)
//либо переставляет перо на конец вектора, при отрицательных d
void DrawVector(int d, float a)
{
  bool IsArc = false;
  if (a > 999) {
    IsArc = true;  //Чтобы обозначить рисование дуги, к значению угла добавляется 1000
    a -= 1000;
  }
  if (d > Dmax || a < 0 || a > 359)return; // не корректные параметры
  if (CurX == 1000 || CurY == 1000)return; // не задана начальная точка
  if (d < 0) //задана перестановка пера
  {
    up_down_pen(false);//поднимем перо
    d = abs(d);
    GoToFinLine(d, a);
  } else //задано рисование
  {
    up_down_pen(true);//опустим перо
    if (!IsArc) //рисование отрезка
    {
      int n = d / Dstep; //разобьем линию на n отрезков.
      for (int i = 1; i <= n; i++)GoToFinLine(Dstep, a);//нарисуем отрезки по очереди
      d -= n * Dstep;
      if (d > 0)GoToFinLine(d, a); //последнюю точку нарисуем отдельно для точности.
    }
    else //рисование дуги
    {
      //посчитаем координаты центра окружности
      float x1, y1;
      float x0 = CurX + d / 2 * cos(GradToRad(a));
      float y0 = CurY + d / 2 * sin(GradToRad(a));
      int nn = d * PI/2 / Dstep; //разобьем дугу на n отрезков.
      float da = PI / nn;// вычислим угловой шаг
      float aa = GradToRad(a);
     
      for (int i = 1; i <= nn; i++)
      {
        //будем вычислять координаты точек, принадлежащих дуге
        aa += da;
        if(aa>PI*2)aa-=PI*2;
        //Будем считать из центра окружности в обратном направлении заданному
        x1 = x0 - d / 2 * cos(aa);
        y1 = y0 - d / 2 * sin(aa);
        //и рисовать линию апроксимации
        SetServosToPoint(x1, y1);
        delay(n*3);
      }
    }

  }
}
///////////////////////////////////////////////////////////////////////////////
// функция перемещает перо в конец заданного вектора
void GoToFinLine(int d,float a)
{
  if(StopFlag)return;
  //определим координаты конца вектора
    int x=CurX+d*cos(GradToRad(a));
    int y=CurY+d*sin(GradToRad(a));
   //переместим перо
    SetServosToPoint(x,y);
    delay(n*d);// выдержка времени для отработки механизма, зависящая от длины пути.
}
//////////////////////////////////////////////////////////////////////////////////
//Функция выставляет servo1 servo2, на точку с координатами X,Y
//X<0 для левого квадранта, X>0 для правого
void SetServosToPoint(int X, int Y)
{
  if(abs(X)>Xmax || Y>Ymax || Y<Ymin) return;
  CurX=X;CurY=Y;
  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);
  if(!EmulatorFlag)
    {
      servo1.writeMicroseconds(RadianToMcs(A)+Acor1);
      servo2.writeMicroseconds(RadianToMcs(B)+Acor2);
    }
    else
    {
      Serial.print(RadianToMcs(A));   
      Serial.print("&");   
      Serial.println(RadianToMcs(B));     
    }

//  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;
  return map(grad, 0, 180, 500, 2500);
}
/////////////////////////////////////////////////////////////////////
float GradToRad(float grad)
{
  float rad=PI*grad/180;
  return rad; 
}
////////////////////////////////////////////////////////////////////////////////////////////
//Задавать строку координат в виде X,Y (Например: -40,60)
// Либо вектор в виде d:a, где d - длина вектора в мм (отрицательное значение состветствует проходу с поднятым пером),
// a - угол направления рисования в диапазоне 0...360°

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;
  if(inputString.startsWith("RBT?")){Serial.println("RBT");return;}//подключение к эмулятору
  int k=inputString.indexOf(',');
  if(k>0)//получены координаты точки
  {
    int x=inputString.substring(0,k).toInt();
    int y=inputString.substring(++k).toInt();
    x=constrain(x, -Xmax, Xmax);
    y=constrain(y, Ymin, Ymax);
    Serial.println("Set point:");
    Serial.print("X= ");
    Serial.print(x);
    Serial.print(" Y= ");
    Serial.println(y);
    SetServosToPoint(-x,y);
  }else
  {
    k=inputString.indexOf(':');
    if(k<0){Serial.println("!!!");return;} //не правильная команда
    //получены параметры вектора
    int d=inputString.substring(0,k).toInt();
    d=constrain(d, -Dmax, Dmax);
    float a=inputString.substring(++k).toFloat();
   // a=constrain(a, 0, 360);
    Serial.println("Draw vector:");
    Serial.print("d= ");
    Serial.print(d);
    Serial.print(" a= ");
    Serial.println(a);
    DrawVector(d,a);
    up_down_pen(false);//поднимем перо
    if(!EmulatorFlag)Parking();//пока убираем, чтобы посмотреть результаты
  }

}
//////////////////////////////////////////////////////////////////////////////////////



Попробовал новую функцию (n пока не тронул, страшно чего то потерять)-через монитор порта рисует всё правильно. Попробовал через функции непосредственно в "лууп"(скетч прикладываю).Точку старта ставит симметрично ОХ, пришлось опять активизировать Х=-Х;(отсчёт углов соответственно тоже меняется).
Попробую буквы через монитор.

Добавлено спустя 3 минуты 2 секунды:
Тока чего они такие большие (прямо красная шапочка ? Надо сразу по 10мм пробовать.

Добавлено спустя 1 минуту 47 секунд:
И по функциям в луупе тоже...

Буквы легко масштабируются (d*k) и поворачиваются (а+n) в этом был и смысл

Добавлено спустя 4 минуты 12 секунд:
Через монитор порта правильно, потому что там х переворачивается при приеме. В лупе тоже х надо перевернуть в -х. Но в общем как нравится. Моя миссия подходит к концу

Да конечно не только в этом. Такой ещё момент (видимо я не до понимаю). В мониторе порта после посылки координат точки и рисования вектора-дуги, перо возвращается в исходное положение. А выставить новую координату для рисования следующего вектора естественно не могу. Всю посылку векторов монитор не воспринимает. Как рисовать букву?, через монитор. Речь идёт о такой версии кода:

Код: Выделить всё

#include <Servo.h>

Servo servo1;//указываем сервопривод основного поворота (при увел. угла - поворот рычага ВЛЕВО)
Servo servo2;//указываем сервопривод дополнительного поворота (при увел. угла - поворот рычага ВПРАВО)
Servo servo3;//указываем сервопривод подъёмника (при увел. угла - поворот рычага ВВЕРХ)

byte n=30;// единица временного интервала

#define S1_0 110 //начальный угол 1 сервопривода
#define S2_0 114 //начальный угол 2 сервопривода
#define S3_UP 110 //начальный угол 3 сервопривода (угол подъёма фломастера над листом)
#define S3_DW 85  //угол 3 сервопривода (угол опускания фломастера на лист)


String inputString = "";

//текущее положение пера
int CurX=1000;
int CurY=1000;
// Текущее состояние пера опущено/поднято
boolean PenIsDown=false;
//параметры манипулятора
#define L1 65  //длина первого плеча манипулятора в мм.
#define L2 65  //длина второго плеча манипулятора в мм.
#define Xmax 120//значение граничных координат
#define Ymax 115//значение граничных координат
#define Acor1 352//поправка на посадку качалки сервы 1
#define Acor2 220//поправка на посадку качалки сервы 2
#define Ymin 50//значение граничных координат
// ограничение длины вектора в мм
#define Dmax 50
// разбивать линию на точки через каждые Dstep в мм
#define Dstep 3

//Для аварийной остановки в процессе рисования
boolean StopFlag=false;
//Для работы с эмулятором заменить на true
boolean EmulatorFlag=false;
//boolean EmulatorFlag=true;

void setup() {
Serial.begin(19200);
if(!EmulatorFlag)
   {
     servo1.attach(5);// подключаем переменную servo к соответствующему выводу платы Ардуино
     servo2.attach(6);
     servo3.attach(7);
   }
Parking();
Serial.println("RBT");
delay(1000);
}

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

//выдержка времени, возвращающая true, если пришел символ в COM (ПОКА НЕ ЗАДЕЙСТВОВАНА)
//boolean DelayWithStop(int ms)
//{
//  while (!Serial.available())
//  {
//    delay(5);
//    ms-=5;
//    if(ms<0)break;
//  }
//  StopFlag=Serial.available();
//  return Stopflag;
//}

///////////////////////////////////////////////////////////////////////////
// устанавливает механизм в начальное положение
void Parking()
{
servo3.write(S3_UP);
delay(1000);
servo1.write(S1_0);//начальная установка положения
servo2.write(S2_0);
CurX=1000;CurY=1000;//Начальная точка для рисования не определена.
}
/////////////////////////////////////////////////////////////////////////
void  up_down_pen(boolean  down)
{
  if(PenIsDown^down)
  {
    byte a3;
    if(down)
      {
        if(!EmulatorFlag)
        {
          a3=S3_UP;
          while (a3>S3_DW){servo3.write(a3--);delay(n);}
        }
        else Serial.println("DWN");
      }
      else 
      {
        if(!EmulatorFlag)
        {
          a3=S3_DW;
          while (a3<S3_UP){servo3.write(a3++);delay(n);}
        }
        else Serial.println("UP"); 
      }
    PenIsDown=down;
  }
   Serial.print("Pen is ");
   Serial.println(PenIsDown);
}
////////////////////////////////////////////////////////////////////////////////
//Рисует отрезок длины d в направлении a(в градусах)
//либо описывает правильную дугу, с диаметром d в направлении a(значения угла задаются как 1000+а)
//либо переставляет перо на конец вектора, при отрицательных d
void DrawVector(int d, float a)
{
  bool IsArc = false;
  if (a > 999) {
    IsArc = true;  //Чтобы обозначить рисование дуги, к значению угла добавляется 1000
    a -= 1000;
  }
  if (d > Dmax || a < 0 || a > 359)return; // не корректные параметры
  if (CurX == 1000 || CurY == 1000)return; // не задана начальная точка
  if (d < 0) //задана перестановка пера
  {
    up_down_pen(false);//поднимем перо
    d = abs(d);
    GoToFinLine(d, a);
  } else //задано рисование
  {
    up_down_pen(true);//опустим перо
    if (!IsArc) //рисование отрезка
    {
      int n = d / Dstep; //разобьем линию на n отрезков.
      for (int i = 1; i <= n; i++)GoToFinLine(Dstep, a);//нарисуем отрезки по очереди
      d -= n * Dstep;
      if (d > 0)GoToFinLine(d, a); //последнюю точку нарисуем отдельно для точности.
    }
    else //рисование дуги
    {
      //посчитаем координаты центра окружности
      float x1, y1;
      float x0 = CurX + d / 2 * cos(GradToRad(a));
      float y0 = CurY + d / 2 * sin(GradToRad(a));
      int nn = d * PI/2 / Dstep; //разобьем дугу на n отрезков.
      float da = PI / nn;// вычислим угловой шаг
      float aa = GradToRad(a);
     
      for (int i = 1; i <= nn; i++)
      {
        //будем вычислять координаты точек, принадлежащих дуге
        aa += da;
        if(aa>PI*2)aa-=PI*2;
        //Будем считать из центра окружности в обратном направлении заданному
        x1 = x0 - d / 2 * cos(aa);
        y1 = y0 - d / 2 * sin(aa);
        //и рисовать линию апроксимации
        SetServosToPoint(x1, y1);
        delay(n*3);
      }
    }

  }
}
///////////////////////////////////////////////////////////////////////////////
// функция перемещает перо в конец заданного вектора
void GoToFinLine(int d,float a)
{
  if(StopFlag)return;
  //определим координаты конца вектора
    int x=CurX+d*cos(GradToRad(a));
    int y=CurY+d*sin(GradToRad(a));
   //переместим перо
    SetServosToPoint(x,y);
    delay(n*d);// выдержка времени для отработки механизма, зависящая от длины пути.
}
//////////////////////////////////////////////////////////////////////////////////
//Функция выставляет servo1 servo2, на точку с координатами X,Y
//X<0 для левого квадранта, X>0 для правого
void SetServosToPoint(int X, int Y)
{
  if(abs(X)>Xmax || Y>Ymax || Y<Ymin) return;
  CurX=X;CurY=Y;
  //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);
  if(!EmulatorFlag)
    {
      servo1.writeMicroseconds(RadianToMcs(A)+Acor1);
      servo2.writeMicroseconds(RadianToMcs(B)+Acor2);
    }
    else
    {
      Serial.print(RadianToMcs(A));   
      Serial.print("&");   
      Serial.println(RadianToMcs(B));     
    }

//  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;
  return map(grad, 0, 180, 500, 2500);
}
/////////////////////////////////////////////////////////////////////
float GradToRad(float grad)
{
  float rad=PI*grad/180;
  return rad; 
}
////////////////////////////////////////////////////////////////////////////////////////////
//Задавать строку координат в виде X,Y (Например: -40,60)
// Либо вектор в виде d:a, где d - длина вектора в мм (отрицательное значение состветствует проходу с поднятым пером),
// a - угол направления рисования в диапазоне 0...360°

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;
  if(inputString.startsWith("RBT?")){Serial.println("RBT");return;}//подключение к эмулятору
  int k=inputString.indexOf(',');
  if(k>0)//получены координаты точки
  {
    int x=inputString.substring(0,k).toInt();
    int y=inputString.substring(++k).toInt();
    x=constrain(x, -Xmax, Xmax);
    y=constrain(y, Ymin, Ymax);
    Serial.println("Set point:");
    Serial.print("X= ");
    Serial.print(x);
    Serial.print(" Y= ");
    Serial.println(y);
    SetServosToPoint(-x,y);
  }else
  {
    k=inputString.indexOf(':');
    if(k<0){Serial.println("!!!");return;} //не правильная команда
    //получены параметры вектора
    int d=inputString.substring(0,k).toInt();
    d=constrain(d, -Dmax, Dmax);
    float a=inputString.substring(++k).toFloat();
   // a=constrain(a, 0, 360);
    Serial.println("Draw vector:");
    Serial.print("d= ");
    Serial.print(d);
    Serial.print(" a= ");
    Serial.println(a);
    DrawVector(d,a);
    up_down_pen(false);//поднимем перо
    if(!EmulatorFlag)Parking();//пока убираем, чтобы посмотреть результаты
  }

}
//////////////////////////////////////////////////////////////////////////////////////



Добавлено спустя 5 минут:

Scorpio писал(а):Буквы легко масштабируются (d*k) и поворачиваются (а+n) в этом был и смысл

Добавлено спустя 4 минуты 12 секунд:
Через монитор порта правильно, потому что там х переворачивается при приеме. В лупе тоже х надо перевернуть в -х. Но в общем как нравится. Моя миссия подходит к концу

Огромное спасибо, буду потихоньку разбираться, конечно функции в таком обилии-вещь!!!