Програмне обмеження переміщення сервоприводу

Не завжди сервопривід може повернутися на заданий кут. Наприклад, закріплений на ньому маніпулятор може упиратися в корпус або мати шарнір з можливим маленьким кутом повороту. У цьому випадку, якщо ми подамо команду повороту на занадто великий кут, то сервопривід уткнеться в перешкоду, але намагатиметься рухатися далі. У гіршому випадку ми отримаємо крайке дзижчання, у гіршому - сервопривід може перегрітися і навіть вийти з ладу. У будь-якому випадку краще заздалегідь програмно подбати про обмеження переміщення.

Для цього додамо до нашого класу пару змінних posMax та posMin. Заодно подбаємо про вихідне положення сервоприводу, в якому він повинен перебувати під час запуску програми, визначимо його за допомогою змінної posInit. Встановлювати значення цих змінних одночасно з ініціалізацією сервоприводу, для цього додамо в функцію AttachServo ще три аргументи:

void AttachServo (int pin, int minpos=0, int maxpos=180, int initpos=0)

Приклад ініціалізації сервоприводів із межами переміщення:

void setup()
{
   serv1.AttachServo(2); // До другого піну підключений перший сервопривід
                           // Стандартні межі переміщення від 0 до 180 градусів
                           // Стандартне вихідне положення 0 градусів
   serv2.AttachServo(3, 20, 160, 90); // До третього піну підключений другий сервопривід
                                       // Межі переміщення від 20 до 160 градусів
                                       // Початкове становище - 90 градусів
}

Тепер створимо функцію MoveTo, за допомогою якої керуватимемо рухом сервоприводу. У найпростішому варіанті нам потрібно буде просто перемістити сервопривід у потрібне положення. Для цього просто викличемо цю функцію з одним аргументом - потрібним нам положенням, наприклад, serv1.MoveTo(20); дасть команду першому сервоприводу повернутися в положення 20 градусів. Оскільки ми раніше встановили максимальні та мінімальні межі переміщення, то спроба викликати функцію з неправильним кутом - serv2.MoveTo(220); Приведе до того, що сервопривід повернеться до встановленого максимального значення 160 градусів і зупиниться.

Ось що в нас вийшло:

//Управління сервоприводами за допомогою класу користувача, завдання
// Переміщення за допомогою виклику однієї функції з програмним обмеженням переміщень
#include  // Підключаємо бібліотеку для роботи із сервоприводом
class ServoClass
{
Servo serv; // Власне сам сервопривід, керований стандартною бібліотекою servo
int servPosition;// Поточна позиція сервоприводу, градусів
int posMax; // Максимально допустима межа переміщення сервоприводу, градусів
int posMin; // Мінімальна допустима межа переміщення сервоприводу, градусів
int posInit; // Вихідна позиція сервоприводу, градусів

public:
ServoClass ()
{
posMin = 0; // Встановлюємо значення за умовчанням
posMax = 180;
posInit = 0;
servPosition=0;
}
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); // Даємо команду змінити положення
}
void MoveTo (int movepos) // Даємо команду на переміщення сервоприводу
{
// Перевіряємо, чи ми виходимо за допустимі межі преміщення
if (movepos<=posMin) // Якщо вказано більше значення, ніж максимальний
// межа переміщення
movePosition=posMin; // Рухатимемося тільки до цієї межі
else
if (movepos>=posMax) // Якщо вказано менше значення, ніж мінімальний
//Межа переміщення
movePosition=posMin; // Рухатимемося тільки до цієї межі
else // У будь-яких інших випадках - ми в допустимих для переміщення межах
movePosition=movepos; // Просто встановлюємо позицію, в яку має
// Перемістити сервопривід

serv.write(servPosition); // Даємо команду змінити положення
}
};

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(220); // Переміщаємо другий сервопривід у положення 220 градусів
// Через обмеження на максимальне переміщення сервопривід
// Повернеться до 160 градусів і зупиниться
}

void loop()
{

}

Тепер ми можемо не боятися, що дамо команду на неправильне переміщення та сервопривід уткнеться у перешкоду. Достатньо один раз обмежити переміщення при ініціалізації.