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

Спробуємо додати можливість більш плавно змінювати положення сервоприводу, так само, як і в прикладі для Незалежне управління кількома сервоприводами

Для цього додамо відповідні пермінні та функції, які їх задають.

Крім вже зробленої нами функції 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; // Даємо комнду почати плавне переміщення
}

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 секунд). Нехай таке переміщення і не зовсім плавне, але воно все одно дозволяє гнучкіше управляти сервоприводом, причому достатньо лише однієї команди