---

#include <Servo.h>

// =====================================================
// ПИНЫ ДЖОЙСТИКОВ
// =====================================================

#define JOY1_X A0
#define JOY1_Y A1

#define JOY2_X A2
#define JOY2_Y A3

#define JOY3_X A4
#define JOY3_Y A5


// =====================================================
// ПИНЫ СЕРВОПРИВОДОВ
// =====================================================

#define SERVO_BASE       3
#define SERVO_MIDDLE_1  5
#define SERVO_MIDDLE_2  6
#define SERVO_GRIP_1    9
#define SERVO_GRIP_2   10


// =====================================================
// СОЗДАЁМ ОБЪЕКТЫ СЕРВО
// =====================================================

Servo servoBase;
Servo servoMiddle1;
Servo servoMiddle2;
Servo servoGrip1;
Servo servoGrip2;


// =====================================================
// ТЕКУЩИЕ ПОЛОЖЕНИЯ СЕРВО
// =====================================================

float posBase    = 90;
float posMiddle1 = 90;
float posMiddle2 = 90;
float posGrip1   = 90;
float posGrip2   = 90;


// =====================================================
// ЦЕЛЕВЫЕ ПОЛОЖЕНИЯ
// =====================================================

float targetBase    = 90;
float targetMiddle1 = 90;
float targetMiddle2 = 90;
float targetGrip1   = 90;
float targetGrip2   = 90;


// =====================================================
// ОГРАНИЧЕНИЯ СЕРВО
// =====================================================

// ОБЯЗАТЕЛЬНО настрой их после первого запуска,
// чтобы механика не упиралась в крайние положения.

int BASE_MIN = 10;
int BASE_MAX = 170;

int MIDDLE1_MIN = 20;
int MIDDLE1_MAX = 160;

int MIDDLE2_MIN = 20;
int MIDDLE2_MAX = 160;

int GRIP1_MIN = 20;
int GRIP1_MAX = 160;

int GRIP2_MIN = 20;
int GRIP2_MAX = 160;


// =====================================================
// ЦЕНТР ДЖОЙСТИКА
// =====================================================

// У большинства джойстиков около 512.
// Если робот начинает двигаться сам,
// поменяй значения после калибровки.

int center1X = 512;
int center1Y = 512;

int center2X = 512;
int center2Y = 512;

int center3X = 512;
int center3Y = 512;


// =====================================================
// МЁРТВАЯ ЗОНА ДЖОЙСТИКА
// =====================================================

const int DEADZONE = 70;


// =====================================================
// СКОРОСТЬ ДВИЖЕНИЯ
// =====================================================

// Чем больше значение — тем быстрее робот.

float MAX_SPEED = 2.0;


// =====================================================
// НАПРАВЛЕНИЕ СЕРВО
// =====================================================

// false = обычное направление
// true  = инвертированное направление

bool invertBase    = false;

bool invertMiddle1 = false;
bool invertMiddle2 = true;

bool invertGrip1   = false;
bool invertGrip2   = true;


// =====================================================
// ФУНКЦИЯ ПОЛУЧЕНИЯ СКОРОСТИ ОТ ДЖОЙСТИКА
// =====================================================

float joystickSpeed(int value, int center)
{
  int difference = value - center;

  // Мёртвая зона
  if (abs(difference) < DEADZONE)
  {
    return 0;
  }

  // Убираем мёртвую зону
  if (difference > 0)
  {
    difference -= DEADZONE;
  }
  else
  {
    difference += DEADZONE;
  }

  // Преобразуем диапазон в скорость
  float speed = (float)difference / (512 - DEADZONE);

  speed *= MAX_SPEED;

  // Ограничение
  if (speed > MAX_SPEED)
    speed = MAX_SPEED;

  if (speed < -MAX_SPEED)
    speed = -MAX_SPEED;

  return speed;
}


// =====================================================
// ОБНОВЛЕНИЕ СЕРВО
// =====================================================

void updateServo(
  Servo &servo,
  float &position,
  float target,
  int minAngle,
  int maxAngle
)
{
  // Плавное приближение
  position += (target - position) * 0.15;

  // Ограничение
  if (position < minAngle)
    position = minAngle;

  if (position > maxAngle)
    position = maxAngle;

  servo.write((int)position);
}


// =====================================================
// SETUP
// =====================================================

void setup()
{
  Serial.begin(115200);

  // Подключаем сервоприводы

  servoBase.attach(SERVO_BASE);

  servoMiddle1.attach(SERVO_MIDDLE_1);

  servoMiddle2.attach(SERVO_MIDDLE_2);

  servoGrip1.attach(SERVO_GRIP_1);

  servoGrip2.attach(SERVO_GRIP_2);


  // =================================================
  // НАЧАЛЬНОЕ ПОЛОЖЕНИЕ
  // =================================================

  servoBase.write(90);

  servoMiddle1.write(90);

  servoMiddle2.write(90);

  servoGrip1.write(90);

  servoGrip2.write(90);


  delay(1000);


  Serial.println("ROBOT ARM STARTED");
}


// =====================================================
// LOOP
// =====================================================

void loop()
{

  // ===================================================
  // ЧИТАЕМ ДЖОЙСТИКИ
  // ===================================================

  int joy1X = analogRead(JOY1_X);
  int joy1Y = analogRead(JOY1_Y);

  int joy2X = analogRead(JOY2_X);
  int joy2Y = analogRead(JOY2_Y);

  int joy3X = analogRead(JOY3_X);
  int joy3Y = analogRead(JOY3_Y);


  // ===================================================
  // ПОЛУЧАЕМ СКОРОСТЬ
  // ===================================================

  float speed1X = joystickSpeed(joy1X, center1X);
  float speed1Y = joystickSpeed(joy1Y, center1Y);

  float speed2X = joystickSpeed(joy2X, center2X);
  float speed2Y = joystickSpeed(joy2Y, center2Y);

  float speed3X = joystickSpeed(joy3X, center3X);
  float speed3Y = joystickSpeed(joy3Y, center3Y);


  // ===================================================
  // ДЖОЙСТИК №1
  //
  // X = вращение основания
  // Y = первое среднее серво
  // ===================================================

  if (invertBase)
    targetBase -= speed1X;
  else
    targetBase += speed1X;


  if (invertMiddle1)
    targetMiddle1 -= speed1Y;
  else
    targetMiddle1 += speed1Y;


  // ===================================================
  // ДЖОЙСТИК №2
  //
  // X = второе среднее серво
  // Y = дополнительное управление
  // ===================================================

  if (invertMiddle2)
    targetMiddle2 -= speed2X;
  else
    targetMiddle2 += speed2X;


  // Здесь Y второго джойстика пока используется
  // для совместного движения двух средних сервоприводов.

  targetMiddle1 += speed2Y * 0.5;

  targetMiddle2 += speed2Y * 0.5;


  // ===================================================
  // ДЖОЙСТИК №3
  //
  // X = клешня
  // Y = дополнительная функция
  // ===================================================

  if (invertGrip1)
    targetGrip1 -= speed3X;
  else
    targetGrip1 += speed3X;


  if (invertGrip2)
    targetGrip2 -= speed3X;
  else
    targetGrip2 += speed3X;


  // Y третьего джойстика пока не используется.


  // ===================================================
  // ОГРАНИЧЕНИЯ
  // ===================================================

  targetBase = constrain(
    targetBase,
    BASE_MIN,
    BASE_MAX
  );

  targetMiddle1 = constrain(
    targetMiddle1,
    MIDDLE1_MIN,
    MIDDLE1_MAX
  );

  targetMiddle2 = constrain(
    targetMiddle2,
    MIDDLE2_MIN,
    MIDDLE2_MAX
  );

  targetGrip1 = constrain(
    targetGrip1,
    GRIP1_MIN,
    GRIP1_MAX
  );

  targetGrip2 = constrain(
    targetGrip2,
    GRIP2_MIN,
    GRIP2_MAX
  );


  // ===================================================
  // ДВИГАЕМ СЕРВО
  // ===================================================

  updateServo(
    servoBase,
    posBase,
    targetBase,
    BASE_MIN,
    BASE_MAX
  );


  updateServo(
    servoMiddle1,
    posMiddle1,
    targetMiddle1,
    MIDDLE1_MIN,
    MIDDLE1_MAX
  );


  updateServo(
    servoMiddle2,
    posMiddle2,
    targetMiddle2,
    MIDDLE2_MIN,
    MIDDLE2_MAX
  );


  updateServo(
    servoGrip1,
    posGrip1,
    targetGrip1,
    GRIP1_MIN,
    GRIP1_MAX
  );


  updateServo(
    servoGrip2,
    posGrip2,
    targetGrip2,
    GRIP2_MIN,
    GRIP2_MAX
  );


  // ===================================================
  // НЕБОЛЬШАЯ ЗАДЕРЖКА
  // ===================================================

  delay(15);
}