NST, то ми залишаємося в поточному стані і розраховуємо наступні параметри:
інтервал часу між імпульсами, тобто час, необхідний для того щоб відбулося переміщення, рівне одному кроку двигуна.
, (1.1)
де - поточна швидкість;
-прискорення;
Прискорення змінюється по трикутному закону. Ми задаємо кількість кроків необхідних для досягнення заданої максимальної швидкості n. Тоді прискорення зростає, поки не наступить крок номер n/2, потім відбувається зниження прискорення до нуля.
, якщо i? n/2
, якщо i gt; n/2
- довжина кроку.
значення поточної швидкості, після закінчення часу dt
(1.2)
- гальмівний шлях-шлях необхідний для зниження поточної швидкості до нуля. St=
час закінчення dt
TendStep=Ttek + dt
Потім встановлюються сигнали на кроковий двигун BSM_x [4] і завдання чекає поки пройде час dt, розраховане раннє.
). CONST. Стан руху на постійній швидкості. У цьому стані ми обчислюємо dx, і перевіряємо чи треба переходити в стан зниження швидкості. Якщо цього не потрібно, то ми розраховуємо параметри: dt, Vt, St і т.д. Формуємо сигнали на кроковий двигун і очікуємо закінчення розрахованого часу dt.
). DECCEL. Стан гальмування. У цьому стані розраховуємо dx і перевіряємо умови переходів в стану ACCEL, CONST. Якщо напрямки швидкості і переміщення збігаються, переміщення по модулю більше гальмівного шляху і поточна швидкість менше максимальної, то переходимо в стан нарощування швидкості, якщо ж швидкість дорівнює максимальної, то переходимо в стан руху на постійній швидкості.
Якщо не виконуються попередні умови, то якщо поточна швидкість дорівнює нулю, переходимо в стан блокування гальма BRAKE.
В іншому випадку розраховуємо параметри руху, формуємо сигнали на порти керування кроковим двигуном і очікуємо закінчення часу dt.
У результаті опису структури отримаємо кінцевий автомат станів завдання Sled_x, представлений на малюнку 2.2.
Малюнок 2.2 - Структура станів завдання Sled_X
Таблиця 2.1. Стани завдання Sled_X
№ІмяВходДействіеВиход0InitState - sost=STOP_SX 1STOP_SX-Xt, Yt=0sost=BRAKE_SX2BRAKE_SX-TendT=Tt + Ttormozsost=STOP_SX sost=ACCEl_SX 3ACCEL_SXVi, A, Xt, XcVi + 1=Vi + ai * dt sost=CONST_SX sost=DEACCEL_SX sost=ACCEL_SX 4CONST_SXXt, Xc Ai Ssh dx=Xc-Xt St=Ssh * i Vt=Vi + ai * dtsost=DEACCEL_SX5DEACCEL_SXXc Xtdx=Xc-Xt Vt=Vi-ai ** dt sost=BRAKE_SX sost =DEACCEL_SX6STEP_SXTt Vi Ai SshTendStep=Tt + dt sost=CONST_SX sost=DEACCEL_SX sost=ACCEL_SX
Де Vt - поточна швидкість переміщення маніпулятора по координаті Y;
Dx - відстань до цільової координати Xс;
St - гальмівний шлях, необхідний для зниження поточної швидкості до нуля;
Vm - максимальна швидкість;
sost - мінлива вказує попередній стан завдання.
Завдання GotoX заснована на тому ж алгоритмі, що і Sled_X, і має подібну структуру станів. Відрізняються лише умови переходів з одного стану в інший. Справа в тому, що на етапі калібрування не відомо, чи де конкретно знаходиться вихідна ланка і скільки потрібно пройти до початку координат. А значить розрахувати часові інтервали ми можемо тільки для стану прискорення, і при подальшому русі орієнтуватися за сигналами, отриманим з датчика зниження швидкості і датчика початкового положення.
Таким чином, завдання приймає наступну структуру:
). STOP. Стан зупину. У цей стан завдання переходить при ініціалізації. Якщо була дана команда виходу в нуль, то завдання переходить у стан звільнення гальма BRAKE.
). BRAKE. У цьому стані завдання вичікує час дії гальма, яке задано за умовою. Якщо попередній стан було STOP (тобто ми починаємо рух), то перевіряються сигнали з датчиків, для того щоб б визначити наступний стан завдання.
Якщо спрацював датчик зниження швидкості (ДСС) і ще не спрацював датчик початкового положення (ДНП), то завдання переходить у стан руху на малій швидкості, яка повинна бути задана користувачем.
Якщо жоден з датчиків не спрацював, то завдання переходить у стан розгону ACCEL.
Якщо обидва датчика спрацювали, то завдання переходить у стан дожима DOGE.
Якщо ж перехід в поточний стан стався для блокування гальма, то переходимо в стан зупинки.
). ACCEL. Стан розгону. У цьому стані відбувається нарощування швидкості, поки не спрацює ДСС або не буде досягнута максимальна швидкість. У першому випадку відбудеться перехід в стан гальмування DEACCEL, у другому - в стан руху на постійній швидкості CONST.
