Регулирование скорости перемещения, класс управления сервоприводом
Серия-статей: Arduino, использование сервоприводов #5
Для этого добавим соответствующие перменные и функции, которые их задают.
Кроме уже сделанной нами функции 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 секунд). Пусть такое перемещение и не совсем плавное, но оно все равно позволяет более гибко управлять сервоприводом, причем достаточно всего одной команды