///////////// МАНИПУЛЯТОР ДЛЯ ИГРЫ В ШАШКИ //////////////////////////////
#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[41]={ 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,
80,30,30,30,30,30,30,30,30};
int tablY[41]={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,
90,58,68,73,83,92,100,107,116};// массивы координат клеток игровых
byte tablP[41]={11,13,15,17,22,24,26,28,31,33,35,37,42,44,46,48,51,53,55,57,62,64,66,68,71,73,75,77,82,84,86,88, 0, 19,29,39,49,59,69,79,89}; //массив перекодировки игровых клеток
///////////////////////////////////////////////////////////////
Servo servo1;
Servo servo2;
Servo servo3;
String inputString;
bool echo=false; //отладка
int ConnectCount=60;
bool AdjustModo;
void setup() {
Serial.begin(19200);
servo1.attach(5);
servo2.attach(6);
servo3.attach(7);
servo3.write(125);
pinMode(Pin, OUTPUT);digitalWrite(Pin, LOW);
////////////////////////
SendMail("RBT!");
////////////////////////
up_down_pen(false);//поднимем захват
SetServosToPoint(nx,ny);//установим захват в исходное положение
delay_(50*n);
}
void loop()
{
CheckSerial();
}
/////////////////////////////////////////////////////////////////
////функция не задействованная
void NewGame()
{}
//////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
////функция не задействованная
void NXTServo_SetStandart()
{}
//////////////////////////////////////////////////////////////////
///функция включения сервоприводов и задания начального положения
void Servo_Power(bool flag)
{
if (flag==true)
{
Servo servo1;
Servo servo2;
Servo servo3;
servo1.attach(5);//включить сервоприводы
servo2.attach(6);
servo3.attach(7);
servo3.write(125);
pinMode(Pin, OUTPUT);digitalWrite(Pin, LOW);
up_down_pen(false);//поднимем захват
SetServosToPoint(nx,ny);//установим захват в исходное положение
delay_(50*n);
}
}
///////////////////////////////////////////////////////////////////
/////////функция остановки игры,снятия питания/////////////////////
void Exit()
{
SetServosToPoint(nx,ny);//установим захват в исходное положение
delay_(5*n);up_down_pen(false);
delay_(5*n);
servo1.detach();//отключим сервоприводы
servo2.detach();
servo3.detach();
}
//////////////////////////////////////////////////////////////////////
//функция выполнения хода, например - выполнить простой ход "33:44"
void SimpleMov(String mov)
{
int tmp=(mov.substring(0,2)).toInt();//преобразовали первые два знака строки в код поля "откуда"
TakePiez((byte)tmp);//взяли шашку
tmp=(mov.substring(3)).toInt();//преобразовали последние два знака строки в код поля "куда"
MoveToCell((byte)tmp); //переместили
PutPiez2((byte)tmp);//опустили
delay_(5*n);
SetServosToPoint(nx,ny);//установим захват в исходное положение
}
//////////////////////////////////////////////////////////////////////
///функция поднимания шашки с игрового поля под номером N=11 (TAP11)
/////////////////////////////////////////////////////////////////////
void TakePiez (byte N)
{
for(byte ka=0;ka<=40;ka++)
{
if (tablP[ka]==N)
{
SetServosToPoint(tablX[ka],tablY[ka]);
up_down_pen(true);digitalWrite(Pin, HIGH);
delay_(5*n);up_down_pen(false);
break;
}
}
}
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
///функция перемещения каретки-магнита на игровую клетку под номером N=11 (MTC11)
/////////////////////////////////////////////////////////////////////
void MoveToCell (byte N)
{
for(byte ka=0;ka<=40;ka++)
{
if (tablP[ka]==N)
{
SetServosToPoint(tablX[ka],tablY[ka]);
break;
}
}
}
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
///функция постановки шашки на игровую клетку поля под номером N=11
//без отпускания - обозначение хода(PUC11)ИЛИ с отпусканием шашки (PUO11)
/////////////////////////////////////////////////////////////////////
void PutPiez1 (byte N)
{
for(byte ka=0;ka<=40;ka++)
{
if (tablP[ka]==N)
{
SetServosToPoint(tablX[ka],tablY[ka]);
delay_(5*n);
up_down_pen(true);
delay_(5*n);
break;
}
}
}
//////////////////////////////////////////////////////////////////////
///функция постановки шашки на игровую клетку поля под номером N=11
//без отпускания - обозначение хода(PUC11)ИЛИ с отпусканием шашки (PUO11)
/////////////////////////////////////////////////////////////////////
void PutPiez2 (byte N)
{
for(byte ka=0;ka<=40;ka++)
{
if (tablP[ka]==N)
{
SetServosToPoint(tablX[ka],tablY[ka]);
up_down_pen(true);digitalWrite(Pin, LOW);
delay_(5*n);up_down_pen(false);
break;
}
}
}
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
///функция удаления шашки с игрового поля под номером N=11 (DEP11)
/////////////////////////////////////////////////////////////////////
void DeletePiez (byte N)
{
for(byte ka=0;ka<=40;ka++)
{
if (tablP[ka]==N)
{
SetServosToPoint(tablX[ka],tablY[ka]);
up_down_pen(true);digitalWrite(Pin, HIGH);
delay_(5*n);up_down_pen(false);
SetServosToPoint(tablX[32],tablY[32]);
delay_(30*n);digitalWrite(Pin, LOW);
SetServosToPoint(nx,ny);
break;
}
}
}
//////////////////////////////////////////////////////////////////////
//Функция выставляет 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));
//////////////////////////////////////////////////////////////////////////
int A2=180*A/PI+Acor1/11; int B2=180*B/PI+Acor2/11;
while (servo1.read()!=A2)
{
delay_(n/2);
if (servo1.read()>A2){ servo1.write(servo1.read()-1);}
else {servo1.write(servo1.read()+1);}
}
while (servo2.read()!=B2)
{
delay_(n/2);
if (servo2.read()>B2){ servo2.write(servo2.read()-1);}
else {servo2.write(servo2.read()+1);}
}
/////////////////////////////////////////////////////////////////////////
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;}
}
/////////////////////////////////////////////////////////////////////////////////////////////////
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(echo)Serial.println("PC-> "+inputString);
if (y<3)return;
String cmd=inputString;
inputString = "";
if (cmd.startsWith("RBT?")){ConnectCount=60; SendMail("RBT!");} //подтверждение связи (подтверждение от PC должно приходить как минимум раз в 5 секунд)
else if (cmd.startsWith("ECH")){echo=!echo;if(echo)SendMail("ECHO ON");else SendMail("ECHO OFF");}// отладка
// else if (cmd.startsWith("VER")){SendMail(VER);}
else if (cmd.startsWith("END")){Exit();}// остановить игру, снять питание
else if (cmd.startsWith("INI")){Servo_Power(true);SendMail("INI");} //включить сервы, задать исходную позицию
else if (cmd.startsWith("NEW")){Servo_Power(true);NewGame();SendMail("NEW");} //произнести приветствие перед началом новой игры
//else if (cmd.startsWith("TLK"))Lisp(); // начать движени ртом
//else if (cmd.startsWith("TLN"))TalkFlag=false; // закончить движени ртом
else if (cmd.startsWith("EMO")){SendMail("EMO");}// выставляет стандартные положения эмоций
//else if (cmd.startsWith("LTC")){PlayEmotion(5);SendMail("LTC");}//посмотреть в центр поля
// else if (cmd.startsWith("THI"))Thinking(); // начать изображать мыслительный процесс
// else if (cmd.startsWith("THS")){ThinkFlag=false;WaitFlag=false;} // закончить
else if (cmd.startsWith("TAP")){TakePiez(GetNumber(cmd));SendMail("TAP");}// поднять шашку с поля (TAP11)
else if (cmd.startsWith("MTC")){MoveToCell(GetNumber(cmd));SendMail("MTC");}// переместить каретку на поле (MTC11)
else if (cmd.startsWith("PPC")){PutPiez1(GetNumber(cmd));SendMail("PPC");}// опустить не разжимая челюсти (обозначить ход)(PPC11)
else if (cmd.startsWith("PPO")){PutPiez2(GetNumber(cmd));SendMail("PPO");}// опустить шашку (PPO11)
else if (cmd.startsWith("DEP")){DeletePiez(GetNumber(cmd));SendMail("DEP");}// удалить шашку с поля (DEP11)
else if (cmd.startsWith("SMO")){SimpleMov(cmd.substring(3));SendMail("SMO");} // выполнить простой ход MOV33:44
//else if (cmd.startsWith("PCC"))PCFlag=true;
else if (cmd.startsWith("STD")){NXTServo_SetStandart();SendMail("STD");}
else if (cmd.startsWith("ADJ")){AdjustModo=true;}
}
byte GetNumber(String cmd)
{
int tmp=(cmd.substring(3)).toInt();
return(byte)tmp;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////
void SendMail(String mail) // ОТПРАВКА СООБЩЕНИЙ
{
Serial.println(mail);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////
void ErrMsg(String msg)
{
Serial.print("ERROR: ");
Serial.println(msg);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////// ФУНКЦИИ ОПИСАНИЕ //////////////////////////////////////////////////////
//Функция земеняет выполнение команды delay(T1); в ней на время паузы статичного поведения сервоприводов проверяется связь с компьютером
void delay_(int T1)
{
long Y2=millis();
while((millis()- Y2)<T1)CheckSerial();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////