Регулирование скорости перемещения, класс управления сервоприводом

Попробуем добавить возможность более плавно изменять положение сервопривода, так же, как и в примере для независимого управления несколькими сервоприводами

Для этого добавим соответствующие перменные и функции, которые их задают.

Кроме уже сделанной нами функции void MoveTo(int movepos) добавим функцию void MoveTo(int movepos, float movespeed). У них одинаковые названия, но разное количество аргументов. Такой вариант используется вместо указания значений недостающих аргументов по умолчанию тогда, когда от количества аргументов сильно зависит функционал. В данном случае при вызове MoveTo(50) будет производиться максимально быстрое перемещение сервопривода в указанную позицию. А вот вызов MoveTo(50, 1.0) будет сответствовать плавному перемещению со скоростью 1 оборот в минуту (6 градусов в секнду). Поскольку сервопривод не дает возможность изменять скорость перемещения, то необходимый поворот будет разбит на несколько микроперемещений по 1 градусу, интервал между которыми будет таким, чтобы обеспечить заданную скорость. Для определения этого интервала произведем несложные вычисления

float RotationSpeed=moveSpeed*360/60;  // Перводим скорость из оборотов в минуту в градусы за секунду
moveInterval=(int)1000/RotationSpeed;  // Вычисляем интервал между перемещениями на один градус

Не забываем предварительно определить, в какую сторону нужно двигаться, т.е. добавлять или отнимать градус за каждое микроперемещение, для этого достаточно знать текущее и требуемое положение

if (movePosition >= servPosition)  // Определяем, в какую сторону от текущего положения нам нужно двигаться
	moveIncrement=1;    // Если вперед - будем добавлять один градус за один интервал времени
else
	moveIncrement=-1; //  Если назад - то отнимать

Не забываем проверять, не выходит ли требуемое перемещение за рамки установленных крайних значений. Поскольку выполнять одинаковую проверку нужно для обоих реализаций функции MoveTo(), то есть смысл выделить для нее отдельную функцию

int PositionCheck(int movepos)
{
	int CheckedPos;
	if (movepos<=posMin)	// Если указано большее значение, чем максимальный
					// предел перемещения
	CheckedPos =posMin;	// Будем двигаться тоько до этого предела
	else
	if (movepos>=posMax)	// Если указано меньшее значение, чем минимальный 
					//предел перемещения
	CheckedPos =posMin;	// Будем двигаться тоько до этого предела
	else			// В любых других случаях - мы в допустимых для перемещения пределах
	CheckedPos =movepos;	// Просто устанавливаем позицию, в которую должен
	return CheckedPos;
}

Подключение стандартное, ко второму и третьему цифровому выходу:

	//Управление сервоприводами с помощью пользовательского класа, задание
	// мгновенного и плавного перемещения с помощью вызова одной функции
#include 		// Подключаем библиотеку для работы с сервоприводом
class ServoClass
{
	Servo serv;	// Собственно сам сервопривод, управляемый стандартной
			// библиотекой servo
	int servPosition;	// Текущая позиция сервопривода, градусов
	int posMax;	// Максимально допустимый предел перемещения сервопривода, градусов
	int posMin;	// Минимальный допустимый предел перемещения сервопривода, градусов
	int posInit;	// Исходная позиция сервопривода, градусов
	// Переменные, которые отвечают за перемещение
	boolean move;	// Должен ли двигатся ли сервопривод в настоящий момент 
	int movePosition;	// Позиция, которую должен принять сервопривод, градусы
	float moveSpeed;	// Скорость, с которой сервопривод должен двигаться, об/мин
	int moveIncrement;	// изменение положения за один интервал времени, градусов
	int moveInterval;	// Интервал между движениями для обеспечения  скорости, миллисекунды
	unsigned long lastTimeCheck;	// Время последнего движения сервопривода, в мс от начала работы программы
public:
ServoClass ()
{
	posMin=0;
	posMax=180;
	posInit=0;
	servPosition=0;
	lastTimeCheck=millis();	
}
void AttachServo (int pin, int minpos=0, int maxpos=180, int initpos=0)
{
	serv.attach(pin);	// Подключаем сервопривод к указанному пину
	posMin= minpos;		// Устанавливаем минимальную позицию 
	posMax= maxpos; 	  // Устанавливаем максимальную позицию 
	posInit= initpos;		// Устанавливаем исходную позицию
	servPosition= posInit;	// Позиция сервопривода должна сответствовать исходному положению	
	serv.write(servPosition);	// Даем команду изменить положение
}
int PositionCheck(int movepos)
{
	int CheckedPos;
	if (movepos<=posMin)	// Если указано большее значение, чем максимальный
					// предел перемещения
	CheckedPos =posMin;	// Будем двигаться тоько до этого предела
	else
	if (movepos>=posMax)	// Если указано меньшее значение, чем минимальный 
					//предел перемещения
	CheckedPos =posMin;	// Будем двигаться тоько до этого предела
	else			// В любых других случаях - мы в допустимых для перемещения пределах
	CheckedPos =movepos;	// Просто устанавливаем позицию, в которую должен
	return CheckedPos;
}
void MoveTo (int movepos)  // Если скорость не указана - просто даем команду на перемещение сервопривода
{
		
		movePosition = PositionCheck(movepos); // Проверяем, не выходим ли мы за допустимые пределы премещения
		serv.write(servPosition);	// Даем команду изменить положение
	}
	void MoveTo (int movepos, float movespeed)  // Если скорость  указана - организуем плавное перемещение
	{
		movePosition = PositionCheck(movepos); // Проверяем, не выходим ли мы за допустимые пределы премещения
		moveSpeed=movespeed;	// Устанавливаем скорость, с которой должен перемещаться сервопривод
		int Angle=abs(movePosition-servPosition);	  // На сколько должен переместиться сервопривод 
					// вне зависимости от направления
	if (movePosition >= servPosition)	// Определяем, в какую сторону от текущего положения нам нужно двигаться
		moveIncrement=1;	// Если вперед - будем добавлять один градус за один интервал времени
	else
		moveIncrement=-1; //  Если назад - то отнимать
	float RotationSpeed=moveSpeed*360/60;  // Перводим скорость из оборотов в минуту в градусы за секунду
	moveInterval=(int)1000/RotationSpeed;  // Вычисляем интервал между перемещениями на один градус
	move=true;  // Даем комнду начать плавное перемещение
	} 
void Update ()
{
	if (move)  //Если дана команда на перемещение
	{
		if ((millis() - lastTimeCheck)>= moveInterval)	// проверяем, сколько прошло времени с последнего 
									//действия и сравниваем с нужным нам интервалом. 
									//Если времени прошло больше – совершаем следующее действие
		{
		servPosition+=moveIncrement;			// Изменяем угол на вычисленное ранее значение
		serv.write(servPosition);		// Посылаем сервоприводу команду с новым углом поворота
		lastTimeCheck= millis();	// Обновляем счетчик времени, для того чтобы следующий отсчет времени начался с этого момента
		if (servPosition==movePosition)	// Если мы достигли нужной позиции-
			move=false;			// остановить движение
	}
	}
}
};
ServoClass serv1;
ServoClass serv2;
void setup() 
{
	serv1.AttachServo(2);			// Первый сервопривод подключен ко второму пину 
						// Стандартные пределы перемещения от 0 до 180 градусов
						// Стандартное исходное положени 0 градусов
	serv2.AttachServo(3, 20, 160, 90);	// Второй сервопривод подключен к третьему пину
						// Пределы перемещения от 20 до 160 градусов
						// Исходное положение - 90 градусов
	serv1.MoveTo(20);			// Пермещаем первый сервопривод в положение 20 градусов
	serv2.MoveTo(150, 0.5);			// Пермещаем второй сервопривод в положение 150 градусов со скоростью 0.5 оборота в минуту
	
}
void loop() 
{
	serv1.Update();  // Обрабатываем перемещения первого сервопривода
	serv2.Update();  // Обрабатываем перемещения второго сервопривода
}

Теперь при запуске программы первый сервопривод, как и в предыдущем примере займет указанное положение. А вот второй будет поворачиваться достаточно медленно (его исходно положение 90 градусов, для перемещения в положение 150 градусов нужно повернуться на 60 градусво. Скорость 0.5 оборота в минуту соответсвует 3 градуса в секунду, т.е перемещение займет 20 секунд). Пусть такое перемещение и не совсем плавное, но оно все равно позволяет более гибко управлять сервоприводом, причем достаточно всего одной команды