//////////////////////////    скетч для управления 8 сервоприводами квадропода   /////////////////
////////////////////////////////////////////// 
//        RemoteXY include library          // 
////////////////////////////////////////////// 

// определение режима соединения и подключение библиотеки RemoteXY  
#define REMOTEXY_MODE__HARDSERIAL

#include <RemoteXY.h> 

// настройки соединения  
#define REMOTEXY_SERIAL Serial 
#define REMOTEXY_SERIAL_SPEED 9600 


// конфигурация интерфейса   
#pragma pack(push, 1) 
uint8_t RemoteXY_CONF[] = 
  { 255,4,0,0,0,81,0,8,159,0,
  1,2,25,5,47,16,133,31,208,159,
  208,160,208,175,208,156,208,158,0,1,
  2,25,42,47,16,133,31,208,157,208,
  144,208,151,208,144,208,148,0,1,2,
  1,23,47,16,133,31,208,146,208,155,
  208,149,208,146,208,158,0,1,2,50,
  23,47,16,133,31,208,146,208,159,208,
  160,208,144,208,146,208,158,0 }; 
   
// структура определяет все переменные вашего интерфейса управления  
struct { 

    // input variable
  uint8_t button_1; // =1 если кнопка нажата, иначе =0 
  uint8_t button_2; // =1 если кнопка нажата, иначе =0 
  uint8_t button_3; // =1 если кнопка нажата, иначе =0 
  uint8_t button_4; // =1 если кнопка нажата, иначе =0 

    // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0 

} RemoteXY; 
#pragma pack(pop) 

///////////////////////////////////////////// 
//           END RemoteXY include          // 
///////////////////////////////////////////// 




#include <Servo.h> //используем библиотеку


Servo myservoK3 ;  //указываем сервопривод заднего левого колена
Servo myservoB4;  //указываем сервопривод  заднего левого бедра
Servo myservoK5 ;  //указываем сервопривод переднего левого колена
Servo myservoB6;  //указываем сервопривод  переднего левого бедра
Servo myservoK7 ;  //указываем сервопривод заднего правого колена
Servo myservoB8;  //указываем сервопривод  заднего правого бедра
Servo myservoK9 ;  //указываем сервопривод переднего правого колена
Servo myservoB10;  //указываем сервопривод переднего правого бедра
int n=17;
int a03=110;int a04=75;int a05=55;int a06=97;int a07=80;int a08=75;int a09=73;int a010=85;//переменные для углов начального положения конечностей
int da=10;//переменная амплитуды изменения угла в градусах для колен
int db=15;//переменная амплитуды изменения угла в градусах для бёдер
int i;
int a3=a03;int a4=a04;int a5=a05;int a6=a06;int a7=a07;int a8=a08;int a9=a09;int a10=a010;// переменные для хранения значений текущих углов
//////при увеличении значения угла сервоприводы движут лапы :B4,B6-вперёд B8,B10-назад K3,K9-вниз K5,K7-вверх ////////
////////////////////////////////


int N=0;                 //Счётчик номера нажатой кнопки
long Y1=0;long Y2=0;

//////////////////////////////// 
void setup()
{
RemoteXY_Init ();  
////////////////////////////////////////////
myservoK3.attach(3);//выводы  для подключения 1-8 сервоприводов
myservoB4.attach(4);
myservoK5.attach(5); 
myservoB6.attach(6); 
myservoK7.attach(7);
myservoB8.attach(8);
myservoK9.attach(9); 
myservoB10.attach(10); 

myservoK3.write(a03); delay(n);// начальная установка-настройка углов поворота валов серв
myservoB4.write(a04); delay(n);
myservoK5.write(a05); delay(n);
myservoB6.write(a06); delay(n);
myservoK7.write(a07); delay(n);
myservoB8.write(a08); delay(n);
myservoK9.write(a09); delay(n);
myservoB10.write(a010); delay(n);



delay(250*n);
}

void loop()
{

 
if(N==0){delay_(5*n);}
if(N==1)
{
  
  /////////////////////////////////////////////  ПРЯМОХОЖДЕНИЕ  /////////////////////////////////////////////
  ///////////// K5,K7 вверх///////////
   for(i = 0 ; i<=da; i++)
  {myservoK5.write(a5+i);
  myservoK7.write(a7+i);
  delay_(n);}
  ///////////// B6,B8- вперёд B4,B10- назад ////////
   for(i = 0 ; i<=db; i++)
  {myservoB6.write(a6+i);
  myservoB8.write(a8-i);
  myservoB4.write(a4-i);
  myservoB10.write(a10+i);
  delay_(n);}
  /////////////K5,K7 вниз /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK5.write(a5-i);
  myservoK7.write(a7-i);
  delay_(n);}
  ////////////K3,K9 вверх /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK3.write(a3-i);
   myservoK9.write(a9-i);
  delay_(n);}
  ///////////// B6,B8- назад B4,B10- вперёд ////////
  for(i = 0 ; i<=db; i++)
  {myservoB6.write(a6-i);
  myservoB8.write(a8+i);
  myservoB4.write(a4+i);
  myservoB10.write(a10-i);
  delay_(n);}
  //////////////K3,K9 вниз /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK3.write(a3+i);
   myservoK9.write(a9+i);
  delay_(n);}
  /////////////////////////////////////////////////////////////////////////////////////////
  
  
 }
if(N==2)
{
  
  /////////////////////////////////////////////  ДВИЖЕНИЕ НАЗАД  /////////////////////////////////////////////
  ////////////K3,K9 вверх /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK3.write(a3-i);
   myservoK9.write(a9-i);
  delay_(n);}
  
  ///////////// B6,B8- вперёд B4,B10- назад ////////
   for(i = 0 ; i<=db; i++)
  {myservoB6.write(a6+i);
  myservoB8.write(a8-i);
  myservoB4.write(a4-i);
  myservoB10.write(a10+i);
  delay_(n);}
   //////////////K3,K9 вниз /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK3.write(a3+i);
   myservoK9.write(a9+i);
  delay_(n);}
  
   ///////////// K5,K7 вверх///////////
   for(i = 0 ; i<=da; i++)
  {myservoK5.write(a5+i);
  myservoK7.write(a7+i);
  delay_(n);}
  
  ///////////// B6,B8- назад B4,B10- вперёд ////////
  for(i = 0 ; i<=db; i++)
  {myservoB6.write(a6-i);
  myservoB8.write(a8+i);
  myservoB4.write(a4+i);
  myservoB10.write(a10-i);
  delay_(n);}
 /////////////K5,K7 вниз /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK5.write(a5-i);
  myservoK7.write(a7-i);
  delay_(n);}
  /////////////////////////////////////////////////////////////////////////////////////////
  
  
 }

if(N==3)
{
  
  /////////////////////////////////////////////  РАЗВОРОТ ВЛЕВО  /////////////////////////////////////////////
  ////////////K3,K9 вверх /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK3.write(a3-i);
   myservoK9.write(a9-i);
  delay_(n);}
  
  ///////////// B6,B10- вперёд B4,B8- назад ////////
   for(i = 0 ; i<=db; i++)
  {myservoB6.write(a6+i);
  myservoB8.write(a8+i);
  myservoB4.write(a4-i);
  myservoB10.write(a10-i);
  delay_(n);}
   //////////////K3,K9 вниз /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK3.write(a3+i);
   myservoK9.write(a9+i);
  delay_(n);}
  
   ///////////// K5,K7 вверх///////////
   for(i = 0 ; i<=da; i++)
  {myservoK5.write(a5+i);
  myservoK7.write(a7+i);
  delay_(n);}
  
  ///////////// B6,B10- назад B4,B8- вперёд ////////
  for(i = 0 ; i<=db; i++)
  {myservoB6.write(a6-i);
  myservoB8.write(a8-i);
  myservoB4.write(a4+i);
  myservoB10.write(a10+i);
  delay_(n);}
 /////////////K5,K7 вниз /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK5.write(a5-i);
  myservoK7.write(a7-i);
  delay_(n);}
  /////////////////////////////////////////////////////////////////////////////////////////
  
  
 }

if(N==4)
{
  
  /////////////////////////////////////////////  РАЗВОРОТ ВПРАВО  /////////////////////////////////////////////
  ///////////// K5,K7 вверх///////////
   for(i = 0 ; i<=da; i++)
  {myservoK5.write(a5+i);
  myservoK7.write(a7+i);
  delay_(n);}
  ///////////// B6,B10- вперёд B4,B8- назад ////////
   for(i = 0 ; i<=db; i++)
  {myservoB6.write(a6+i);
  myservoB8.write(a8+i);
  myservoB4.write(a4-i);
  myservoB10.write(a10-i);
  delay_(n);}
  /////////////K5,K7 вниз /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK5.write(a5-i);
  myservoK7.write(a7-i);
  delay_(n);}
  ////////////K3,K9 вверх /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK3.write(a3-i);
   myservoK9.write(a9-i);
  delay_(n);}
  ///////////// B6,B10- назад B4,B8- вперёд ////////
  for(i = 0 ; i<=db; i++)
  {myservoB6.write(a6-i);
  myservoB8.write(a8-i);
  myservoB4.write(a4+i);
  myservoB10.write(a10+i);
  delay_(n);}
  //////////////K3,K9 вниз /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK3.write(a3+i);
   myservoK9.write(a9+i);
  delay_(n);}
  /////////////////////////////////////////////////////////////////////////////////////////
  
  
 }

}
 
 ////////////////////////////////////////   ФУНКЦИИ ОПИСАНИЕ   //////////////////////////////////////////////////////
//Функция заменяет выполнение команды delay(n); в ней на время паузы  идёт опрос датчика

void delay_(int T1)
{
 Y2=millis();metka:Y1=millis();if(Y1-Y2>=T1){} 
 else {  RemoteXY_Handler ();
  if(RemoteXY.connect_flag==1&&RemoteXY.button_1==1){N=1;}
  if(RemoteXY.connect_flag==1&&RemoteXY.button_2==1){N=2;}
  if(RemoteXY.connect_flag==1&&RemoteXY.button_3==1){N=3;}
  if(RemoteXY.connect_flag==1&&RemoteXY.button_4==1){N=4;}
  if(RemoteXY.connect_flag==1&&RemoteXY.button_1==0&&RemoteXY.button_2==0&&RemoteXY.button_3==0&&RemoteXY.button_4==0){N=0;}
  if(RemoteXY.connect_flag==0){N=0;}
    goto metka;}

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