roboforum.ru

///////////// МАНИПУЛЯТОР ДЛЯ ГРАФИЧЕСКИХ ИЗОБРАЖЕНИЙ скетч рисование сетки с координатами х,у //////////////////////////////
#include <Servo.h>
////////////////////////////////////////////////////////////////
#define L1 65  //длина первого плеча манипулятора в мм.
#define L2 65  //длина второго плеча манипулятора в мм.
#define Xmax 120//значение граничных координат
#define Ymax 115//значение граничных координат
#define Acor1 352//поправка на посадку качалки сервы 1
#define Acor2 220//поправка на посадку качалки сервы 2
#define Ymin 50//значение граничных координат
////////////////////////////////////////////////////////////////
int a3;int a03=125;
int n=20;//переменная единицы временного интервала
int x1;int y1;int f=0;
///////////////////////////////////////////////////////////////
Servo servo1;
Servo servo2;
Servo servo3;
String inputString = "";

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

void loop()
  {
  //однократная печать с паузой на старте
    delay(5000);
    while(f==0){
  for (x1=-30;x1<=30;x1=x1+5)
  {
   for (y1=55;y1<=110;y1=y1+5)
  {
SetServosToPoint(x1,y1);
  }
  }f++;
  }
}

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

  if(abs(X)>Xmax || Y>Ymax || Y<Ymin) 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);
  delay(5*n);
  up_down_pen(true);
  up_down_pen(false);
  int CurX=X;int CurY=Y;

  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);
}
///////////////////////////////////////////////////////////////////////
//////функция поднимания-опускания фломастера (значения аргумента false и true  соответственно)/////
void  up_down_pen(boolean  k)
{
  while (a3>85&&k==1)
  {servo3.write(a3);a3=a3-1;delay(n);}if(k== true ){a3=85;}
   while (a3<a03&&k==0)
  {servo3.write(a3);a3=a3+1;delay(n);}if (k==false){a3=a03;}
  }
  /////////////////////////////////////////////////////////////////////

//servo1.writeMicroseconds(RadianToMcs(A)+Acor1);
// servo2.writeMicroseconds(RadianToMcs(B)+Acor2);
    servo1.write(RadianToGrad(A)+32);
    servo2.write(RadianToGrad(B)+20);

//функция перевода радиан в градусы для управления сервоприводами
int RadianToGrad(float rad)
{
int grad=180*rad/PI;
//grad=map(grad, 0, 180, 500, 2500);
return grad;
}

//функция перевода радиан в микросекунды для управления сервоприводами
int RadianToMcs(float rad)
{
int grad=2000*rad/PI+500;

return grad;
}
//////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////
  //Функция рисования отрезка по заданным парам координат х1,у1,х2,у2
  void Linia(int X1, int Y1,int X2, int Y2)
  {
   int k;// модуль коэфф. наклона
   SetServosToPoint(X1,Y1);//ставим на начало отрезка стержень фломастера
   up_down_pen(true);// опускаем его
   if (X1!=X2&&Y1!=Y2){ k=abs((Y2-Y1)/(X2-X1));}// считаем модуль коэфф. наклона
   int h;int z=0;// счётчики
   if (k>=1||Y1==Y2){h=abs(X2-X1);} if (k<1||X1==X2){h=abs(Y2-Y1);}//определяем число шагов рисования отрезка
   int dx=0;int dy=0;//приращения координат
   ///////////////////////////////////////////////////////////////////////////////////
   if (k>=1&&X2>X1&&Y2>Y1){dx=1;dy=k;}//условия определения приращения координат
   if (k>=1&&X2<X1&&Y2>Y1){dx=-1;dy=k;}
   if (k>=1&&X2>X1&&Y2<Y1){dx=1;dy=-k;}
   if (k>=1&&X2<X1&&Y2<Y1){dx=-1;dy=-k;}
   if (k<1&&X2>X1&&Y2>Y1){dx=1/k;dy=1;}
   if (k<1&&X2<X1&&Y2>Y1){dx=-1/k;dy=1;}
   if (k<1&&X2>X1&&Y2<Y1){dx=1/k;dy=-1;}
   if (k<1&&X2<X1&&Y2<Y1){dx=-1/k;dy=-1;}
   if (X1==X2&&Y2>Y1){dx=0;dy=1;}
   if (X1==X2&&Y2<Y1){dx=0;dy=-1;}
   if (Y1==Y2&&X2>X1){dx=1;dy=0;}
   if (Y1==Y2&&X2<X1){dx=-1;dy=0;}
   ///////////////////////////////////////////////////////////////////////////////////
   while (z<=h)
   {
    SetServosToPoint(X1+z*dx,Y1+z*dy);delay(2*n);//рисуем с шагом 1 мм
    z++;
    }
   
    up_down_pen(false);//поднимаем стержень фломастера 
  }
  /////////////////////////////////////////////////////////////////////////////////////

#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
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 отрезков.
    int x,y;
    for(int i=0;i<n;i++)GoToFinLine(i*Dstep,a);//нарисуем отрезки по очереди
    GoToFinLine(d,a); //последнюю точку нарисуем отдельно для точности.
  }
 
}
// функция перемещает перо в конец заданного вектора
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();//пока убираем, чтобы посмотреть результаты
  }

}