roboforum.ru

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

#define Lmax 120//значение граничных координат
#define Qmax 140//значение граничных координат
#define Acor1 352//поправка на посадку качалки сервы 1
#define Acor2 220//поправка на посадку качалки сервы 2
#define Lmin 50//значение граничных координат
#define Dstep 1 //шаг при рисовании отрезка
#define Dmax 50//максимальной длины отрезок
//Для аварийной остановки в процессе рисования - не задействована
boolean StopFlag=false;
////////////////////////////////////////////////////////////////
float CurL=1000;
float CurQ=1000;
byte a03=125;
byte a3=a03;
int n=10;//переменная единицы временного интервала
String inputString;// строка данных
///////////////////////////////////////////////////////////////
Servo servo1;
Servo servo2;
Servo servo3;


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

void loop()
    {
       CheckSerial();//ЖДЁМ ПОСТУПЛЕНИЯ КОМАНДЫ НА РИСОВАНИЕ ЗАДАННОГО ОБЪЕКТА
    }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////ФУНКЦИИ ФУНУЦИИ ФУНКЦИИ ФУНУЦИИ ФУНКЦИИ ФУНУЦИИ////////////////////////////////////////////////////////// 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//функция рисует окружность диаметром d
///////////////////////////////////////
void Okr(float d)
{
up_down_pen(true);//опустим перо
for (byte i=0;i<12;i++)
{
  DrawVector(PI*d/12,30*i);
  }
   
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//функция рисует равносторонний треугольник со стороной d, наклонённый под углом a (больше 0, меньше 60 градусов)
///////////////////////////////////////
void Treug(int d, int a)
{
up_down_pen(true);//опустим перо
DrawVector(d,60-a); DrawVector(d,300-a); DrawVector(d,180-a);
   
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//функция рисует квадрат со стороной d, наклонённый под углом a (больше 0, меньше 90 градусов)
///////////////////////////////////////
void Kvad(int d, int a)
{
up_down_pen(true);//опустим перо
DrawVector(d,90+a); DrawVector(d,0+a); DrawVector(d,270+a); DrawVector(d,180+a);
 
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// фунуция рисует линию длины d в направлении a(в градусах)
//либо переставляет перо на конец вектора, при отрицательных d
void DrawVector(float d, float a)
{
  if (d > Dmax || a < 0 || a > 359)return; // не корректные параметры
  if (CurL == 1000 || CurQ == 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); //последнюю точку нарисуем отдельно для точности.
  }
}
// функция перемещает перо в конец заданного вектора
void GoToFinLine(float d,float a)
{
  if(StopFlag)return;
  //определим координаты конца вектора
    float l=CurL+d*sin(GradToRad(90+a-CurQ));
    float g=CurQ-(180*asin(d*cos(GradToRad(90+a-CurQ))/CurL))/PI;
   //переместим перо
    SetServosToPoint(l,g);
    delay(n*d);// выдержка времени для отработки механизма, зависящая от длины пути.
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Функция выставляет servo1 servo2, на точку с декартовыми координатами Х,Y
//X,Y расстоямие в мм
void SetServosToPointXY(float X, float Y)
{
  float l; float q;
  if(X==0){l=Y;q=90;}
  if (X>0){q=(180/PI)*asin((Y/X)/sqrt(1+((Y*Y)/(X*X))));l=Y/sin(PI*q/180);}
  if (X<0){q=180-((180/PI)*asin((Y/abs(X))/sqrt(1+((Y*Y)/(X*X)))));l=Y/sin(PI*(180-q)/180);}
  SetServosToPoint(l,q);

}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Функция выставляет servo1 servo2, на точку с полярными координатами L,Q
//L расстоямие в мм от полюса,Q угол от полярной оси в градусах
void SetServosToPoint(float L, float Q)
{

  if(L>=Lmax ||L<=Lmin  ||Q>= Qmax ) return;
  float lp=65; //длина одного из равных плеч
  float B=acos(1.00-(L*L)/(2*lp*lp));
  float A=(PI*Q/180)+(B/2)-(PI/2);
// 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);
  CurL=L;
  CurQ=Q;

// Serial.print("Current point: L= ");
// Serial.print(L);
// Serial.print(", Q= ");
// Serial.println(Q);
}
//////////////////////////////////////////////////////////////////////
//функция перевода радиан в микросекунды для управления сервоприводами
int RadianToMcs(float rad)
{
  int grad=180*rad/PI;
  grad=map(grad, 0, 180, 500, 2500);
  return grad;
}
/////////////////////////////////////////////////////////////////////
float GradToRad(float grad)
{
  float rad=PI*grad/180;
  return rad; 
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////функция поднимания-опускания фломастера (значения аргумента 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;}
  }
  /////////////////////////////////////////////////////////////////////////////////////////////////
//ФУНКЦИИ ПРИЁМА И ОБРАБОТКИ ДАННЫХ
void CheckSerial()
{
//////////////////////////////////////////////////////////////////////////////
  while (Serial.available())//считываем строку данных пока они поступают
  {
    char inChar = (char)Serial.read();
    if (inChar == '\n')     //если окончание строки
    {
      MakeCmd();//обрабатываем её данной функцией
      break;
    }
    else inputString += inChar;// иначе удлиняем строку на один символ
  }
}
/////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
void MakeCmd()
{
    int y=inputString.length();// присваиваем переменной у число символов в строке
    if (y<3||y>3)return;//если их меньше 3 или больше, выходим из функции
    String cmd=inputString;// передаём строку другой переменной
    inputString = "";//"обнуляем переменную"

if (cmd=="DOM"){RisunokDomik ();}//РИСУЕМ ДОМИК
if (cmd=="KUB"){ RisunokKubik ();}//РИСУЕМ КУБИК 
if (cmd=="CVE"){RisunokCvetok();}//РИСУЕМ ЦВЕТОК
}

///////////////////////////////////////////////////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //////////////////////// РИСУНКИ РИСУНКИ РИСУНКИ РИСУНКИ РИСУНКИ РИСУНКИ РИСУНКИ РИСУНКИ РИСУНКИ РИСУНКИ ///////////////
  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   void RisunokDomik ()
   {
  SetServosToPoint (80,110);// стены, крыша
   DrawVector(20,90);DrawVector(10,180);DrawVector(14,45);DrawVector(20,0);DrawVector(14,315);DrawVector(10,180);
   DrawVector(20,270);DrawVector(20,180);DrawVector(20,90);DrawVector(22,0);
   up_down_pen (false);
   
   SetServosToPoint (85,103);//окошко
   DrawVector(7,90);DrawVector(7,0);DrawVector(7,270);DrawVector(7,180);
    up_down_pen (false);

    SetServosToPoint (107,100);//труба
   DrawVector(7,90);DrawVector(5,0);DrawVector(7,270);DrawVector(5,180);
    up_down_pen (false);

     SetServosToPoint (75,100);//дорожка
   DrawVector(40,0);
    up_down_pen (false);
    SetServosToPoint (70,103);
     DrawVector(40,0);
    up_down_pen (false);
   
    SetServosToPointXY(20,90);Okr(15);//солнышко

     SetServosToPointXY(5,95); DrawVector(10,20);up_down_pen (false);SetServosToPointXY(5,85); DrawVector(10,45);up_down_pen (false);SetServosToPointXY(10,80); DrawVector(10,70);up_down_pen (false);
     SetServosToPointXY(5,105); DrawVector(10,330);up_down_pen (false);SetServosToPointXY(15,80); DrawVector(10,80);up_down_pen (false);SetServosToPointXY(20,80); DrawVector(10,80);up_down_pen (false);
     SetServosToPointXY(10,110); DrawVector(10,320);up_down_pen (false);SetServosToPointXY(20,107); DrawVector(10,120);up_down_pen (false);SetServosToPointXY(25,107); DrawVector(10,90);up_down_pen (false);

    for (int z=0;z<9;z++)// заборчик
     {
       SetServosToPointXY(-45+2*z,75); DrawVector(12,90);DrawVector(4,300);up_down_pen (false);
      }
        SetServosToPointXY(-45,75); DrawVector(20,0);up_down_pen (false);
      }
     
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   void RisunokKubik ()
   {
   SetServosToPointXY(-20,75);Kvad(20,0);up_down_pen (false);// сам кубик
  SetServosToPointXY(-20,95);DrawVector(15,45);up_down_pen (false);
  SetServosToPointXY(0,95);DrawVector(13,47);up_down_pen (false);
  SetServosToPointXY(0,75);DrawVector(15,45);up_down_pen (false);
  SetServosToPointXY(-12,106);DrawVector(23,0);DrawVector(22,270);up_down_pen (false);

  SetServosToPointXY(-11,85);up_down_pen (true);delay(2000);up_down_pen (false);//точки на гранях
   SetServosToPointXY(8,93);up_down_pen (true);delay(2000);up_down_pen (false);
  SetServosToPointXY(2,96);up_down_pen (true);delay(2000);up_down_pen (false);
  SetServosToPointXY(3,81);up_down_pen (true);delay(2000);up_down_pen (false);
  SetServosToPointXY(9,102);up_down_pen (true);delay(2000);up_down_pen (false);
  SetServosToPointXY(10,88);up_down_pen (true);delay(2000);up_down_pen (false);
   SetServosToPointXY(-7,102);up_down_pen (true);delay(2000);up_down_pen (false);
  SetServosToPointXY(-11,104);up_down_pen (true);delay(2000);up_down_pen (false);
  SetServosToPointXY(-1,98);up_down_pen (true);delay(2000);up_down_pen (false);

  SetServosToPointXY(-23,85);DrawVector(25,215);DrawVector(45,0);DrawVector(25,45);DrawVector(8,180);up_down_pen (false);//последний штрих-контур 
   }
   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   void RisunokCvetok ()
   {
for(byte i=0;i<12;i++)//цветок
{
  SetServosToPointXY(5,110); DrawVector(10,i*30);up_down_pen(false);
}
  SetServosToPointXY(5,110);DrawVector(20,248);DrawVector(20,270);up_down_pen(false);//стебель,лист,горшок
  SetServosToPointXY(-4,91);DrawVector(15,135);DrawVector(13,215);DrawVector(11,20);DrawVector(15,320);up_down_pen(false);
  SetServosToPointXY(-13,73);DrawVector(20,0);DrawVector(10,225);DrawVector(7,180);DrawVector(10,135);up_down_pen(false);
  SetServosToPointXY(-8,57);Kvad(10,0);up_down_pen(false);
   }
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////

Graphics.TextRenderingHint = TextRenderingHint.AntiAlias