При досягненні чергового цілого значення x завдання виробляє крок вихідної ланки в потрібному напрямку. Після закінчення часу Tconst з моменту входження в стан завдання переходить у стан DECCEL.
При малих переміщеннях вихідної ланки даний стан мало б ігноруватися. Однак для спрощення умов переходу між станами завдання обов'язково пройде через даний стан, можливо і не зробивши жодного кроку в ньому.
). DECCEL. Стан гальмування. У цьому стані прискорення протягом першого періоду T2 зменшується, протягом другого - зростає до нуля. Розрахунок прискорення, швидкості і координати ведеться за наступними ітераційним формулами:
a=-k * t, якщо t lt;=T2=k * t-Tr * k, якщо t gt; T2=v0 + (a + a0)/2 * (t-t0)=y0 + (v0 + a * (t-t0)/2) * (t-t0)
де t - час з моменту переходу в стан.
При досягненні чергового цілого значення y завдання виробляє крок вихідної ланки в потрібному напрямку. Після закінчення часу Tr з моменту переходу в стан завдання переходить у стан блокування гальма BRAKE.
). WAIT. Стан зупину. У цьому стані завдання витримує заданий час зупинки Tstop і переходить в стан READY.
У результаті опису структури отримаємо кінцевий автомат станів завдання Sled_Y, представлений на малюнку 2.4.
Малюнок 2.4 - Структура станів завдання Sled_Y
Таблиця 2.3. Стани завдання Sled_Y
№ИмяВходДействиеВыход0InitState--sost=STOP_SY1STOP_SY-Yt=0sost=BRAKE_SY2BRAKE_SY-TendT=Tt+ Ttormozsost=STOP_SY sost=ACCEl_SY 3ACCEL_SYVi, Ai, Yt, YcVi + 1=Vi + ai * dt sost=CONST_SY sost=DEACCEL_SY sost=ACCEL_SY 4CONST_SYYt, Yc Ai Ssh dy=Yc-Yt St=Ssh * i Vt=Vi + ai * dtsost=DEACCEL_SY5DEACCEL_SYYc Ytdy=Yc-Yt Vt=Vi-ai ** dt sost=BRAKE_SY sost=DEACCEL_SY6STEP_SYTt Vi Ai SshTendStep=Tt + dt sost=CONST_SY sost=DEACCEL_SY sost=ACCEL_SY
Де Vt - поточна швидкість переміщення маніпулятора по координаті Y;
Dx - відстань до цільової координати Yс;
St - гальмівний шлях, необхідний для зниження поточної швидкості до нуля;
Vm - максимальна швидкість;
sost - мінлива вказує попередній стан завдання.
Завдання GotoY заснована на тому ж алгоритмі, що і Sled_Y, і має подібну структуру станів. Відрізняються лише умови переходів з одного стану в інший. Справа в тому, що на етапі калібрування не відомо, чи де конкретно знаходиться вихідна ланка і скільки потрібно пройти до початку координат. А значить розрахувати часові інтервали ми можемо тільки для стану прискорення, і при подальшому русі орієнтуватися за сигналами, отриманим з датчика зниження швидкості і датчика початкового положення.
Таким чином, завдання приймає наступну структуру:
). Stop. Стан зупину. У цей стан завдання переходить при ініціалізації. Якщо була дана команда виходу в нуль, то завдання переходить у стан звільнення гальма BRAKE.
). BRAKE. У цьому стані завдання вичікує час дії гальма, яке задано за умовою. Якщо попередній стан було STOP (тобто ми починаємо рух), то перевіряються сигнали з датчиків, для того щоб б визначити наступний стан завдання.
Якщо спрацював датчик зниження швидкості (ДСС) і ще не спрацював датчик початкового положення (ДНП), то завдання переходить у стан руху на малій швидкості, яка повинна бути задана користувачем.
Якщо жоден з датчиків не спрацював, то завдання переходить у стан розгону ACCEL.
Якщо обидва датчика спрацювали, то завдання переходить у стан дожима DOGE.
Якщо ж перехід в поточний стан стався для блокування гальма, то переходимо в стан зупинки.
). ACCEL. Стан розгону. У цьому стані відбувається нарощування швидкості, поки не спрацює ДСС або не буде досягнута максимальна швидкість. У першому випадку відбудеться перехід в стан гальмування DEACCEL, у другому - в стан руху на постійній швидкості CONST.
). SMALL_V. У цьому стані відбувається рух на заданій малій швидкості, поки не спрацює датчик початкового положення. Як тільки спрацює датчик, завдання перейде в стан дожима DOGE.
). DOGE. Дожим. Тут ми робимо m імпульсів на першому швидкості і переходимо в стан BACK.
). CONST. У даному стані відбувається рух на постійній швидкості поки не спрацює ДСС, а потім відбувається перехід в стан гальмування DEACCEL.
). DEACCEL. Стан гальмування. Тут відбувається зниження швидкості до заданого мінімального значення, а потім перехід в стан SMALL_V.
