/*---------------------------------------------------------------------------
Z-RoboDog v2.4.7 Stabil
Автор: ZSeregaA
Сайт проекта: http://robocontroller.ru/
Инструкция по сборке и дополнительное описание:
http://robocontroller.ru/news/z_robodog_sobaka_robot_svoimi_rukami/2013-07-19-26
---------------------------------------------------------------------------*/

#include <Servo.h>

// Пины ультразвукового дальномера
#define trigPin 22
#define echoPin 24
long duration, distance;

// Название сервоприводов
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
Servo servo7;
Servo servo8;

// Переменные 1 = вкл. 0 = откл.
int gozforward = 1; // идти вперед
int gozstop = 0; // Стоп
int go = 1; // Для удобства тестирования дополнений

// Переменные скорости
int movespeed = 100; // Скорость подъема в стойку
int stepspeed = 220; // скорость передвижения

// Крайние точки углов при которых нужно устанавливать лапы
int zs1 = 0;
int zs2 = 180;
int zs3 = 180;
int zs4 = 0;
int zs5 = 0;
int zs6 = 180;
int zs7 = 180;
int zs8 = 0;

// Основная стойка. Можете написать отдельное число.
// например int s1 = 130;
// Я использовал такой способ для подгонки каждой лапы. Измените если нужно.
int s1 = zs1 + 130;
int s2 = zs2 - 130;
int s3 = zs3 - 130;
int s4 = zs4 + 130;
int s5 = zs5 + 130;
int s6 = zs6 - 130;
int s7 = zs7 - 130;
int s8 = zs8 + 130;

// Основная стойка используется для остановки
int pos_stop[1][8]={s1,s2, s3,s4, s5,s6, s7,s8};
// откройте при настройке исходного положения лап
//int pos_stop[1][8]={zs1,zs2, zs3,zs4, zs5,zs6, zs7,zs8};

// Переменные для массива шагов
int zgos1 = 0;
int zgos2 = 0;
int zgos3 = 0;

/*-------------------------------------------------------------------
Массив шагов в котором все движения совершаются изменением углов относительно основной стойки
s1 s2 s3 и т.д. переменные в которых заданы углы каждого из сервоприводов.
Подробности можно прочитать тут http://robocontroller.ru/news/z_robodog_sobaka_robot_svoimi_rukami/2013-07-19-26
--------------------------------------------------------------------*/

// Массив в котором записаны все шаги
int pos_steps[2][6][8]={
    //---------- Часть 1 Шаг начинается с передней левой ноги -------------
    {
    {s1,s2, s3,s4, s5+10,s6-10, s7+20,s8-20},
    {s1-100,s2+50, s3,s4, s5+10,s6-10, s7+20,s8-20},
    {s1-30,s2-40, s3-20,s4+20, s5,s6, s7,s8},
    {s1-30,s2-40, s3-20,s4+20, s5,s6, s7+40,s8-20},
    {s1-10,s2-30, s3,s4, s5,s6, s7+10,s8+30},
    {s1,s2, s3-10,s4-10, s5+10,s6+10, s7,s8}
    },
    
    //---------- Часть 2 Шаг начинается с передней правой ноги -------------
    {
    {s1,s2, s3,s4, s5-20,s6+20, s7-10,s8+10},
    {s1,s2, s3+100,s4-50, s5-20,s6+20, s7-10,s8+10},
    {s1+20,s2-20, s3+30,s4+40, s5,s6, s7-10,s8+10},
    {s1+20,s2-20, s3+30,s4+40, s5-40,s6+20, s7-10,s8+10},
    {s1,s2, s3+10,s4+30, s5-10,s6-30, s7,s8},
    {s1+10,s2+10, s3,s4, s5,s6, s7-10,s8-10}
    }
  };

void setup ()
{
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Пины сервоприводов
  servo1.attach(2);
  servo2.attach(3);
  servo3.attach(4);
  servo4.attach(5);
  servo5.attach(6);
  servo6.attach(7);
  servo7.attach(8);
  servo8.attach(9);
  
  // Занимает исходное положение
  servo1.write(pos_stop[0][0]);
  delay(movespeed);
  servo2.write(pos_stop[0][1]);
  delay(movespeed);
  servo3.write(pos_stop[0][2]);
  delay(movespeed);
  servo4.write(pos_stop[0][3]);
  delay(movespeed);
  servo5.write(pos_stop[0][4]);
  delay(movespeed);
  servo6.write(pos_stop[0][5]);
  delay(movespeed);
  servo7.write(pos_stop[0][6]);
  delay(movespeed);
  servo8.write(pos_stop[0][7]);
  //gozstop = 1;
 
  delay (3000);
  
}

void loop (){
  if (go == 1){
  // Замер дальномером
  if ((zgos1 == 0 && zgos2 == 5) || (zgos1 == 1 && zgos2 == 5)){
  long duration, distance;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
  delay(50);
  // собака останавливается если расстояние меньше или равно указанному (в сантиметрах).
  if(distance <= 10){
    gozstop = 1;
    gozforward = 0;
  }
  // Если расстояние больше указанного (в сантиметрах) и до этого была остановка, тогда собака начинает идти вперед.
   if(distance > 10 && gozstop == 1){
    gozstop = 0;
    gozforward = 1;
    zgos1 = 0;
    zgos2 = 0;
    zgos3 = 0;
   }
  }
  
  // Стоп
  if (gozstop == 1){
    zstop();
  }
  // Движение вперед
  if (gozforward == 1){
    zforward();
  }
  }
}

// Стоп
void zstop(){
  servo1.write(pos_stop[0][0]);
  servo3.write(pos_stop[0][2]);
  servo5.write(pos_stop[0][4]);
  servo7.write(pos_stop[0][6]);
  
  servo2.write(pos_stop[0][1]);
  servo4.write(pos_stop[0][3]);
  servo6.write(pos_stop[0][5]);
  servo8.write(pos_stop[0][7]);
}

// Движение вперед
void zforward(){
  
  // при этом условии в первую очередь работает нога 4
  // это улучшает подсадку и собачку не перевешивает
  // данное условие возможно вам не понадобиться!
  // оставьте часть после else!
  if (zgos1 == 0 && zgos2 == 0){
    servo7.write(pos_steps[zgos1][zgos2][6]);
    servo8.write(pos_steps[zgos1][zgos2][7]);
    servo5.write(pos_steps[zgos1][zgos2][4]);
    servo6.write(pos_steps[zgos1][zgos2][5]);
    servo1.write(pos_steps[zgos1][zgos2][0]);
    servo2.write(pos_steps[zgos1][zgos2][1]);
    servo3.write(pos_steps[zgos1][zgos2][2]);
    servo4.write(pos_steps[zgos1][zgos2][3]);   
  }
  else{
    servo1.write(pos_steps[zgos1][zgos2][0]);
    servo3.write(pos_steps[zgos1][zgos2][2]);
    servo2.write(pos_steps[zgos1][zgos2][1]);
    servo4.write(pos_steps[zgos1][zgos2][3]);
    servo5.write(pos_steps[zgos1][zgos2][4]);
    servo7.write(pos_steps[zgos1][zgos2][6]);
    servo6.write(pos_steps[zgos1][zgos2][5]);
    servo8.write(pos_steps[zgos1][zgos2][7]);
  }
// переключение частей в массиве шагов
  if (zgos2 != 5 && zgos1 != 2){
    zgos2++;
    delay(stepspeed);
  }
  else{
    delay(stepspeed);
      zgos1++;
      zgos2 = 0;
    }
    if (zgos1 == 2){
      zgos1 = 0;
      zgos2 = 0;
      zgos3 = 0;
    }
    
}