#include <Servo.h>

Servo servoLeft;
Servo servoRight;


int globalSpeed = 5000;
int minSpeed = 500;
int maxSpeed = 5000;
int minAngle = 10; // минимальный угол для серв
int maxAngle = 170; // максимальный угол для серв
int startLeftPos = 10; // стартовая позиция в градусах для левой сервы (должна быть в интервале (minAngle, maxAngle))
int startRightPos = 10; // стартовая позиция в градусах для правой сервы (должна быть в интервале (minAngle, maxAngle))
int currentLeftPos;
int currentRightPos;
int nextLeftPos;
int nextRightPos;
int leftDelay;
int rightDelay;
int signLeftPosDelta;
int signRightPosDelta;
unsigned long currentLeftTime;
unsigned long currentRightTime;

void setup() {
	servoLeft.attach(10); // левая рука на 10 пине
	servoRight.attach(11); // правая рука на 11 пине
	randomSeed(analogRead(0)); // активируем генератор рандомных значений
	servoLeft.write(startLeftPos); // ставим левый серв в стартовую позицию
	servoRight.write(startRightPos); // ставим правый серв в стартовую позицию
	currentLeftPos = startLeftPos;
	currentRightPos = startRightPos;
	initLeft();
	initRight();
}

void loop() {
	unsigned long time = millis();
	if (time - currentLeftTime > leftDelay) {
		currentLeftPos = max(minAngle, min(maxAngle, currentLeftPos + signLeftPosDelta));
		servoLeft.write(currentLeftPos);
		currentLeftTime = time;
		if (currentLeftPos == nextLeftPos) initLeft();
	}
	if (time - currentRightTime > rightDelay) {
		currentRightPos = max(minAngle, min(maxAngle, currentRightPos + signRightPosDelta));
		servoRight.write(currentRightPos);
		currentRightTime = time;
		if (currentRightPos == nextRightPos) initRight();
	}
}

int generateNextPos(int currentPos) {
	int newPos = random(minAngle, maxAngle + 1);
	while (newPos == currentPos) newPos = random(minAngle, maxAngle + 1);
	return newPos;
}

void initLeft() {
	nextLeftPos = generateNextPos(currentLeftPos);
	int nextLeftSpeed = random(minSpeed, maxSpeed + 1);
	int leftPosDelta = currentLeftPos - nextLeftPos;
	leftDelay = nextLeftSpeed / abs(leftPosDelta);
	signLeftPosDelta = sign(leftPosDelta);
	currentLeftTime = millis();
}

void initRight() {
	nextRightPos = generateNextPos(currentRightPos);
	int nextRightSpeed = random(minSpeed, maxSpeed + 1);
	int rightPosDelta = currentRightPos - nextRightPos;
	rightDelay = nextRightSpeed / abs(rightPosDelta);
	signRightPosDelta = sign(rightPosDelta);
	currentRightTime = millis();
}

int sign(int x) {
	return x < 0 ? -1 : (x > 0 ? 1 : 0);
}