///////////// МАНИПУЛЯТОР ДЛЯ ИГРЫ В ШАШКИ ПРОХОД ПО КЛЕТКАМ С ОПУСКАНИЕМ-ПОДНИМАНИЕМ МАГНИТА И ЗАДАННОЙ ЕДИНИЧНОЙ ПЕРЕСТАНОВКОЙ ШАШКИ //////////////////////////////
#include <Servo.h>
////////////////////////////////////////////////////////////////
#define L1 65 //длина первого плеча манипулятора в мм.
#define L2 65 //длина второго плеча манипулятора в мм.
#define Xmax 120//значение граничных координат
#define Ymax 120//значение граничных координат
#define Acor1 352//поправка на посадку качалки сервы 1
#define Acor2 220//поправка на посадку качалки сервы 2
#define Ymin 50//значение граничных координат
#define Pin 8//вывод для подключения электромагнита
#define nx -25
#define ny 58 //начальные координаты первой игровой клетки (11)
////////////////////////////////////////////////////////////////
int CurX;int CurY;
byte a03=125;
byte a3=a03;
int n=20;//переменная единицы временного интервала
int tablX[32]={ nx,-12,1,14,
-19,-5,8,21,
-26,-12,1,14,
-20,-5,9,22,
-27,-12,3,18,
-20,-5,11,25,
-27,-12,3,18,
-20,-5,11,26};
int tablY[32]={ny,58,58,58,
66,64,66,68,
74,73,73,73,
82,80,83,83,
92,92,92,92,
100,98,100,100,
107,107,107,107,
115,115,115,116};// массивы координат клеток
///////////////////////////////////////////////////////////////
Servo servo1;
Servo servo2;
Servo servo3;
void setup() {
Serial.begin(9600);
servo1.attach(5);
servo2.attach(6);
servo3.attach(7);
//servo1.write(98);
//servo2.write(110);
servo3.write(125);
pinMode(Pin, OUTPUT);digitalWrite(Pin, LOW);
up_down_pen(false);//поднимем захват
SetServosToPoint(nx,ny);//установим захват в исходное положение
}
void loop()
{
for (int j=0;j<=31;j++)
{
SetServosToPoint(tablX[j],tablY[j]); up_down_pen(true);if(j==9){digitalWrite(Pin, HIGH);delay(5*n);}if(j==14){digitalWrite(Pin, LOW);delay(5*n); }up_down_pen(false);
}
digitalWrite(Pin, LOW);
metka:goto metka;
}
/////////////////////////////////////////////////////////////////////////
//Функция ПЛАВНО выставляет servo1 servo2, на точку с координатами X,Y
//X<0 для левого квадранта, X>0 для правого
/*void SetServosToPointPl(int X, int Y)
{
byte Step1=2;
while (abs(CurY-Y)>2*Step1)
{
if(Y-CurY>0){SetServosToPoint(CurX,CurY);CurY=CurY+Step1;}
if(Y-CurY<0){SetServosToPoint(CurX,CurY);CurY=CurY-Step1;}
}
while (abs(CurX-X)>2*Step1)
{
if(X-CurX>0){SetServosToPoint(CurX,CurY);CurX=CurX+Step1;}
if(X-CurX<0){SetServosToPoint(CurX,CurY);CurX=CurX-Step1;}
}
SetServosToPoint(X,Y);
}*/
/////////////////////////////////////////////////////////////////////////
//Функция выставляет 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));
servo1.writeMicroseconds(RadianToMcs(A)+Acor1);
servo2.writeMicroseconds(RadianToMcs(B)+Acor2);
delay(n);
CurX=X;CurY=Y;
}
////////////////////////////////////////////////////////////////////////
//Функция перевода радиан в микросекунды для управления сервоприводами
int RadianToMcs(float rad)
{
int grad=2000*rad/PI+500;
return grad;
}
//////////////////////////////////////////////////////////////////////
//////Функция поднимания-опускания фломастера (значения аргумента false и true соответственно)/////
void up_down_pen(boolean ka)
{
while (a3>85&&ka==1)
{servo3.write(a3);a3=a3-1;delay(n);}if(ka== true ){a3=85;}
while (a3<a03&&ka==0)
{servo3.write(a3);a3=a3+1;delay(n);}if (ka==false){a3=a03;}
}
/////////////////////////////////////////////////////////////////////////////////////////////////
Подробно в работу не вникал.
