ign="justify">). SMALL_V. У цьому стані відбувається рух на заданій малій швидкості, поки не спрацює датчик початкового положення. Як тільки спрацює датчик, завдання перейде в стан дожима DOGE.
). DOGE. Дожим. Тут ми робимо m імпульсів на першому швидкості і переходимо в стан BACK.
). CONST. У даному стані відбувається рух на постійній швидкості поки не спрацює ДСС, а потім відбувається перехід в стан гальмування DEACCEL.
). DEACCEL. Стан гальмування. Тут відбувається зниження швидкості до заданого мінімального значення, а потім перехід в стан SMALL_V.
). BACK. Вихід в нульову позицію. У даному стані ми робимо n імпульсів назад, для виходу в задану позицію, встановлюємо значення поточної координати рівною нулю і переходимо в стан блокування гальма BRAKE.
У результаті одержимо структуру станів завдання Goto0_X, представлену на малюнку 2.3.
Малюнок 2.3 - Структура станів завдання GotoX
Таблиця 2.2 Стани завдання GotoX
№ИмяВходДействиеВыход0InitState--Ostanovka=1=gt;sost=STOP1STOPOstanovka=1-sost=BRAKE2BRAKEsost=1 do T_x=1 else T_x=0 Tt T=Tt + T_xDSS_X=0 or (DSS_X=1 amp; (DSP_X=1orDSP_X=1) 3ACCELVt, Tt, Vm, A, Xc, Xt Vt, dt, Ttek4SMALL_VXt, Xc Ai Ssh, Tt, Vtdx=Xc-Xt St=Ssh * i Vt=Vi + ai * dtDSP=1= gt; sost=DOGE5DOGEDSP=1Stcount=msost=BACK6CONSTXt, Xc Ai Sshdx=Xc-Xt St=Ssh * i Vt=Vi + ai * dtDSS=1= gt; sost=DEACCEL7DEACCELXc Xtdx=Xc-Xt Vt=Vi-ai ** dt Vt=Vmin Sost=SMALL_V 8BACKXt sost=DOGEStcount=nXt=0 sost=BRAKE
Де Vt - поточна швидкість переміщення маніпулятора по координаті Y;
Dx - відстань до цільової координати Xс;
St - гальмівний шлях, необхідний для зниження поточної швидкості до нуля;
Vm - максимальна швидкість;
sost - мінлива вказує попередній стан завдання.
Як вже говорилося, завдання Sled_Y відповідальна за розрахунок тимчасових характеристик і координат руху ШД2. Так як Привід з ШД не має зворотного зв'язку, вся траєкторія повинна розраховуватися до початку руху. Розрахунок характеристик руху здійснюється за допомогою миттєвого значення часу і незалежного від нього параметра. Бо за завданням прискорення змінюється по трикутному законом, то цим параметром можна вважати модуль похідною від прискорення k.
У задачі Sled_Y можна виділити такі стани:
). READY. Стан очікування дій. У цей стан завдання переходить при ініціалізації. У ньому перевіряється умова руху, а саме різниця між поточною і цільовою координатами по осі Y. Якщо умова виконується, розраховуються тимчасові інтервали для кожного стану руху:
T2=Tr/2=T2 * Vm=(S - 2 * Sr)/Vm=Vm/T2/T2,
де Tr - час розгону/гальмування (встановлюється відповідно з обладнанням);
T2 - період часу, після закінчення якого k змінить знак в прискореному режимі;
Sr - шлях, який пройде вихідна ланка на етапі прискорення/гальмування;
S - спільний шлях;
Vm - задана максимальна швидкість на шляху;
k - похідна прискорення.
Розрахувавши дані характеристики, завдання переходить у стан звільнення гальма BRAKE.
). BRAKE. У цьому стані завдання вичікує час дії гальма, яке задано за умовою, і потім переходить в стан прискорення або зупину.
). ACCEL. Стан прискорення. У цьому стані прискорення протягом першого періоду T2 зростає, протягом другого - зменшується до нуля. Розрахунок прискорення, швидкості і координати ведеться за ітераційним формулами:
a=k * t, якщо t lt;=T2
a=-k * t + Tr * k, якщо t gt; T2=v0 + (a + a0)/2 * (t-t0)=y0 + (v0 + a * (t-t0)/2) * (t-t0),
де a - миттєве прискорення;
t - час від початку стану;
t0 - момент часу минулого розрахунку;
v - миттєва швидкість;
v0 - початкова швидкість;
y - розрахункове значення поточної координати;
y0 - початкова координата.
При досягненні чергового цілого значення y завдання виробляє крок вихідної ланки в потрібному напрямку. Після закінчення часу Tr завдання переходить у стан CONST.
). CONST. У цьому стані параметр k і прискорення дорівнюють нулю, а швидкість постійна, що відповідає рівномірному руху. Тому в цьому стані розраховується лише поточна координата:
y=y0 + v0 * (t-t0)....
