// https://forum.arduino.cc/t/no-logro-sincronizar-el-movimiento-de-dos-servos/1244974

#include <Servo.h>

Servo servo_h;
Servo servo_v;

int pos[6][2] = {{45, 45}, {90, 180}, {135, 135}, {0, 180}, {90, 90}, {0, 0}};

void mover_servos(int hpos, int vpos, unsigned long s) {
  static int oldhpos;
  static int oldvpos;
  Serial.println("h: " + String(oldhpos) + " ->" + String(hpos));
  Serial.println("v: " + String(oldvpos) + " ->" + String(vpos));
  Serial.println("s: " + String(s));
  Serial.println();

  int hdif = hpos - oldhpos;
  int hdir = hdif < 0 ? -1 : 1;
  hdif = abs(hdif);
  int vdif = vpos - oldvpos;
  int vdir = vdif < 0 ? -1 : 1;
  vdif = abs(vdif);
  int pasos = vdif >= hdif ? vdif : hdif;

  for (int i = 0 ; i <= pasos; i++) {
    servo_h.write(oldhpos + hdif / (float)pasos * hdir * i );
    delay(s);
    servo_v.write(oldvpos + vdif / (float)pasos * vdir * i );
    delay(s);
  }
  oldhpos = hpos;
  oldvpos = vpos;
}

void setup() {
  Serial.begin(115200);
  servo_h.attach(6);
  servo_v.attach(7);
  servo_h.write(0);
  servo_v.write(0);
  delay(500);
  mover_servos(180, 180, 1);
  delay(100);
  mover_servos(0, 0, 1);
  delay(1000);
}

void loop() {
  static int x;
  mover_servos(pos[x][0], pos[x][1], random(1, 5));
  x++;
  x %= 6;
  delay(1000);
}