Материал: Разработка системы управления механической системы в режиме мягкого реального времени

В нашем процессе мы выделили шесть задач, которые имеют структуру состояний. Это - задачи управления скоростью. В общем случае, в движении выходного звена можно выделить три состояния: ускорение, равномерное движение, торможение. Рассмотрим каждую задачу.

Как уже говорилось, задача Sled_X ответственна за расчет временных характеристик и координат движения ШД1. Так как Привод с ШД не имеет обратной связи, вся траектория должна рассчитываться до начала движения. Расчет характеристик движения осуществляется при помощи мгновенного значения времени и независящего от него параметра. Так как по заданию ускорение изменяется по треугольному закону, то этим параметром можно считать модуль производной от ускорения k.

В задаче Sled_X можно выделить следующие состояния:

). STOP. Состояние ожидания действий. В это состояние задача переходит при инициализации. В нем проверяется выполнение выхода в ноль по обеим координатам. Если выход в ноль сделан, то проверяется условие движения, а именно разница между текущей и целевой координатами по оси X. Если условие выполняется, то задача переходит в состояние освобождения тормоза BRAKE.

). BRAKE. В этом состоянии задача выжидает время действия тормоза, которое задано по условию, и затем переходит в состояние ускорения либо останова.

). ACCEL. Состояние ускорения. В этом состоянии происходит разгон, то есть увеличение текущей скорости до максимальной. Так как система следящая, и пользователь может в любой момент времени изменить координаты целевой точки, то мы должны учитывать эти изменения введенные пользователем. В данном состоянии мы рассчитываем разницу между текущей и целевой координатами по оси X (dx).

Далее проверяются условия перехода в другие состояния.

Если перемещение dx не совпадает по направлению с текущей скоростью или dx по модулю меньше или равно пути, требуемого для снижения текущей скорости до нуля, то происходит переход в состоянии DEACCEL. Если же уже набрана максимальная скорость и не выполняется условия перехода в состояние DEACCEL, то происходит переход в состояние движения на постоянной скорости CONST. Если же не происходит переходов в состояния DEACCEL и CONST, то мы остаемся в текущем состоянии и рассчитываем следующие параметры:

интервал времени между импульсами, то есть время необходимое для того чтобы произошло перемещение, равное одному шагу двигателя.

, (1.1)

где  - текущая скорость;

-ускорение;

Ускорение изменяется по треугольному закону. Мы задаем количество шагов необходимых для достижения заданной максимальной скорости n. Тогда ускорение растет, пока не наступит шаг номер n/2, затем происходит снижение ускорения до нуля.

, если i≤n/2

, если i>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

Где Vt - текущая скорость перемещения манипулятора по координате Y;

Dx - расстояние до целевой координаты Xс;

St - тормозной путь, требуемый для снижения текущей скорости до нуля;

Vm - максимальная скорость;

sost - переменная указывающая предыдущее состояние задачи.

Задача GotoX основана на том же алгоритме, что и Sled_X, и имеет подобную структуру состояний. Отличаются лишь условия переходов из одного состояния в другое. Дело в том, что на этапе калибровки не известно, где конкретно находится выходное звено и сколько нужно пройти до начала координат. А значит рассчитать временные интервалы мы можем только для состояния ускорения, и при дальнейшем движении ориентироваться по сигналам, полученным с датчика снижения скорости и датчика начального положения.

Таким образом, задача принимает следующую структуру:

). STOP. Состояние останова. В это состояние задача переходит при инициализации. Если была дана команда выхода в ноль, то задача переходит в состояние освобождения тормоза BRAKE.

). BRAKE. В этом состоянии задача выжидает время действия тормоза, которое задано по условию. Если предыдущее состояние было STOP(то есть мы начинаем движение), то проверяются сигналы с датчиков, для того чтобы бы определить следующее состояние задачи.

Если сработал датчик снижения скорости (ДСС) и еще не сработал датчик начального положения(ДНП), то задача переходит в состояние движения на малой скорости, которая должна быть задана пользователем.

Если ни один из датчиков не сработал, то задача переходит в состояние разгона ACCEL.

Если оба датчика сработали, то задача переходит в состояние дожима DOGE.

Если же переход в текущее состояние произошел для блокировки тормоза, то переходим в состояние останова.

). ACCEL. Состояние разгона. В этом состоянии происходит наращивание скорости, пока не сработает ДСС или не будет достигнута максимальная скорость. В первом случае произойдет переход в состояние торможения DEACCEL, во втором - в состояние движения на постоянной скорости CONST.

). SMALL_V. В этом состоянии происходит движение на заданной малой скорости, пока не сработает датчик начального положения. Как только сработает датчик, задача перейдет в состояние дожима DOGE.

). DOGE. Дожим. Здесь мы делаем m импульсов на 1-ой скорости и переходим в состояние BACK.

). CONST. В данном состоянии происходит движение на постоянной скорости пока не сработает ДСС, а затем происходит переход в состояние торможения DEACCEL.

). DEACCEL. Состояние торможения. Здесь происходит снижение скорости до заданного минимального значения , а затем переход в состояние SMALL_V.

). BACK. Выход в нулевую позицию. В данном состоянии мы делаем n импульсов назад, для выхода в заданную позицию, устанавливаем значение текущей координаты равной нулю и переходим в состояние блокировки тормоза BRAKE.

В результате получим структуру состояний задачи Goto0_X, представленную на рисунке 2.3.

Рисунок 2.3 - Структура состояний задачи GotoX

Таблица 2.2 Состояния задачи GotoX

Где 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<=T2

a=-k*t+Tr*k, если t>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).

При достижении очередного целого значения x задача производит шаг выходного звена в нужном направлении. По истечении времени Tconst с момента вхождения в состояние задача переходит в состояние DECCEL.

При малых перемещениях выходного звена данное состояние должно было бы игнорироваться. Однако для упрощения условий перехода между состояниями задача обязательно пройдет через данное состояние, возможно и не сделав ни одного шага в нём.

). DECCEL. Состояние торможения. В этом состоянии ускорение в течение первого периода T2 уменьшается, в течение второго - растет до нуля. Расчет ускорения, скорости и координаты ведется по следующим итерационным формулам:

a=-k*t, если t<=T2=k*t-Tr*k, если t>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